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 #include "rauchtungstriebel.h"
00019 #include <cmath>
00020
00021 namespace BFL
00022 {
00023 using namespace MatrixWrapper;
00024
00025 #define AnalyticSys AnalyticSystemModelGaussianUncertainty
00026
00027 RauchTungStriebel::RauchTungStriebel(Gaussian * prior)
00028 : BackwardFilter<ColumnVector>(prior)
00029 , _x(prior->DimensionGet())
00030 , _xf(prior->DimensionGet())
00031 , _xpred(prior->DimensionGet())
00032 , _xsmooth(prior->DimensionGet())
00033 , _F(prior->DimensionGet(),prior->DimensionGet())
00034 , _Ppred(prior->DimensionGet(),prior->DimensionGet())
00035 , _Pxx(prior->DimensionGet(),prior->DimensionGet())
00036 , _K(prior->DimensionGet(),prior->DimensionGet())
00037 , _Psmooth(prior->DimensionGet(),prior->DimensionGet())
00038 , _Q(prior->DimensionGet())
00039 , _Sigma_new(prior->DimensionGet())
00040 {
00041
00042 _post = new Gaussian(*prior);
00043 }
00044
00045 RauchTungStriebel::~RauchTungStriebel()
00046 {
00047 delete _post;
00048 }
00049
00050 void
00051 RauchTungStriebel::SysUpdate(SystemModel<ColumnVector>* const sysmodel, const ColumnVector& u, Pdf<ColumnVector>* const filtered_post)
00052 {
00053 _x = _post->ExpectedValueGet();
00054 _xf = filtered_post->ExpectedValueGet();
00055 _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
00056 _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
00057
00058 _Ppred = _F * (Matrix)filtered_post->CovarianceGet() * _F.transpose() + (Matrix)_Q;
00059 _Pxx = (Matrix)filtered_post->CovarianceGet() * _F.transpose();
00060 _K = _Pxx * _Ppred.inverse();
00061 _xpred = ((AnalyticSys*)sysmodel)->PredictionGet(u,_xf);
00062 _xsmooth = _xf + _K * (_x - _xpred);
00063
00064 _Psmooth = (Matrix)filtered_post->CovarianceGet() - _K * ( _Ppred - (Matrix)_post->CovarianceGet() ) * _K.transpose();
00065 _Psmooth.convertToSymmetricMatrix(_Sigma_new);
00066
00067
00068 PostMuSet ( _xsmooth );
00069 PostSigmaSet( _Sigma_new );
00070 }
00071
00072 bool
00073 RauchTungStriebel::UpdateInternal(SystemModel<ColumnVector>* const sysmodel, const ColumnVector& u,Pdf<ColumnVector>* const filtered_post)
00074 {
00075 SysUpdate(sysmodel,u,(Gaussian*)filtered_post);
00076 return true;
00077 }
00078
00079 void
00080 RauchTungStriebel::PostSigmaSet( const SymmetricMatrix& s)
00081 {
00082 dynamic_cast<Gaussian *>(_post)->CovarianceSet(s);
00083 }
00084
00085 void
00086 RauchTungStriebel::PostMuSet( const ColumnVector& c)
00087 {
00088 dynamic_cast<Gaussian *>(_post)->ExpectedValueSet(c);
00089 }
00090
00091 }
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