rauchtungstriebel.cpp
Go to the documentation of this file.
00001 // $Id: rauchtungstriebel.cpp 6736 2006-12-22 11:24:42Z tdelaet $
00002 // Copyright (C) 2006  Tinne De Laet <first dot last at mech dot kuleuven dot be>
00003 //
00004 // This program is free software; you can redistribute it and/or modify
00005 // it under the terms of the GNU Lesser General Public License as published by
00006 // the Free Software Foundation; either version 2.1 of the License, or
00007 // (at your option) any later version.
00008 //
00009 // This program is distributed in the hope that it will be useful,
00010 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00011 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012 // GNU Lesser General Public License for more details.
00013 //
00014 // You should have received a copy of the GNU Lesser General Public License
00015 // along with this program; if not, write to the Free Software
00016 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
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     // create posterior dencity
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     // set new state gaussian
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 Thu Feb 11 2016 22:31:57