kalmanfilter.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 be>
00004 //                    Tinne De Laet  <tinne dot delaet at mech dot kuleuven dot be>
00005 //
00006 // This program is free software; you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation; either version 2.1 of the License, or
00009 // (at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program; if not, write to the Free Software
00018 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00019 //
00020 #include "kalmanfilter.h"
00021 #include <cmath>
00022 
00023 namespace BFL
00024 {
00025   using namespace MatrixWrapper;
00026 
00027 
00028   KalmanFilter::KalmanFilter(Gaussian * prior)
00029     : Filter<ColumnVector,ColumnVector>(prior)
00030     , _Mu_new(prior->DimensionGet())
00031     , _Sigma_new(prior->DimensionGet())
00032     , _Sigma_temp(prior->DimensionGet(),prior->DimensionGet())
00033     , _Sigma_temp_par(prior->DimensionGet(),prior->DimensionGet())
00034   {
00035     // create posterior dencity
00036     _post = new Gaussian(*prior);
00037   }
00038 
00039   KalmanFilter::~KalmanFilter()
00040   {
00041     delete _post;
00042   }
00043 
00044   void
00045   KalmanFilter::AllocateMeasModel(const vector<unsigned int>& meas_dimensions)
00046   {
00047     unsigned int meas_dimension;
00048     for(int i = 0 ; i< meas_dimensions.size(); i++)
00049     {
00050         // find if variables with size meas_sizes[i] are already allocated
00051         meas_dimension = meas_dimensions[i];
00052         _mapMeasUpdateVariables_it =  _mapMeasUpdateVariables.find(meas_dimension);
00053         if( _mapMeasUpdateVariables_it == _mapMeasUpdateVariables.end())
00054         {
00055             //variables with size z.rows() not allocated yet
00056             _mapMeasUpdateVariables_it = (_mapMeasUpdateVariables.insert
00057                 (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
00058          }
00059      }
00060   }
00061 
00062   void
00063   KalmanFilter::AllocateMeasModel(const unsigned int& meas_dimension)
00064   {
00065      // find if variables with size meas_sizes[i] are already allocated
00066      _mapMeasUpdateVariables_it =  _mapMeasUpdateVariables.find(meas_dimension);
00067      if( _mapMeasUpdateVariables_it == _mapMeasUpdateVariables.end())
00068      {
00069          //variables with size z.rows() not allocated yet
00070          _mapMeasUpdateVariables_it = (_mapMeasUpdateVariables.insert
00071              (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
00072       }
00073   }
00074 
00075   void
00076   KalmanFilter::CalculateSysUpdate(const ColumnVector& J, const Matrix& F, const SymmetricMatrix& Q)
00077   {
00078     _Sigma_temp = F * ( (Matrix)_post->CovarianceGet() * F.transpose());
00079     _Sigma_temp += (Matrix)Q;
00080     _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
00081 
00082     // set new state gaussian
00083     PostMuSet   ( J );
00084     PostSigmaSet( _Sigma_new );
00085   }
00086 
00087   void
00088   KalmanFilter::CalculateMeasUpdate(const ColumnVector& z, const ColumnVector& Z, const Matrix& H, const SymmetricMatrix& R)
00089   {
00090     // allocate measurement for z.rows() if needed
00091     AllocateMeasModel(z.rows());
00092 
00093     (_mapMeasUpdateVariables_it->second)._postHT =   (Matrix)(_post->CovarianceGet()) * H.transpose() ;
00094     (_mapMeasUpdateVariables_it->second)._S_Matrix =  H * (_mapMeasUpdateVariables_it->second)._postHT;
00095     (_mapMeasUpdateVariables_it->second)._S_Matrix += (Matrix)R;
00096 
00097     // _K = covariance * H' * S(-1)
00098     (_mapMeasUpdateVariables_it->second)._K =  (_mapMeasUpdateVariables_it->second)._postHT * ( (_mapMeasUpdateVariables_it->second)._S_Matrix.inverse());
00099 
00100     // calcutate new state gaussian
00101     // Mu = expectedValue + K*(z-Z)
00102     (_mapMeasUpdateVariables_it->second)._innov = z-Z;
00103      _Mu_new  =  (_mapMeasUpdateVariables_it->second)._K * (_mapMeasUpdateVariables_it->second)._innov  ;
00104      _Mu_new  +=  _post->ExpectedValueGet() ;
00105     // Sigma = post - K*H*post
00106     _Sigma_temp = (_post->CovarianceGet());
00107     _Sigma_temp_par = (_mapMeasUpdateVariables_it->second)._K * H ;
00108     _Sigma_temp -=  _Sigma_temp_par * (Matrix)(_post->CovarianceGet());
00109     // convert to symmetric matrix
00110     _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
00111 
00112     // set new state gaussian
00113     PostMuSet   ( _Mu_new );
00114     PostSigmaSet( _Sigma_new );
00115 
00116     /*
00117       cout << "H:\n" << H << endl;
00118       cout << "R:\n" << R << endl;
00119       cout << "Z:\n" << Z << endl;
00120       cout << "inov:\n" << z-Z << endl;
00121       cout << "S:\n" << S << endl;
00122       cout << "S.inverse:\n" << S.inverse() << endl;
00123       cout << "K:\n" << K << endl;
00124       cout << "Mu_new:\n" << Mu_new << endl;
00125       cout << "sigma_new\n" << Sigma_new << endl;
00126     */
00127   }
00128 
00129   bool
00130   KalmanFilter::UpdateInternal(SystemModel<ColumnVector>* const sysmodel,
00131                                const ColumnVector& u,
00132                                MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00133                                const ColumnVector& z, const ColumnVector& s)
00134   {
00135     if (sysmodel != NULL)
00136       {
00137         SysUpdate(sysmodel,u);
00138       }
00139     if (measmodel != NULL)
00140       {
00141         MeasUpdate(measmodel,z,s);
00142       }
00143     return true;
00144   }
00145 
00146   void
00147   KalmanFilter::PostSigmaSet( const SymmetricMatrix& s)
00148   {
00149     dynamic_cast<Gaussian *>(_post)->CovarianceSet(s);
00150   }
00151 
00152   void
00153   KalmanFilter::PostMuSet( const ColumnVector& c)
00154   {
00155     dynamic_cast<Gaussian *>(_post)->ExpectedValueSet(c);
00156   }
00157 
00158 
00159   Gaussian*
00160   KalmanFilter::PostGet()
00161   {
00162     return (Gaussian*)Filter<ColumnVector,ColumnVector>::PostGet();
00163   }
00164 
00165 }


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