nonlinearanalyticconditionalgaussian_ginac.cpp
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
3 // Wim Meeussen <wim dot meeussen at mech dot kuleuven dot ac dot be>
4 //
5 // This program is free software; you can redistribute it and/or modify
6 // it under the terms of the GNU Lesser General Public License as published by
7 // the Free Software Foundation; either version 2.1 of the License, or
8 // (at your option) any later version.
9 //
10 // This program is distributed in the hope that it will be useful,
11 // but WITHOUT ANY WARRANTY; without even the implied warranty of
12 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 // GNU Lesser General Public License for more details.
14 //
15 // You should have received a copy of the GNU Lesser General Public License
16 // along with this program; if not, write to the Free Software
17 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
18 //
19 
21 #include <cmath>
22 #include <cassert>
23 #include "../wrappers/rng/rng.h" // Wrapper around several rng
24  // libraries
25 
26 namespace BFL
27 {
28  // constructor
30  (const GiNaC::matrix& func,
31  const vector<GiNaC::symbol>& u,
32  const vector<GiNaC::symbol>& x,
33  const Gaussian& additiveNoise,
34  const vector<GiNaC::symbol>& cond )
36  func_sym (func),
37  cond_sym (cond),
38  u_sym (u),
39  x_sym (x),
40  cond_size (cond_sym.size()),
41  u_size (u_sym.size()),
42  x_size (x_sym.size()),
43  func_size (func_sym.rows()),
44  dfunc_dcond (cond_size),
45  dfunc_dx (x_size)
46  {
47  // test for consistent input
48  assert (func_sym.cols() == 1);
49  assert (additiveNoise.DimensionGet() == cond_size);
50 
51  // derive func to cond
52  for (unsigned int i=0; i < cond_size; i++)
53  dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
54 
55  // derive func to x
56  for (unsigned int i=0; i < x_size; i++)
57  dfunc_dx[i] = func_sym.diff(x_sym[i]);
58  }
59 
60  // constructor
62  (const GiNaC::matrix& func,
63  const vector<GiNaC::symbol>& u,
64  const vector<GiNaC::symbol>& x,
65  const Gaussian& additiveNoise)
67  func_sym (func),
68  u_sym (u),
69  x_sym (x),
70  cond_size (0),
71  u_size (u_sym.size()),
72  x_size (x_sym.size()),
73  func_size (func_sym.rows()),
74  dfunc_dx (x_size)
75  {
76  // test for consistent input
77  assert (func_sym.cols() == 1);
78 
79  // derive func to x
80  for (unsigned int i=0; i < x_size; i++)
81  dfunc_dx[i] = func_sym.diff(x_sym[i]);
82  }
83  // copy constructor
87  func_sym (g.func_sym),
88  cond_sym (g.cond_sym),
89  u_sym (g.u_sym),
90  x_sym (g.x_sym),
91  cond_size (cond_sym.size()),
92  u_size (u_sym.size()),
93  x_size (x_sym.size()),
94  func_size (func_sym.rows()),
95  dfunc_dcond (cond_size),
96  dfunc_dx (x_size)
97  {
98  // test for consistent input
99  assert (func_sym.cols() == 1);
100  if (cond_size!=0) assert (g.AdditiveNoiseSigmaGet().rows() == cond_size);
101 
102  // derive func to cond
103  for (unsigned int i=0; i<cond_size; i++)
104  dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
105 
106  // derive func to x
107  for (unsigned int i=0; i < x_size; i++)
108  dfunc_dx[i] = func_sym.diff(x_sym[i]);
109  }
110 
111  // destructor
113  {}
114 
115 
116  std::ostream& operator<< (std::ostream& os, NonLinearAnalyticConditionalGaussian_Ginac& p)
117  {
118  os << "function: " << p.func_size << endl;
119  os << "input: " << p.u_size << endl;
120  os << "State: " << p.x_size << endl;
121  os << "Conditional: " << ((p.cond_size) !=0) << endl;
122  return os;
123  }
124 
125 
126  MatrixWrapper::ColumnVector
128  {
129  MatrixWrapper::ColumnVector u_num (u_size);
130  MatrixWrapper::ColumnVector x_num (x_size);
131  MatrixWrapper::ColumnVector func_num(func_size);
132  GiNaC::ex substitute (func_size);
133  MatrixWrapper::ColumnVector expected(func_size);
134 
135  u_num = ConditionalArgumentGet(1);
136  x_num = ConditionalArgumentGet(0);
137 
138  // use Mu of additive noise
139  if (cond_size!=0)
140  for (unsigned int i=0; i<u_size; i++)
141  for (unsigned int j=0; j<cond_size; j++)
142  if (u_sym[i] == cond_sym[j])
143  u_num(i+1) += (this->AdditiveNoiseMuGet())(j+1);
144 
145 
146  // evaluate func
147  for (unsigned int i=0; i<func_size; i++)
148  {
149  // temp variable to substitute in
150  substitute = func_sym(i,0);
151 
152  // substitute all u_sym with u_num
153  for (unsigned int j=0; j<u_size; j++)
154  substitute = substitute.subs( u_sym[j]==u_num(j+1) );
155 
156  // substitute all x_sym with x_num
157  for (unsigned int j=0; j<x_size; j++)
158  substitute = substitute.subs( x_sym[j]==x_num(j+1) );
159 
160  // build matrix func_num
161  func_num(i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute.evalf() ).to_double();
162  }
163  expected = func_num;
164 
165  if (cond_size==0)
166  expected += AdditiveNoiseMuGet();
167 
168  return expected;
169  }
170 
171 
172  MatrixWrapper::SymmetricMatrix
174  {
175  if (cond_size!=0)
176  {
177  MatrixWrapper::ColumnVector u_num (u_size);
178  MatrixWrapper::ColumnVector x_num (x_size);
179  GiNaC::ex substitute (func_size);
180  MatrixWrapper::Matrix D (func_size,cond_size);
181 
182 
183  u_num = ConditionalArgumentGet(1);
184  x_num = ConditionalArgumentGet(0);
185 
186  for (unsigned int i=0; i<cond_size; i++)
187  {
188  // temp variable to substitute in
189  substitute = dfunc_dcond[i];
190 
191  // substitute all u_sym with u_num
192  for (unsigned int j=0; j<u_size; j++)
193  substitute = substitute.subs( u_sym[j]==u_num(j+1) );
194 
195  // substitute all x_sym with x_num
196  for (unsigned int j=0; j<x_size; j++)
197  substitute = substitute.subs( x_sym[j]==x_num(j+1) );
198 
199  // convert substitute back to matrix
200  GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
201 
202  // build matrix D
203  for (unsigned int j=0; j<func_size; j++)
204  D(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
205  }
206  //cout << "D: " << D << endl;
207  //cout << "CondCov:\n" << (Matrix)cond_covariance << endl;
208  MatrixWrapper::Matrix temp = D * (MatrixWrapper::Matrix)AdditiveNoiseSigmaGet() * D.transpose();
209 
210  // convert func_covariance_matrix to symmetric matrix
211  MatrixWrapper::SymmetricMatrix additiveNoise(temp.rows());
212  temp.convertToSymmetricMatrix(additiveNoise);
213  return additiveNoise;
214 
215  }
216  else
217  {
218  return AdditiveNoiseSigmaGet();
219  }
220  }
221 
222 
223  MatrixWrapper::Matrix
225  {
226  // Check if i = 0, since this is the old df_dxGet method!
227  assert(i == 0);
228 
229  // evaluate function
230  MatrixWrapper::ColumnVector u_num (u_size);
231  MatrixWrapper::ColumnVector x_num (x_size);
232  GiNaC::ex substitute (func_size);
233  MatrixWrapper::Matrix F (func_size, x_size);
234 
235  u_num = ConditionalArgumentGet(1);
236  x_num = ConditionalArgumentGet(0);
237 
238  // numeric evaluation of derivative: dfunc_dx = F
239  for (unsigned int i=0; i<x_size; i++)
240  {
241  // temp variable to substitute in
242  substitute = dfunc_dx[i];
243 
244  // substitute all u_sym with u_num
245  for (unsigned int j=0; j<u_size; j++)
246  substitute = substitute.subs( u_sym[j]==u_num(j+1) );
247 
248  // substitute all x_sym with x_num
249  for (unsigned int j=0; j<x_size; j++)
250  substitute = substitute.subs( x_sym[j]==x_num(j+1) );
251 
252  // convert substitute to matrix. Now all elements in matrix are accessible
253  GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
254 
255  // build matrix F
256  for (unsigned int j=0; j<func_size; j++)
257  F(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
258  }
259 
260  return F;
261  }
262 
263 
264 
265  GiNaC::matrix
267  {
268  return func_sym;
269  }
270 
271 
272  vector<GiNaC::symbol>
274  {
275  return u_sym;
276  }
277 
278  vector<GiNaC::symbol>
280  {
281  return x_sym;
282  }
283 
284  vector<GiNaC::symbol>
286  {
287  if ( cond_size==0 )
288  return vector<GiNaC::symbol>(0);
289  else
290  return cond_sym;
291  }
292 
293 } // End namespace BFL
vector< GiNaC::symbol > InputGet()
return substitution symbols
const MatrixWrapper::SymmetricMatrix & AdditiveNoiseSigmaGet() const
Get the covariance matrix of the Additive Gaussian uncertainty.
friend std::ostream & operator<<(std::ostream &os, NonLinearAnalyticConditionalGaussian_Ginac &p)
output stream for measurement model
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
Conditional Gaussian for an analytic nonlinear system using Ginac:
Abstract Class representing all full Analytical Conditional gaussians with Additive Gaussian Noise...
const MatrixWrapper::ColumnVector & ConditionalArgumentGet(unsigned int n_argument) const
Get the n-th argument of the list.
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
NonLinearAnalyticConditionalGaussian_Ginac(const GiNaC::matrix &func, const vector< GiNaC::symbol > &u, const vector< GiNaC::symbol > &x, const Gaussian &additiveNoise, const vector< GiNaC::symbol > &cond)
constructor
vector< GiNaC::symbol > ConditionalGet()
Get conditional arguments.
virtual MatrixWrapper::ColumnVector ExpectedValueGet() const
Get the expected value E[x] of the pdf.
unsigned int DimensionGet() const
Get the dimension of the argument.
const MatrixWrapper::ColumnVector & AdditiveNoiseMuGet() const
Get the mean Value of the Additive Gaussian uncertainty.


bfl
Author(s): Klaas Gadeyne, Wim Meeussen, Tinne Delaet and many others. See web page for a full contributor list. ROS package maintained by Wim Meeussen.
autogenerated on Mon Feb 28 2022 21:56:33