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 "kalmanfilter.h"
00021 #include <cmath>
00022
00023 namespace BFL
00024 {
00025 using namespace MatrixWrapper;
00026
00027
00028 KalmanFilter::KalmanFilter(Gaussian * prior)
00029 : Filter<ColumnVector,ColumnVector>(prior)
00030 , _Mu_new(prior->DimensionGet())
00031 , _Sigma_new(prior->DimensionGet())
00032 , _Sigma_temp(prior->DimensionGet(),prior->DimensionGet())
00033 , _Sigma_temp_par(prior->DimensionGet(),prior->DimensionGet())
00034 {
00035
00036 _post = new Gaussian(*prior);
00037 }
00038
00039 KalmanFilter::~KalmanFilter()
00040 {
00041 delete _post;
00042 }
00043
00044 void
00045 KalmanFilter::AllocateMeasModel(const vector<unsigned int>& meas_dimensions)
00046 {
00047 unsigned int meas_dimension;
00048 for(int i = 0 ; i< meas_dimensions.size(); i++)
00049 {
00050
00051 meas_dimension = meas_dimensions[i];
00052 _mapMeasUpdateVariables_it = _mapMeasUpdateVariables.find(meas_dimension);
00053 if( _mapMeasUpdateVariables_it == _mapMeasUpdateVariables.end())
00054 {
00055
00056 _mapMeasUpdateVariables_it = (_mapMeasUpdateVariables.insert
00057 (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
00058 }
00059 }
00060 }
00061
00062 void
00063 KalmanFilter::AllocateMeasModel(const unsigned int& meas_dimension)
00064 {
00065
00066 _mapMeasUpdateVariables_it = _mapMeasUpdateVariables.find(meas_dimension);
00067 if( _mapMeasUpdateVariables_it == _mapMeasUpdateVariables.end())
00068 {
00069
00070 _mapMeasUpdateVariables_it = (_mapMeasUpdateVariables.insert
00071 (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
00072 }
00073 }
00074
00075 void
00076 KalmanFilter::CalculateSysUpdate(const ColumnVector& J, const Matrix& F, const SymmetricMatrix& Q)
00077 {
00078 _Sigma_temp = F * ( (Matrix)_post->CovarianceGet() * F.transpose());
00079 _Sigma_temp += (Matrix)Q;
00080 _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
00081
00082
00083 PostMuSet ( J );
00084 PostSigmaSet( _Sigma_new );
00085 }
00086
00087 void
00088 KalmanFilter::CalculateMeasUpdate(const ColumnVector& z, const ColumnVector& Z, const Matrix& H, const SymmetricMatrix& R)
00089 {
00090
00091 AllocateMeasModel(z.rows());
00092
00093 (_mapMeasUpdateVariables_it->second)._postHT = (Matrix)(_post->CovarianceGet()) * H.transpose() ;
00094 (_mapMeasUpdateVariables_it->second)._S_Matrix = H * (_mapMeasUpdateVariables_it->second)._postHT;
00095 (_mapMeasUpdateVariables_it->second)._S_Matrix += (Matrix)R;
00096
00097
00098 (_mapMeasUpdateVariables_it->second)._K = (_mapMeasUpdateVariables_it->second)._postHT * ( (_mapMeasUpdateVariables_it->second)._S_Matrix.inverse());
00099
00100
00101
00102 (_mapMeasUpdateVariables_it->second)._innov = z-Z;
00103 _Mu_new = (_mapMeasUpdateVariables_it->second)._K * (_mapMeasUpdateVariables_it->second)._innov ;
00104 _Mu_new += _post->ExpectedValueGet() ;
00105
00106 _Sigma_temp = (_post->CovarianceGet());
00107 _Sigma_temp_par = (_mapMeasUpdateVariables_it->second)._K * H ;
00108 _Sigma_temp -= _Sigma_temp_par * (Matrix)(_post->CovarianceGet());
00109
00110 _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
00111
00112
00113 PostMuSet ( _Mu_new );
00114 PostSigmaSet( _Sigma_new );
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127 }
00128
00129 bool
00130 KalmanFilter::UpdateInternal(SystemModel<ColumnVector>* const sysmodel,
00131 const ColumnVector& u,
00132 MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00133 const ColumnVector& z, const ColumnVector& s)
00134 {
00135 if (sysmodel != NULL)
00136 {
00137 SysUpdate(sysmodel,u);
00138 }
00139 if (measmodel != NULL)
00140 {
00141 MeasUpdate(measmodel,z,s);
00142 }
00143 return true;
00144 }
00145
00146 void
00147 KalmanFilter::PostSigmaSet( const SymmetricMatrix& s)
00148 {
00149 dynamic_cast<Gaussian *>(_post)->CovarianceSet(s);
00150 }
00151
00152 void
00153 KalmanFilter::PostMuSet( const ColumnVector& c)
00154 {
00155 dynamic_cast<Gaussian *>(_post)->ExpectedValueSet(c);
00156 }
00157
00158
00159 Gaussian*
00160 KalmanFilter::PostGet()
00161 {
00162 return (Gaussian*)Filter<ColumnVector,ColumnVector>::PostGet();
00163 }
00164
00165 }
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