Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #ifndef __KALMAN_FILTER__
00022 #define __KALMAN_FILTER__
00023
00024 #include "filter.h"
00025 #include "../pdf/gaussian.h"
00026 #include "../model/analyticmeasurementmodel_gaussianuncertainty.h"
00027 #include "../model/analyticsystemmodel_gaussianuncertainty.h"
00028 # include <map>
00029
00030 namespace BFL
00031 {
00032
00034
00049 class KalmanFilter : public Filter<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
00050 {
00051 public:
00053
00056 KalmanFilter(Gaussian* prior);
00057
00059 virtual ~KalmanFilter();
00060
00061
00062 virtual Gaussian* PostGet();
00063
00065
00066
00067
00068
00069 void AllocateMeasModel( const vector<unsigned int>& meas_dimensions);
00070
00072
00073
00074
00075
00076 void AllocateMeasModel( const unsigned int& meas_dimensions);
00077
00078 private:
00079 struct MeasUpdateVariables
00080 {
00081 Matrix _S_Matrix;
00082 Matrix _K;
00083 ColumnVector _innov;
00084 Matrix _postHT;
00085 MeasUpdateVariables() {};
00086 MeasUpdateVariables(unsigned int meas_dimension, unsigned int state_dimension):
00087 _S_Matrix(meas_dimension,meas_dimension)
00088 , _K(state_dimension,meas_dimension)
00089 , _innov(meas_dimension)
00090 , _postHT(state_dimension,meas_dimension)
00091 {};
00092 };
00093
00094 protected:
00095
00096 ColumnVector _Mu_new;
00097 SymmetricMatrix _Sigma_new;
00098 Matrix _Sigma_temp;
00099 Matrix _Sigma_temp_par;
00100 Matrix _SMatrix;
00101 Matrix _K;
00102 std::map<unsigned int, MeasUpdateVariables> _mapMeasUpdateVariables;
00103 std::map<unsigned int, MeasUpdateVariables>::iterator _mapMeasUpdateVariables_it;
00104
00105
00110 friend class NonminimalKalmanFilter;
00111
00113 void PostSigmaSet( const MatrixWrapper::SymmetricMatrix& s);
00114
00116 void PostMuSet( const MatrixWrapper::ColumnVector& c);
00117
00122 void CalculateSysUpdate(const MatrixWrapper::ColumnVector& J, const MatrixWrapper::Matrix& F, const MatrixWrapper::SymmetricMatrix& Q);
00123
00130 void CalculateMeasUpdate(const MatrixWrapper::ColumnVector& z, const MatrixWrapper::ColumnVector& Z, const MatrixWrapper::Matrix& H, const MatrixWrapper::SymmetricMatrix& R);
00131
00133
00138 virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00139 const MatrixWrapper::ColumnVector& u) = 0;
00140
00142
00153 virtual void MeasUpdate(MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00154 const MatrixWrapper::ColumnVector& z,
00155 const MatrixWrapper::ColumnVector& s) = 0;
00156
00157 virtual bool UpdateInternal(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
00158 const MatrixWrapper::ColumnVector& u,
00159 MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>* const measmodel,
00160 const MatrixWrapper::ColumnVector& z,
00161 const MatrixWrapper::ColumnVector& s);
00162
00163 };
00164
00165
00166
00167 }
00168
00169 #endif // __KALMAN_FILTER__
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 Sun Oct 5 2014 22:29:53