extendedkalmanfilter.h
Go to the documentation of this file.
1 // $Id$
2 // Copyright (C) 2002 Klaas Gadeyne <first dot last at gmail dot com>
3 // Wim Meeussen <wim dot meeussen at mech dot kuleuven dot be>
4 // Tinne De Laet <tinne dot delaet at mech dot kuleuven dot be>
5 //
6 // This program is free software; you can redistribute it and/or modify
7 // it under the terms of the GNU Lesser General Public License as published by
8 // the Free Software Foundation; either version 2.1 of the License, or
9 // (at your option) any later version.
10 //
11 // This program is distributed in the hope that it will be useful,
12 // but WITHOUT ANY WARRANTY; without even the implied warranty of
13 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 // GNU Lesser General Public License for more details.
15 //
16 // You should have received a copy of the GNU Lesser General Public License
17 // along with this program; if not, write to the Free Software
18 // Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
19 //
20 
21 #ifndef __EXTENDED_KALMAN_FILTER__
22 #define __EXTENDED_KALMAN_FILTER__
23 
24 #include "kalmanfilter.h"
25 #include "../pdf/conditionalpdf.h"
26 #include "../pdf/gaussian.h"
27 # include <map>
28 
29 namespace BFL
30 {
31 
44 {
45 public:
51 
53  virtual ~ExtendedKalmanFilter();
54 
56  // For realtime use, this function should be called before calling measUpdate
57  /* @param vector containing the dimension of the measurement models which are
58  going to be used
59  */
60  void AllocateMeasModelExt( const vector<unsigned int>& meas_dimensions);
61 
63  // For realtime use, this function should be called before calling measUpdate
64  /* @param dimension of the measurement models which is
65  going to be used
66  */
67  void AllocateMeasModelExt( const unsigned int& meas_dimensions);
68 
69 private:
71  {
72  SymmetricMatrix _R;
73  Matrix _H;
74  ColumnVector _Z;
76  MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension):
77  _R(meas_dimension)
78  , _H(meas_dimension,state_dimension)
79  , _Z(meas_dimension)
80 {};
81  }; //struct
82 
83  protected:
84  virtual void SysUpdate(SystemModel<MatrixWrapper::ColumnVector>* const sysmodel,
85  const MatrixWrapper::ColumnVector& u);
87  const MatrixWrapper::ColumnVector& z,
88  const MatrixWrapper::ColumnVector& s);
89  // variables to avoid allocation on the heap
90  ColumnVector _x;
91  ColumnVector _J;
92  Matrix _F;
93  SymmetricMatrix _Q;
94  std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
95  std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
96 
97 
98 }; // class
99 
100 } // End namespace BFL
101 
102 #endif // __EXTENDED_KALMAN_FILTER__
MeasUpdateVariablesExt(unsigned int meas_dimension, unsigned int state_dimension)
void AllocateMeasModelExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
Class representing Gaussian (or normal density)
Definition: gaussian.h:27
std::map< unsigned int, MeasUpdateVariablesExt >::iterator _mapMeasUpdateVariablesExt_it
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
virtual ~ExtendedKalmanFilter()
Destructor.
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition: kalmanfilter.h:49
std::map< unsigned int, MeasUpdateVariablesExt > _mapMeasUpdateVariablesExt


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