extendedkalmanfilter.cpp
Go to the documentation of this file.
00001 // $Id: extendedkalmanfilter.cpp 29762 2008-12-02 08:37:11Z tdelaet $
00002 // Copyright (C) 2003 Klaas Gadeyne <first dot last at gmail dot com>
00003 //                    Wim Meeussen  <wim dot meeussen at mech dot kuleuven dot be>
00004 //                    Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
00005 //
00006 // This program is free software; you can redistribute it and/or modify
00007 // it under the terms of the GNU Lesser General Public License as published by
00008 // the Free Software Foundation; either version 2.1 of the License, or
00009 // (at your option) any later version.
00010 //
00011 // This program is distributed in the hope that it will be useful,
00012 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00013 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014 // GNU Lesser General Public License for more details.
00015 //
00016 // You should have received a copy of the GNU Lesser General Public License
00017 // along with this program; if not, write to the Free Software
00018 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00019 //
00020 #include "extendedkalmanfilter.h"
00021 
00022 namespace BFL
00023 {
00024   using namespace MatrixWrapper;
00025 
00026 
00027 
00028 #define AnalyticSys    AnalyticSystemModelGaussianUncertainty
00029 #define AnalyticMeas   AnalyticMeasurementModelGaussianUncertainty
00030 
00031 
00032 ExtendedKalmanFilter::ExtendedKalmanFilter(Gaussian* prior)
00033 : KalmanFilter(prior)
00034 , _x(prior->DimensionGet())
00035 , _J(prior->DimensionGet())
00036 , _F(prior->DimensionGet(),prior->DimensionGet())
00037 , _Q(prior->DimensionGet())
00038 {}
00039 
00040 ExtendedKalmanFilter::~ExtendedKalmanFilter()
00041 {}
00042 
00043 void
00044 ExtendedKalmanFilter::AllocateMeasModelExt(const vector<unsigned int>& meas_dimensions)
00045 {
00046   unsigned int meas_dimension;
00047   for(int i = 0 ; i< meas_dimensions.size(); i++)
00048   {
00049       // find if variables with size meas_sizes[i] are already allocated
00050       meas_dimension = meas_dimensions[i];
00051       _mapMeasUpdateVariablesExt_it =  _mapMeasUpdateVariablesExt.find(meas_dimension);
00052       if( _mapMeasUpdateVariablesExt_it == _mapMeasUpdateVariablesExt.end())
00053       {
00054           //variables with size z.rows() not allocated yet
00055           _mapMeasUpdateVariablesExt_it = (_mapMeasUpdateVariablesExt.insert
00056               (std::pair<unsigned int, MeasUpdateVariablesExt>( meas_dimension,MeasUpdateVariablesExt(meas_dimension,_x.rows()) ))).first;
00057        }
00058    }
00059 }
00060 
00061 void
00062 ExtendedKalmanFilter::AllocateMeasModelExt(const unsigned int& meas_dimension)
00063 {
00064    // find if variables with size meas_sizes[i] are already allocated
00065    _mapMeasUpdateVariablesExt_it =  _mapMeasUpdateVariablesExt.find(meas_dimension);
00066    if( _mapMeasUpdateVariablesExt_it == _mapMeasUpdateVariablesExt.end())
00067    {
00068        //variables with size z.rows() not allocated yet
00069        _mapMeasUpdateVariablesExt_it = (_mapMeasUpdateVariablesExt.insert
00070            (std::pair<unsigned int, MeasUpdateVariablesExt>( meas_dimension,MeasUpdateVariablesExt(meas_dimension,_x.rows()) ))).first;
00071     }
00072 }
00073 
00074 void
00075 ExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00076                                 const ColumnVector& u)
00077 {
00078   _x = _post->ExpectedValueGet();
00079   _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
00080   _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
00081   _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
00082 
00083   CalculateSysUpdate(_J, _F, _Q);
00084 }
00085 
00086 void
00087 ExtendedKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00088                                  const ColumnVector& z,
00089                                  const ColumnVector& s)
00090 {
00091   // allocate measurement for z.rows() if needed
00092   AllocateMeasModelExt(z.rows());
00093 
00094   _x = _post->ExpectedValueGet();
00095   (_mapMeasUpdateVariablesExt_it->second)._Z = ((AnalyticMeas*)measmodel)->PredictionGet(s,_x);
00096   (_mapMeasUpdateVariablesExt_it->second)._H = ((AnalyticMeas*)measmodel)->df_dxGet(s,_x);
00097   (_mapMeasUpdateVariablesExt_it->second)._R = ((AnalyticMeas*)measmodel)->CovarianceGet(s,_x);
00098 
00099   CalculateMeasUpdate(z, (_mapMeasUpdateVariablesExt_it->second)._Z, (_mapMeasUpdateVariablesExt_it->second)._H, (_mapMeasUpdateVariablesExt_it->second)._R);
00100 }
00101 
00102 } // end namespace BFL


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:56