nonminimalkalmanfilter.cpp
Go to the documentation of this file.
00001 // $Id: nonminimalkalmanfilter.cpp 29495 2008-08-13 12:57:49Z 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 ac dot be>
00004 //
00005 // This program is free software; you can redistribute it and/or modify
00006 // it under the terms of the GNU Lesser General Public License as published by
00007 // the Free Software Foundation; either version 2.1 of the License, or
00008 // (at your option) any later version.
00009 //
00010 // This program is distributed in the hope that it will be useful,
00011 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00012 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013 // GNU Lesser General Public License for more details.
00014 //
00015 // You should have received a copy of the GNU Lesser General Public License
00016 // along with this program; if not, write to the Free Software
00017 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
00018 //
00019 #include "nonminimalkalmanfilter.h"
00020 #include "nonminimal_state/linearise.h"
00021 #include "../pdf/nonlinearanalyticconditionalgaussian.h"
00022 
00023 namespace BFL
00024 {
00025 
00026 #define NLinSys    NonLinearAnalyticSystemModelGaussianUncertainty
00027 #define NLinMeas   NonLinearAnalyticMeasurementModelGaussianUncertainty
00028 
00029 
00030   NonminimalKalmanFilter::NonminimalKalmanFilter(Gaussian* prior,
00031                                                  unsigned int NrIterations,
00032                                                  vector<NLSysModel*>   minimalsysmodels,
00033                                                  vector<NLMeasModel*>  minimalmeasmodels,
00034                                                  vector<GiNaC::symbol> makelinear)
00035     : KalmanFilter(prior)
00036   {
00037     // create linearise
00038     Linear = new Linearise(minimalsysmodels, minimalmeasmodels, makelinear);
00039 
00040     // create STATE
00041     MinimalState    = Linear->NonlinearStateGet();
00042     NonminimalState = Linear->LinearStateGet();
00043 
00044     // create PRIOR
00045     ColumnVector mu_prior(NonminimalState.size());  mu_prior = 0;
00046     SymmetricMatrix sigma_prior(NonminimalState.size());  sigma_prior = 0;
00047     for (unsigned int i=0; i< NonminimalState.size(); i++)
00048       sigma_prior(i+1,i+1) = 333*333;
00049     NonminimalPrior = new Gaussian(mu_prior,sigma_prior);
00050     MinimalPrior    = prior;
00051     cout << "nonminimal prior " << *NonminimalPrior << endl;
00052 
00053     // create FILTER
00054     NonminimalFilter = new IteratedExtendedKalmanFilter(NonminimalPrior, NrIterations);
00055     MinimalFilter    = new IteratedExtendedKalmanFilter(MinimalPrior,    NrIterations);
00056 
00057 
00058     // create MODEL
00059     vector<GiNaC::symbol> empty_sym(0);
00060     ColumnVector mu_add(NonminimalState.size()); mu_add = 0;
00061     Gaussian additiveNoise(mu_add,NonminimalFilter->PostGet().CovarianceGet());
00062     NonLinearConditionalGaussian pdf(Linear->SubstitutionGet(), empty_sym, MinimalState, additiveNoise);
00063     MinimalMeasModel = new NLinMeas( &pdf );
00064 
00065     // show substitutions
00066     for (unsigned int i=0; i<NonminimalState.size() ; i++)
00067       cout << NonminimalState[i] << " -> " << Linear->SubstitutionGet()[i] << endl;
00068   }
00069 
00070 
00071   NonminimalKalmanFilter::~NonminimalKalmanFilter()
00072   {
00073     delete Linear;
00074     delete NonminimalPrior;
00075     delete NonminimalFilter;
00076     delete MinimalFilter;
00077     delete MinimalMeasModel;
00078   }
00079 
00080   void
00081   NonminimalKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const sysmodel,
00082                                     const ColumnVector& u)
00083   {
00084     NonminimalFilter->SysUpdate(Linear->LinearSysModelGet((NLinSys*)sysmodel),u);
00085   }
00086 
00087   void
00088   NonminimalKalmanFilter::MeasUpdate(MeasurementModel<ColumnVector,ColumnVector>* const measmodel,
00089                                      const ColumnVector& z,
00090                                      const ColumnVector& s)
00091   {
00092     NonminimalFilter->MeasUpdate(Linear->LinearMeasModelGet((NLinMeas*)measmodel),z,s);
00093   }
00094 
00095 }// End namespace


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