00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #include "nonlinearanalyticconditionalgaussian_ginac.h"
00021 #include <cmath>
00022 #include <cassert>
00023 #include "../wrappers/rng/rng.h"
00024
00025
00026 namespace BFL
00027 {
00028
00029 NonLinearAnalyticConditionalGaussian_Ginac::NonLinearAnalyticConditionalGaussian_Ginac
00030 (const GiNaC::matrix& func,
00031 const vector<GiNaC::symbol>& u,
00032 const vector<GiNaC::symbol>& x,
00033 const Gaussian& additiveNoise,
00034 const vector<GiNaC::symbol>& cond )
00035 :AnalyticConditionalGaussianAdditiveNoise(additiveNoise,3),
00036 func_sym (func),
00037 cond_sym (cond),
00038 u_sym (u),
00039 x_sym (x),
00040 cond_size (cond_sym.size()),
00041 u_size (u_sym.size()),
00042 x_size (x_sym.size()),
00043 func_size (func_sym.rows()),
00044 dfunc_dcond (cond_size),
00045 dfunc_dx (x_size)
00046 {
00047
00048 assert (func_sym.cols() == 1);
00049 assert (additiveNoise.DimensionGet() == cond_size);
00050
00051
00052 for (unsigned int i=0; i < cond_size; i++)
00053 dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
00054
00055
00056 for (unsigned int i=0; i < x_size; i++)
00057 dfunc_dx[i] = func_sym.diff(x_sym[i]);
00058 }
00059
00060
00061 NonLinearAnalyticConditionalGaussian_Ginac::NonLinearAnalyticConditionalGaussian_Ginac
00062 (const GiNaC::matrix& func,
00063 const vector<GiNaC::symbol>& u,
00064 const vector<GiNaC::symbol>& x,
00065 const Gaussian& additiveNoise)
00066 : AnalyticConditionalGaussianAdditiveNoise(additiveNoise,2),
00067 func_sym (func),
00068 u_sym (u),
00069 x_sym (x),
00070 cond_size (0),
00071 u_size (u_sym.size()),
00072 x_size (x_sym.size()),
00073 func_size (func_sym.rows()),
00074 dfunc_dx (x_size)
00075 {
00076
00077 assert (func_sym.cols() == 1);
00078
00079
00080 for (unsigned int i=0; i < x_size; i++)
00081 dfunc_dx[i] = func_sym.diff(x_sym[i]);
00082 }
00083
00084 NonLinearAnalyticConditionalGaussian_Ginac::NonLinearAnalyticConditionalGaussian_Ginac
00085 (const NonLinearAnalyticConditionalGaussian_Ginac& g)
00086 : AnalyticConditionalGaussianAdditiveNoise( Gaussian(g.AdditiveNoiseMuGet(),g.AdditiveNoiseSigmaGet()) ,2),
00087 func_sym (g.func_sym),
00088 cond_sym (g.cond_sym),
00089 u_sym (g.u_sym),
00090 x_sym (g.x_sym),
00091 cond_size (cond_sym.size()),
00092 u_size (u_sym.size()),
00093 x_size (x_sym.size()),
00094 func_size (func_sym.rows()),
00095 dfunc_dcond (cond_size),
00096 dfunc_dx (x_size)
00097 {
00098
00099 assert (func_sym.cols() == 1);
00100 if (cond_size!=0) assert (g.AdditiveNoiseSigmaGet().rows() == cond_size);
00101
00102
00103 for (unsigned int i=0; i<cond_size; i++)
00104 dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
00105
00106
00107 for (unsigned int i=0; i < x_size; i++)
00108 dfunc_dx[i] = func_sym.diff(x_sym[i]);
00109 }
00110
00111
00112 NonLinearAnalyticConditionalGaussian_Ginac::~NonLinearAnalyticConditionalGaussian_Ginac()
00113 {}
00114
00115
00116 std::ostream& operator<< (std::ostream& os, NonLinearAnalyticConditionalGaussian_Ginac& p)
00117 {
00118 os << "function: " << p.func_size << endl;
00119 os << "input: " << p.u_size << endl;
00120 os << "State: " << p.x_size << endl;
00121 os << "Conditional: " << ((p.cond_size) !=0) << endl;
00122 return os;
00123 }
00124
00125
00126 MatrixWrapper::ColumnVector
00127 NonLinearAnalyticConditionalGaussian_Ginac::ExpectedValueGet() const
00128 {
00129 MatrixWrapper::ColumnVector u_num (u_size);
00130 MatrixWrapper::ColumnVector x_num (x_size);
00131 MatrixWrapper::ColumnVector func_num(func_size);
00132 GiNaC::ex substitute (func_size);
00133 MatrixWrapper::ColumnVector expected(func_size);
00134
00135 u_num = ConditionalArgumentGet(1);
00136 x_num = ConditionalArgumentGet(0);
00137
00138
00139 if (cond_size!=0)
00140 for (unsigned int i=0; i<u_size; i++)
00141 for (unsigned int j=0; j<cond_size; j++)
00142 if (u_sym[i] == cond_sym[j])
00143 u_num(i+1) += (this->AdditiveNoiseMuGet())(j+1);
00144
00145
00146
00147 for (unsigned int i=0; i<func_size; i++)
00148 {
00149
00150 substitute = func_sym(i,0);
00151
00152
00153 for (unsigned int j=0; j<u_size; j++)
00154 substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00155
00156
00157 for (unsigned int j=0; j<x_size; j++)
00158 substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00159
00160
00161 func_num(i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute.evalf() ).to_double();
00162 }
00163 expected = func_num;
00164
00165 if (cond_size==0)
00166 expected += AdditiveNoiseMuGet();
00167
00168 return expected;
00169 }
00170
00171
00172 MatrixWrapper::SymmetricMatrix
00173 NonLinearAnalyticConditionalGaussian_Ginac::CovarianceGet() const
00174 {
00175 if (cond_size!=0)
00176 {
00177 MatrixWrapper::ColumnVector u_num (u_size);
00178 MatrixWrapper::ColumnVector x_num (x_size);
00179 GiNaC::ex substitute (func_size);
00180 MatrixWrapper::Matrix D (func_size,cond_size);
00181
00182
00183 u_num = ConditionalArgumentGet(1);
00184 x_num = ConditionalArgumentGet(0);
00185
00186 for (unsigned int i=0; i<cond_size; i++)
00187 {
00188
00189 substitute = dfunc_dcond[i];
00190
00191
00192 for (unsigned int j=0; j<u_size; j++)
00193 substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00194
00195
00196 for (unsigned int j=0; j<x_size; j++)
00197 substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00198
00199
00200 GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
00201
00202
00203 for (unsigned int j=0; j<func_size; j++)
00204 D(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
00205 }
00206
00207
00208 MatrixWrapper::Matrix temp = D * (MatrixWrapper::Matrix)AdditiveNoiseSigmaGet() * D.transpose();
00209
00210
00211 MatrixWrapper::SymmetricMatrix additiveNoise(temp.rows());
00212 temp.convertToSymmetricMatrix(additiveNoise);
00213 return additiveNoise;
00214
00215 }
00216 else
00217 {
00218 return AdditiveNoiseSigmaGet();
00219 }
00220 }
00221
00222
00223 MatrixWrapper::Matrix
00224 NonLinearAnalyticConditionalGaussian_Ginac::dfGet(unsigned int i) const
00225 {
00226
00227 assert(i == 0);
00228
00229
00230 MatrixWrapper::ColumnVector u_num (u_size);
00231 MatrixWrapper::ColumnVector x_num (x_size);
00232 GiNaC::ex substitute (func_size);
00233 MatrixWrapper::Matrix F (func_size, x_size);
00234
00235 u_num = ConditionalArgumentGet(1);
00236 x_num = ConditionalArgumentGet(0);
00237
00238
00239 for (unsigned int i=0; i<x_size; i++)
00240 {
00241
00242 substitute = dfunc_dx[i];
00243
00244
00245 for (unsigned int j=0; j<u_size; j++)
00246 substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00247
00248
00249 for (unsigned int j=0; j<x_size; j++)
00250 substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00251
00252
00253 GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
00254
00255
00256 for (unsigned int j=0; j<func_size; j++)
00257 F(j+1,i+1) = GiNaC::ex_to<GiNaC::numeric>( substitute_matrix(j,0).evalf() ).to_double();
00258 }
00259
00260 return F;
00261 }
00262
00263
00264
00265 GiNaC::matrix
00266 NonLinearAnalyticConditionalGaussian_Ginac::FunctionGet()
00267 {
00268 return func_sym;
00269 }
00270
00271
00272 vector<GiNaC::symbol>
00273 NonLinearAnalyticConditionalGaussian_Ginac::InputGet()
00274 {
00275 return u_sym;
00276 }
00277
00278 vector<GiNaC::symbol>
00279 NonLinearAnalyticConditionalGaussian_Ginac::StateGet()
00280 {
00281 return x_sym;
00282 }
00283
00284 vector<GiNaC::symbol>
00285 NonLinearAnalyticConditionalGaussian_Ginac::ConditionalGet()
00286 {
00287 if ( cond_size==0 )
00288 return vector<GiNaC::symbol>(0);
00289 else
00290 return cond_sym;
00291 }
00292
00293 }