iteratedextendedkalmanfilter.h
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2002 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 //
20 
21 #ifndef __ITERATED_EXTENDED_KALMAN_FILTER__
22 #define __ITERATED_EXTENDED_KALMAN_FILTER__
23 
24 #include "kalmanfilter.h"
25 #include "../pdf/conditionalpdf.h"
26 #include "../pdf/gaussian.h"
27 #include "innovationCheck.h"
28 # include <map>
29 
30 namespace BFL
31 {
41  {
42  protected:
43  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
44  const MatrixWrapper::ColumnVector& u);
46  const MatrixWrapper::ColumnVector& z,
47  const MatrixWrapper::ColumnVector& s);
48 
49  public:
56  IteratedExtendedKalmanFilter(Gaussian* prior, unsigned int nr_it=1, InnovationCheck* innov = NULL);
57 
60 
62  // For realtime use, this function should be called before calling measUpdate
63  /* @param vector containing the dimension of the measurement models which are
64  going to be used
65  */
66  void AllocateMeasModelIExt( const vector<unsigned int>& meas_dimensions);
67 
69  // For realtime use, this function should be called before calling measUpdate
70  /* @param dimension of the measurement models which is
71  going to be used
72  */
73  void AllocateMeasModelIExt( const unsigned int& meas_dimensions);
74 
75  private:
77  unsigned int _nr_iterations;
80 
82  {
83  SymmetricMatrix _R_i;
84  Matrix _K_i;
85  Matrix _H_i;
86  ColumnVector _Z_i;
88  MeasUpdateVariablesIExt(unsigned int meas_dimension, unsigned int state_dimension):
89  _R_i(meas_dimension)
90  , _K_i(state_dimension,meas_dimension)
91  , _H_i(meas_dimension,state_dimension)
92  , _Z_i(meas_dimension)
93  {};
94  }; //struct
95 
96  // variables to avoid allocation on the heap
97  ColumnVector _x;
98  ColumnVector _x_i;
99  ColumnVector _x_i_prev;
100  ColumnVector _J;
101  ColumnVector _innovation;
102  Matrix _F;
103  SymmetricMatrix _Q;
104  SymmetricMatrix _P_Matrix;
105  Matrix _S_i;
106  std::map<unsigned int, MeasUpdateVariablesIExt> _mapMeasUpdateVariablesIExt;
107  std::map<unsigned int, MeasUpdateVariablesIExt>::iterator _mapMeasUpdateVariablesIExt_it;
108  }; // class
109 
110 } // End namespace BFL
111 
112 #endif // __ITERATED_EXTENDED_KALMAN_FILTER__
113 
MeasUpdateVariablesIExt(unsigned int meas_dimension, unsigned int state_dimension)
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.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
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
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,.
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.


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