Bayesian Filtering Library Generated from SVN r
iteratedextendedkalmanfilter.h
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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
30namespace BFL
31{
41 {
42 protected:
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;
79 InnovationCheck* _innovationChecker;
80
81 struct MeasUpdateVariablesIExt
82 {
83 SymmetricMatrix _R_i;
84 Matrix _K_i;
85 Matrix _H_i;
86 ColumnVector _Z_i;
87 MeasUpdateVariablesIExt() {};
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
Class representing Gaussian (or normal density)
Definition gaussian.h:28
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)
void AllocateMeasModelIExt(const unsigned int &meas_dimensions)
Function to allocate memory needed during the measurement update.
IteratedExtendedKalmanFilter(Gaussian *prior, unsigned int nr_it=1, InnovationCheck *innov=NULL)
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
void AllocateMeasModelIExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
virtual ~IteratedExtendedKalmanFilter()
Destructor.
Class representing the family of all Kalman Filters (EKF, IEKF, ...)