iteratedextendedkalmanfilter.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 // Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU Lesser General Public License as published by
8 // the Free Software Foundation; either version 2.1 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU Lesser General Public License for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
19 //
21 
22 namespace BFL
23 {
24  using namespace MatrixWrapper;
25 
26 
27 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
28 #define AnalyticMeas AnalyticMeasurementModelGaussianUncertainty
29 
30 
32  : KalmanFilter(prior)
33  , _nr_iterations(nr_it)
34  , _innovationChecker(I)
35  , _x(prior->DimensionGet())
36  , _x_i(prior->DimensionGet())
37  , _x_i_prev(prior->DimensionGet())
38  , _J(prior->DimensionGet())
39  , _innovation(prior->DimensionGet())
40  , _F(prior->DimensionGet(),prior->DimensionGet())
41  , _Q(prior->DimensionGet())
42  , _P_Matrix(prior->DimensionGet())
43  {}
44 
46  {}
47 
48  void
49  IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const vector<unsigned int>& meas_dimensions)
50  {
51  unsigned int meas_dimension;
52  for(int i = 0 ; i< meas_dimensions.size(); i++)
53  {
54  // find if variables with size meas_sizes[i] are already allocated
55  meas_dimension = meas_dimensions[i];
58  {
59  //variables with size z.rows() not allocated yet
61  (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
62  }
63  }
64  }
65 
66  void
67  IteratedExtendedKalmanFilter::AllocateMeasModelIExt(const unsigned int& meas_dimension)
68  {
69  // find if variables with size meas_sizes[i] are already allocated
72  {
73  //variables with size z.rows() not allocated yet
75  (std::pair<unsigned int, MeasUpdateVariablesIExt>( meas_dimension,MeasUpdateVariablesIExt(meas_dimension,_x.rows()) ))).first;
76  }
77  }
78 
79  void
81  const ColumnVector& u)
82  {
84  _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
85  _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
86  _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
87 
88  /*
89  cout << "x :\n" << x << endl;
90  cout << "J:\n " << J << endl;
91  cout << "F:\n " << F << endl;
92  cout << "Q:\n " << Q << endl;
93  */
94 
96  }
97 
98  void
100  const ColumnVector& z,
101  const ColumnVector& s)
102  {
103  // allocate measurement for z.rows() if needed
104  AllocateMeasModelIExt(z.rows());
105 
109 
110  bool test_innovation = true;
111 
112  for (unsigned int i=0; i<_nr_iterations && test_innovation; i++)
113  {
114  _x_i_prev = _x_i;
115  (_mapMeasUpdateVariablesIExt_it->second)._H_i = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x_i);
116  (_mapMeasUpdateVariablesIExt_it->second)._R_i = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x_i);
117  _S_i = ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) ) + (Matrix)((_mapMeasUpdateVariablesIExt_it->second)._R_i);
118  (_mapMeasUpdateVariablesIExt_it->second)._K_i = (Matrix)_P_Matrix * ((_mapMeasUpdateVariablesIExt_it->second)._H_i.transpose()) * (_S_i.inverse());
119  (_mapMeasUpdateVariablesIExt_it->second)._Z_i = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x_i) + ( (_mapMeasUpdateVariablesIExt_it->second)._H_i * (_x - _x_i) );
120  _x_i = _x + (_mapMeasUpdateVariablesIExt_it->second)._K_i * (z - (_mapMeasUpdateVariablesIExt_it->second)._Z_i);
121  _innovation = (_x_i - _x_i_prev);
122  if (_innovationChecker != NULL)
123  test_innovation = _innovationChecker->check(_innovation) ; //test if the innovation is not too small
124 
125  /*
126  cout << "H_i :\n" << H_i << endl;
127  cout << "R_i:\n " << R_i << endl;
128  cout << "S_i:\n " << S_i << endl;
129  cout << "S_i inverse:\n " << S_i.inverse() << endl;
130  cout << "innovation:\n" << z- ( ((NLinMeas*)measmodel)->PredictionGet(s,x_i) )<< endl;
131  cout << "x_i:\n" << x_i << endl;
132  */
133  }
134 
136  }
137 
138 }
IteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1, InnovationCheck *innov=NULL)
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
Class implementing an innovationCheck used in IEKF.
#define AnalyticMeas
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
Pdf< MatrixWrapper::ColumnVector > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
InnovationCheck * _innovationChecker
pointer to InnovationCheck (to end the iterations if the innovation is too small) ...
unsigned int _nr_iterations
number of iterations for iterated extended kalman filter
std::map< unsigned int, MeasUpdateVariablesIExt > _mapMeasUpdateVariablesIExt
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
#define AnalyticSys
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
std::map< unsigned int, MeasUpdateVariablesIExt >::iterator _mapMeasUpdateVariablesIExt_it
void AllocateMeasModelIExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
void CalculateMeasUpdate(const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R)
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
bool check(MatrixWrapper::ColumnVector innovation)
check Innovation
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.


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