kalmanfilter.cpp
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2003 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., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
19 //
20 #include "kalmanfilter.h"
21 #include <cmath>
22 
23 namespace BFL
24 {
25  using namespace MatrixWrapper;
26 
27 
29  : Filter<ColumnVector,ColumnVector>(prior)
30  , _Mu_new(prior->DimensionGet())
31  , _Sigma_new(prior->DimensionGet())
32  , _Sigma_temp(prior->DimensionGet(),prior->DimensionGet())
33  , _Sigma_temp_par(prior->DimensionGet(),prior->DimensionGet())
34  {
35  // create posterior dencity
36  _post = new Gaussian(*prior);
37  }
38 
40  {
41  delete _post;
42  }
43 
44  void
45  KalmanFilter::AllocateMeasModel(const vector<unsigned int>& meas_dimensions)
46  {
47  unsigned int meas_dimension;
48  for(int i = 0 ; i< meas_dimensions.size(); i++)
49  {
50  // find if variables with size meas_sizes[i] are already allocated
51  meas_dimension = meas_dimensions[i];
54  {
55  //variables with size z.rows() not allocated yet
57  (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
58  }
59  }
60  }
61 
62  void
63  KalmanFilter::AllocateMeasModel(const unsigned int& meas_dimension)
64  {
65  // find if variables with size meas_sizes[i] are already allocated
68  {
69  //variables with size z.rows() not allocated yet
71  (std::pair<unsigned int, MeasUpdateVariables>( meas_dimension,MeasUpdateVariables(meas_dimension,_Mu_new.rows()) ))).first;
72  }
73  }
74 
75  void
76  KalmanFilter::CalculateSysUpdate(const ColumnVector& J, const Matrix& F, const SymmetricMatrix& Q)
77  {
78  _Sigma_temp = F * ( (Matrix)_post->CovarianceGet() * F.transpose());
79  _Sigma_temp += (Matrix)Q;
80  _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
81 
82  // set new state gaussian
83  PostMuSet ( J );
85  }
86 
87  void
88  KalmanFilter::CalculateMeasUpdate(const ColumnVector& z, const ColumnVector& Z, const Matrix& H, const SymmetricMatrix& R)
89  {
90  // allocate measurement for z.rows() if needed
91  AllocateMeasModel(z.rows());
92 
93  (_mapMeasUpdateVariables_it->second)._postHT = (Matrix)(_post->CovarianceGet()) * H.transpose() ;
94  (_mapMeasUpdateVariables_it->second)._S_Matrix = H * (_mapMeasUpdateVariables_it->second)._postHT;
95  (_mapMeasUpdateVariables_it->second)._S_Matrix += (Matrix)R;
96 
97  // _K = covariance * H' * S(-1)
98  (_mapMeasUpdateVariables_it->second)._K = (_mapMeasUpdateVariables_it->second)._postHT * ( (_mapMeasUpdateVariables_it->second)._S_Matrix.inverse());
99 
100  // calcutate new state gaussian
101  // Mu = expectedValue + K*(z-Z)
102  (_mapMeasUpdateVariables_it->second)._innov = z-Z;
103  _Mu_new = (_mapMeasUpdateVariables_it->second)._K * (_mapMeasUpdateVariables_it->second)._innov ;
105  // Sigma = post - K*H*post
109  // convert to symmetric matrix
110  _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);
111 
112  // set new state gaussian
113  PostMuSet ( _Mu_new );
115 
116  /*
117  cout << "H:\n" << H << endl;
118  cout << "R:\n" << R << endl;
119  cout << "Z:\n" << Z << endl;
120  cout << "inov:\n" << z-Z << endl;
121  cout << "S:\n" << S << endl;
122  cout << "S.inverse:\n" << S.inverse() << endl;
123  cout << "K:\n" << K << endl;
124  cout << "Mu_new:\n" << Mu_new << endl;
125  cout << "sigma_new\n" << Sigma_new << endl;
126  */
127  }
128 
129  bool
131  const ColumnVector& u,
133  const ColumnVector& z, const ColumnVector& s)
134  {
135  if (sysmodel != NULL)
136  {
137  SysUpdate(sysmodel,u);
138  }
139  if (measmodel != NULL)
140  {
141  MeasUpdate(measmodel,z,s);
142  }
143  return true;
144  }
145 
146  void
147  KalmanFilter::PostSigmaSet( const SymmetricMatrix& s)
148  {
149  dynamic_cast<Gaussian *>(_post)->CovarianceSet(s);
150  }
151 
152  void
153  KalmanFilter::PostMuSet( const ColumnVector& c)
154  {
155  dynamic_cast<Gaussian *>(_post)->ExpectedValueSet(c);
156  }
157 
158 
159  Gaussian*
161  {
163  }
164 
165 }
Abstract class representing an interface for Bayesian Filters.
Definition: filter.h:77
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
Matrix _Sigma_temp_par
Definition: kalmanfilter.h:99
virtual ~KalmanFilter()
Destructor.
std::map< unsigned int, MeasUpdateVariables >::iterator _mapMeasUpdateVariables_it
Definition: kalmanfilter.h:103
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
Pdf< MatrixWrapper::ColumnVector > * _post
Pointer to the Posterior Pdf.
Definition: filter.h:95
KalmanFilter(Gaussian *prior)
Constructor.
virtual Pdf< StateVar > * PostGet()
Get Posterior density.
Definition: filter.cpp:126
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 Gaussian * PostGet()
Get Posterior density.
void CalculateSysUpdate(const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
std::map< unsigned int, MeasUpdateVariables > _mapMeasUpdateVariables
Definition: kalmanfilter.h:102
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)=0
System Update.
ColumnVector _Mu_new
Definition: kalmanfilter.h:96
void CalculateMeasUpdate(const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R)
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)=0
Measurement Update (overloaded)
void AllocateMeasModel(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
SymmetricMatrix _Sigma_new
Definition: kalmanfilter.h:97
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.


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 Mon Feb 28 2022 21:56:33