nonlinearanalyticconditionalgaussian_ginac.cpp
Go to the documentation of this file.
00001 // $Id$
00002 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00003 //                    Wim Meeussen  <wim dot meeussen at mech dot kuleuven dot ac dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 //
00019 
00020 #include "nonlinearanalyticconditionalgaussian_ginac.h"
00021 #include <cmath>
00022 #include <cassert>
00023 #include "../wrappers/rng/rng.h" // Wrapper around several rng
00024                                  // libraries
00025 
00026 namespace BFL
00027 {
00028   // constructor
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     // test for consistent input
00048     assert (func_sym.cols() == 1);
00049     assert (additiveNoise.DimensionGet() == cond_size);
00050 
00051     // derive func to cond
00052     for (unsigned int i=0; i < cond_size; i++)
00053       dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
00054 
00055     // derive func to x
00056     for (unsigned int i=0; i < x_size; i++)
00057       dfunc_dx[i] = func_sym.diff(x_sym[i]);
00058   }
00059 
00060   // constructor
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     // test for consistent input
00077     assert (func_sym.cols() == 1);
00078 
00079     // derive func to x
00080     for (unsigned int i=0; i < x_size; i++)
00081       dfunc_dx[i] = func_sym.diff(x_sym[i]);
00082   }
00083   // copy constructor
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     // test for consistent input
00099     assert (func_sym.cols() == 1);
00100     if (cond_size!=0) assert (g.AdditiveNoiseSigmaGet().rows() == cond_size);
00101 
00102     // derive func to cond
00103     for (unsigned int i=0; i<cond_size; i++)
00104       dfunc_dcond[i] = func_sym.diff(cond_sym[i]);
00105 
00106     // derive func to x
00107     for (unsigned int i=0; i < x_size; i++)
00108       dfunc_dx[i] = func_sym.diff(x_sym[i]);
00109   }
00110 
00111   // destructor
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     // use Mu of additive noise
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     // evaluate func
00147     for (unsigned int i=0; i<func_size; i++)
00148       {
00149         // temp variable to substitute in
00150         substitute = func_sym(i,0);
00151 
00152         // substitute all u_sym with u_num
00153         for (unsigned int j=0; j<u_size; j++)
00154           substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00155 
00156         // substitute all x_sym with x_num
00157         for (unsigned int j=0; j<x_size; j++)
00158           substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00159 
00160         // build matrix func_num
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             // temp variable to substitute in
00189             substitute = dfunc_dcond[i];
00190 
00191             // substitute all u_sym with u_num
00192             for (unsigned int j=0; j<u_size; j++)
00193               substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00194 
00195             // substitute all x_sym with x_num
00196             for (unsigned int j=0; j<x_size; j++)
00197               substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00198 
00199             // convert substitute back to matrix
00200             GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
00201 
00202             // build matrix D
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         //cout << "D: " << D << endl;
00207         //cout << "CondCov:\n" << (Matrix)cond_covariance << endl;
00208         MatrixWrapper::Matrix temp = D * (MatrixWrapper::Matrix)AdditiveNoiseSigmaGet() * D.transpose();
00209 
00210         // convert func_covariance_matrix to symmetric matrix
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     // Check if i = 0, since this is the old df_dxGet method!
00227     assert(i == 0);
00228 
00229     // evaluate function
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     // numeric evaluation of derivative: dfunc_dx = F
00239     for (unsigned int i=0; i<x_size; i++)
00240       {
00241         // temp variable to substitute in
00242         substitute = dfunc_dx[i];
00243 
00244         // substitute all u_sym with u_num
00245         for (unsigned int j=0; j<u_size; j++)
00246           substitute = substitute.subs( u_sym[j]==u_num(j+1) );
00247 
00248         // substitute all x_sym with x_num
00249         for (unsigned int j=0; j<x_size; j++)
00250           substitute = substitute.subs( x_sym[j]==x_num(j+1) );
00251 
00252         // convert substitute to matrix. Now all elements in matrix are accessible
00253         GiNaC::matrix substitute_matrix = GiNaC::ex_to<GiNaC::matrix>(substitute);
00254 
00255         // build matrix F
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 } // End namespace BFL


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 11 2019 03:45:12