iteratedextendedkalmanfilter.cpp
Go to the documentation of this file.
00001 // $Id: iteratedextendedkalmanfilter.cpp 30022 2009-03-06 15:10:30Z kgadeyne $
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 //                    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 "iteratedextendedkalmanfilter.h"
00021 
00022 namespace BFL
00023 {
00024   using namespace MatrixWrapper;
00025 
00026 
00027 #define AnalyticSys    AnalyticSystemModelGaussianUncertainty
00028 #define AnalyticMeas   AnalyticMeasurementModelGaussianUncertainty
00029 
00030 
00031   IteratedExtendedKalmanFilter::IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it, InnovationCheck* I)
00032     : KalmanFilter(prior)
00033     , _nr_iterations(nr_it)
00034     , _innovationChecker(I)
00035     , _x(prior->DimensionGet())
00036     , _x_i(prior->DimensionGet())
00037     , _x_i_prev(prior->DimensionGet())
00038     , _J(prior->DimensionGet())
00039     , _innovation(prior->DimensionGet())
00040     , _F(prior->DimensionGet(),prior->DimensionGet())
00041     , _Q(prior->DimensionGet())
00042     , _P_Matrix(prior->DimensionGet())
00043   {}
00044 
00045   IteratedExtendedKalmanFilter::~IteratedExtendedKalmanFilter()
00046   {}
00047 
00048   void
00049   IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const vector<unsigned int>& meas_dimensions)
00050   {
00051     unsigned int meas_dimension;
00052     for(int i = 0 ; i< meas_dimensions.size(); i++)
00053     {
00054         // find if variables with size meas_sizes[i] are already allocated
00055         meas_dimension = meas_dimensions[i];
00056         _mapMeasUpdateVariablesIExt_it =  _mapMeasUpdateVariablesIExt.find(meas_dimension);
00057         if( _mapMeasUpdateVariablesIExt_it == _mapMeasUpdateVariablesIExt.end())
00058         {
00059             //variables with size z.rows() not allocated yet
00060             _mapMeasUpdateVariablesIExt_it = (_mapMeasUpdateVariablesIExt.insert
00061                 (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
00062          }
00063      }
00064   }
00065 
00066   void
00067   IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const unsigned int& meas_dimension)
00068   {
00069      // find if variables with size meas_sizes[i] are already allocated
00070      _mapMeasUpdateVariablesIExt_it =  _mapMeasUpdateVariablesIExt.find(meas_dimension);
00071      if( _mapMeasUpdateVariablesIExt_it == _mapMeasUpdateVariablesIExt.end())
00072      {
00073          //variables with size z.rows() not allocated yet
00074          _mapMeasUpdateVariablesIExt_it = (_mapMeasUpdateVariablesIExt.insert
00075              (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
00076       }
00077   }
00078 
00079   void
00080   IteratedExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00081                                           const ColumnVector& u)
00082   {
00083     _x = _post->ExpectedValueGet();
00084     _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
00085     _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
00086     _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
00087 
00088     /*
00089       cout << "x :\n" << x << endl;
00090       cout << "J:\n " << J << endl;
00091       cout << "F:\n " << F << endl;
00092       cout << "Q:\n " << Q << endl;
00093     */
00094 
00095     CalculateSysUpdate(_J, _F, _Q);
00096   }
00097 
00098   void
00099   IteratedExtendedKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00100                                            const ColumnVector& z,
00101                                            const ColumnVector& s)
00102   {
00103     // allocate measurement for z.rows() if needed
00104     AllocateMeasModelIExt(z.rows());
00105 
00106     _x = _post->ExpectedValueGet();
00107     _P_Matrix = _post->CovarianceGet();
00108     _x_i = _post->ExpectedValueGet();
00109 
00110     bool            test_innovation = true;
00111 
00112     for (unsigned int i=0; i<_nr_iterations && test_innovation; i++)
00113       {
00114     _x_i_prev = _x_i;
00115         (_mapMeasUpdateVariablesIExt_it->second)._H_i  = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x_i);
00116         (_mapMeasUpdateVariablesIExt_it->second)._R_i  = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x_i);
00117         _S_i  = ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) ) + (Matrix)((_mapMeasUpdateVariablesIExt_it->second)._R_i);
00118         (_mapMeasUpdateVariablesIExt_it->second)._K_i  = (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) * (_S_i.inverse());
00119         (_mapMeasUpdateVariablesIExt_it->second)._Z_i  = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x_i) + ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (_x - _x_i) );
00120         _x_i  = _x + (_mapMeasUpdateVariablesIExt_it->second)._K_i * (z - (_mapMeasUpdateVariablesIExt_it->second)._Z_i);
00121     _innovation = (_x_i - _x_i_prev);
00122     if (_innovationChecker != NULL)
00123         test_innovation = _innovationChecker->check(_innovation) ; //test if the innovation is not too small
00124 
00125         /*
00126           cout << "H_i :\n" << H_i << endl;
00127           cout << "R_i:\n " << R_i << endl;
00128           cout << "S_i:\n " << S_i << endl;
00129           cout << "S_i inverse:\n " << S_i.inverse() << endl;
00130           cout << "innovation:\n" << z- ( ((NLinMeas*)measmodel)->PredictionGet(s,x_i) )<< endl;
00131           cout << "x_i:\n" << x_i << endl;
00132         */
00133       }
00134 
00135     CalculateMeasUpdate(z, (_mapMeasUpdateVariablesIExt_it->second)._Z_i, (_mapMeasUpdateVariablesIExt_it->second)._H_i, (_mapMeasUpdateVariablesIExt_it->second)._R_i);
00136   }
00137 
00138 }


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 Thu Feb 11 2016 22:31:56