Bayesian Filtering Library Generated from SVN r
extendedkalmanfilter.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 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 __EXTENDED_KALMAN_FILTER__
22#define __EXTENDED_KALMAN_FILTER__
23
24#include "kalmanfilter.h"
25#include "../pdf/conditionalpdf.h"
26#include "../pdf/gaussian.h"
27# include <map>
28
29namespace BFL
30{
31
44{
45public:
51
54
56 // For realtime use, this function should be called before calling measUpdate
57 /* @param vector containing the dimension of the measurement models which are
58 going to be used
59 */
60 void AllocateMeasModelExt( const vector<unsigned int>& meas_dimensions);
61
63 // For realtime use, this function should be called before calling measUpdate
64 /* @param dimension of the measurement models which is
65 going to be used
66 */
67 void AllocateMeasModelExt( const unsigned int& meas_dimensions);
68
69 // Get NIS value
70 double NisGet(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel, const MatrixWrapper::ColumnVector& z);
71
72private:
73 struct MeasUpdateVariablesExt
74 {
75 SymmetricMatrix _R;
76 Matrix _H;
77 ColumnVector _Z;
78 MeasUpdateVariablesExt() {};
79 MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension):
80 _R(meas_dimension)
81 , _H(meas_dimension,state_dimension)
82 , _Z(meas_dimension)
83{};
84 }; //struct
85
86 protected:
88 const MatrixWrapper::ColumnVector& u);
90 const MatrixWrapper::ColumnVector& z,
91 const MatrixWrapper::ColumnVector& s);
92 // variables to avoid allocation on the heap
93 ColumnVector _x;
94 ColumnVector _J;
95 Matrix _F;
96 SymmetricMatrix _Q;
97 std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
98 std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
99
100
101}; // class
102
103} // End namespace BFL
104
105#endif // __EXTENDED_KALMAN_FILTER__
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
void AllocateMeasModelExt(const unsigned int &meas_dimensions)
Function to allocate memory needed during the measurement update.
virtual ~ExtendedKalmanFilter()
Destructor.
ExtendedKalmanFilter(Gaussian *prior)
void AllocateMeasModelExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
Class representing Gaussian (or normal density)
Definition: gaussian.h:28
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:50