rauchtungstriebel.cpp
Go to the documentation of this file.
1 // $Id: rauchtungstriebel.cpp 6736 2006-12-22 11:24:42Z tdelaet $
2 // Copyright (C) 2006 Tinne De Laet <first dot last at mech dot kuleuven dot be>
3 //
4 // This program is free software; you can redistribute it and/or modify
5 // it under the terms of the GNU Lesser General Public License as published by
6 // the Free Software Foundation; either version 2.1 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU Lesser General Public License for more details.
13 //
14 // You should have received a copy of the GNU Lesser General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
17 //
18 #include "rauchtungstriebel.h"
19 #include <cmath>
20 
21 namespace BFL
22 {
23  using namespace MatrixWrapper;
24 
25 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
26 
28  : BackwardFilter<ColumnVector>(prior)
29  , _x(prior->DimensionGet())
30  , _xf(prior->DimensionGet())
31  , _xpred(prior->DimensionGet())
32  , _xsmooth(prior->DimensionGet())
33  , _F(prior->DimensionGet(),prior->DimensionGet())
34  , _Ppred(prior->DimensionGet(),prior->DimensionGet())
35  , _Pxx(prior->DimensionGet(),prior->DimensionGet())
36  , _K(prior->DimensionGet(),prior->DimensionGet())
37  , _Psmooth(prior->DimensionGet(),prior->DimensionGet())
38  , _Q(prior->DimensionGet())
39  , _Sigma_new(prior->DimensionGet())
40  {
41  // create posterior dencity
42  _post = new Gaussian(*prior);
43  }
44 
46  {
47  delete _post;
48  }
49 
50  void
51  RauchTungStriebel::SysUpdate(SystemModel<ColumnVector>* const sysmodel, const ColumnVector& u, Pdf<ColumnVector>* const filtered_post)
52  {
54  _xf = filtered_post->ExpectedValueGet();
55  _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
56  _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
57 
58  _Ppred = _F * (Matrix)filtered_post->CovarianceGet() * _F.transpose() + (Matrix)_Q;
59  _Pxx = (Matrix)filtered_post->CovarianceGet() * _F.transpose();
60  _K = _Pxx * _Ppred.inverse();
61  _xpred = ((AnalyticSys*)sysmodel)->PredictionGet(u,_xf);
62  _xsmooth = _xf + _K * (_x - _xpred);
63 
64  _Psmooth = (Matrix)filtered_post->CovarianceGet() - _K * ( _Ppred - (Matrix)_post->CovarianceGet() ) * _K.transpose();
65  _Psmooth.convertToSymmetricMatrix(_Sigma_new);
66 
67  // set new state gaussian
68  PostMuSet ( _xsmooth );
70  }
71 
72  bool
73  RauchTungStriebel::UpdateInternal(SystemModel<ColumnVector>* const sysmodel, const ColumnVector& u,Pdf<ColumnVector>* const filtered_post)
74  {
75  SysUpdate(sysmodel,u,(Gaussian*)filtered_post);
76  return true;
77  }
78 
79  void
80  RauchTungStriebel::PostSigmaSet( const SymmetricMatrix& s)
81  {
82  dynamic_cast<Gaussian *>(_post)->CovarianceSet(s);
83  }
84 
85  void
86  RauchTungStriebel::PostMuSet( const ColumnVector& c)
87  {
88  dynamic_cast<Gaussian *>(_post)->ExpectedValueSet(c);
89  }
90 
91 }
RauchTungStriebel(Gaussian *prior)
Constructor.
#define AnalyticSys
Class PDF: Virtual Base class representing Probability Density Functions.
Definition: pdf.h:53
SymmetricMatrix _Sigma_new
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
Virtual Baseclass representing all bayesian backward filters.
virtual bool UpdateInternal(SystemModel< ColumnVector > *const sysmodel, const ColumnVector &u, Pdf< ColumnVector > *const filtered_post)
void PostMuSet(const MatrixWrapper::ColumnVector &c)
Set expected value of posterior estimate.
void PostSigmaSet(const MatrixWrapper::SymmetricMatrix &s)
Set covariance of posterior estimate.
virtual ~RauchTungStriebel()
Destructor.
virtual T ExpectedValueGet() const
Get the expected value E[x] of the pdf.
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, Pdf< ColumnVector > *const filtered_post)
System Update.
Pdf< MatrixWrapper::ColumnVector > * _post
Pointer to the Posterior Pdf.
virtual MatrixWrapper::SymmetricMatrix CovarianceGet() const
Get the Covariance Matrix E[(x - E[x])^2] of the Analytic pdf.
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