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 #include "extendedkalmanfilter.h"
00021
00022 namespace BFL
00023 {
00024 using namespace MatrixWrapper;
00025
00026
00027
00028 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
00029 #define AnalyticMeas AnalyticMeasurementModelGaussianUncertainty
00030
00031
00032 ExtendedKalmanFilter::ExtendedKalmanFilter(Gaussian* prior)
00033 : KalmanFilter(prior)
00034 , _x(prior->DimensionGet())
00035 , _J(prior->DimensionGet())
00036 , _F(prior->DimensionGet(),prior->DimensionGet())
00037 , _Q(prior->DimensionGet())
00038 {}
00039
00040 ExtendedKalmanFilter::~ExtendedKalmanFilter()
00041 {}
00042
00043 void
00044 ExtendedKalmanFilter::AllocateMeasModelExt(const vector<unsigned int>& meas_dimensions)
00045 {
00046 unsigned int meas_dimension;
00047 for(int i = 0 ; i< meas_dimensions.size(); i++)
00048 {
00049
00050 meas_dimension = meas_dimensions[i];
00051 _mapMeasUpdateVariablesExt_it = _mapMeasUpdateVariablesExt.find(meas_dimension);
00052 if( _mapMeasUpdateVariablesExt_it == _mapMeasUpdateVariablesExt.end())
00053 {
00054
00055 _mapMeasUpdateVariablesExt_it = (_mapMeasUpdateVariablesExt.insert
00056 (std::pair<unsigned int, MeasUpdateVariablesExt>( meas_dimension,MeasUpdateVariablesExt(meas_dimension,_x.rows()) ))).first;
00057 }
00058 }
00059 }
00060
00061 void
00062 ExtendedKalmanFilter::AllocateMeasModelExt(const unsigned int& meas_dimension)
00063 {
00064
00065 _mapMeasUpdateVariablesExt_it = _mapMeasUpdateVariablesExt.find(meas_dimension);
00066 if( _mapMeasUpdateVariablesExt_it == _mapMeasUpdateVariablesExt.end())
00067 {
00068
00069 _mapMeasUpdateVariablesExt_it = (_mapMeasUpdateVariablesExt.insert
00070 (std::pair<unsigned int, MeasUpdateVariablesExt>( meas_dimension,MeasUpdateVariablesExt(meas_dimension,_x.rows()) ))).first;
00071 }
00072 }
00073
00074 void
00075 ExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00076 const ColumnVector& u)
00077 {
00078 _x = _post->ExpectedValueGet();
00079 _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
00080 _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
00081 _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
00082
00083 CalculateSysUpdate(_J, _F, _Q);
00084 }
00085
00086 void
00087 ExtendedKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00088 const ColumnVector& z,
00089 const ColumnVector& s)
00090 {
00091
00092 AllocateMeasModelExt(z.rows());
00093
00094 _x = _post->ExpectedValueGet();
00095 (_mapMeasUpdateVariablesExt_it->second)._Z = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x);
00096 (_mapMeasUpdateVariablesExt_it->second)._H = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x);
00097 (_mapMeasUpdateVariablesExt_it->second)._R = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x);
00098
00099 CalculateMeasUpdate(z, (_mapMeasUpdateVariablesExt_it->second)._Z, (_mapMeasUpdateVariablesExt_it->second)._H, (_mapMeasUpdateVariablesExt_it->second)._R);
00100 }
00101
00102 }
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:52