Bayesian Filtering Library Generated from SVN r
kalmanfilter.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 __KALMAN_FILTER__
22#define __KALMAN_FILTER__
23
24#include "filter.h"
25#include "../pdf/gaussian.h"
26#include "../model/analyticmeasurementmodel_gaussianuncertainty.h"
27#include "../model/analyticsystemmodel_gaussianuncertainty.h"
28# include <map>
29
30namespace BFL
31{
32
34
49class KalmanFilter : public Filter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
50{
51public:
53
57
59 virtual ~KalmanFilter();
60
61 // implement virtual function
62 virtual Gaussian* PostGet();
63
65 // For realtime use, this function should be called before calling measUpdate
66 /* @param vector containing the dimension of the measurement models which are
67 going to be used
68 */
69 void AllocateMeasModel( const vector<unsigned int>& meas_dimensions);
70
72 // For realtime use, this function should be called before calling measUpdate
73 /* @param dimension of the measurement models which is
74 going to be used
75 */
76 void AllocateMeasModel( const unsigned int& meas_dimensions);
77
78private:
79 struct MeasUpdateVariables
80 {
81 Matrix _S_Matrix;
82 Matrix _K;
83 ColumnVector _innov;
84 Matrix _postHT;
85 MeasUpdateVariables() {};
86 MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension):
87 _S_Matrix(meas_dimension,meas_dimension)
88 , _K(state_dimension,meas_dimension)
89 , _innov(meas_dimension)
90 , _postHT(state_dimension,meas_dimension)
91{};
92 }; //struct
93
94protected:
95 // variables to avoid allocation during update calls
96 ColumnVector _Mu_new;
97 SymmetricMatrix _Sigma_new;
98 Matrix _Sigma_temp;
99 Matrix _Sigma_temp_par;
100 Matrix _SMatrix;
101 Matrix _K;
102 double _Nis;
103 std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables;
104 std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it;
105
106
112
114 void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s);
115
117 void PostMuSet( const MatrixWrapper::ColumnVector& c);
118
123 void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q);
124
131 void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
132
134
140 const MatrixWrapper::ColumnVector& u) = 0;
141
143
155 const MatrixWrapper::ColumnVector& z,
156 const MatrixWrapper::ColumnVector& s) = 0;
157
159 const MatrixWrapper::ColumnVector& u,
161 const MatrixWrapper::ColumnVector& z,
162 const MatrixWrapper::ColumnVector& s);
163
164 // Calculate Normalised Innovation Squared (NIS) value
165 void CalculateNis(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
166
167}; // class
168
169
170
171} // End namespace BFL
172
173#endif // __KALMAN_FILTER__
Abstract class representing an interface for Bayesian Filters.
Definition filter.h:78
Class representing Gaussian (or normal density)
Definition gaussian.h:28
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
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)=0
System Update.
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
void AllocateMeasModel(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
void AllocateMeasModel(const unsigned int &meas_dimensions)
Function to allocate memory needed during the measurement update.
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
virtual ~KalmanFilter()
Destructor.
virtual Gaussian * PostGet()
Get Posterior density.
KalmanFilter(Gaussian *prior)
Constructor.
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
virtual bool UpdateInternal(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Actual implementation of Update, varies along filters.
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)=0
Measurement Update (overloaded)