Bayesian Filtering Library
Generated from SVN r
src
filter
extendedkalmanfilter.h
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., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 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
43
class
ExtendedKalmanFilter
:
public
KalmanFilter
44
{
45
public
:
50
ExtendedKalmanFilter
(
Gaussian
* prior);
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
// Get NIS value
70
double
NisGet(
MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
*
const
measmodel,
const
MatrixWrapper::ColumnVector
& z);
71
72
private
:
73
struct
MeasUpdateVariablesExt
74
{
75
SymmetricMatrix _R;
76
Matrix _H;
77
ColumnVector _Z;
78
MeasUpdateVariablesExt() {};
79
MeasUpdateVariablesExt(
unsigned
int
meas_dimension,
unsigned
int
state_dimension):
80
_R(meas_dimension)
81
, _H(meas_dimension,state_dimension)
82
, _Z(meas_dimension)
83
{};
84
};
//struct
85
86
protected
:
87
virtual
void
SysUpdate
(
SystemModel<MatrixWrapper::ColumnVector>
*
const
sysmodel,
88
const
MatrixWrapper::ColumnVector
& u);
89
virtual
void
MeasUpdate
(
MeasurementModel<MatrixWrapper::ColumnVector,MatrixWrapper::ColumnVector>
*
const
measmodel,
90
const
MatrixWrapper::ColumnVector
& z,
91
const
MatrixWrapper::ColumnVector
& s);
92
// variables to avoid allocation on the heap
93
ColumnVector _x;
94
ColumnVector _J;
95
Matrix _F;
96
SymmetricMatrix _Q;
97
std::map<unsigned int, MeasUpdateVariablesExt> _mapMeasUpdateVariablesExt;
98
std::map<unsigned int, MeasUpdateVariablesExt>::iterator _mapMeasUpdateVariablesExt_it;
99
100
101
};
// class
102
103
}
// End namespace BFL
104
105
#endif // __EXTENDED_KALMAN_FILTER__
BFL::ExtendedKalmanFilter
Definition:
extendedkalmanfilter.h:43
BFL::Gaussian
Class representing Gaussian (or normal density)
Definition:
gaussian.h:27
BFL::ExtendedKalmanFilter::SysUpdate
virtual void SysUpdate(SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
System Update.
MatrixWrapper::ColumnVector
Wrapper class for ColumnVectors (Boost implementation)
Definition:
vector_BOOST.h:41
BFL::ExtendedKalmanFilter::ExtendedKalmanFilter
ExtendedKalmanFilter(Gaussian *prior)
BFL::MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector >
BFL::ExtendedKalmanFilter::MeasUpdate
virtual void MeasUpdate(MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
Measurement Update (overloaded)
BFL::SystemModel< MatrixWrapper::ColumnVector >
BFL::ExtendedKalmanFilter::AllocateMeasModelExt
void AllocateMeasModelExt(const vector< unsigned int > &meas_dimensions)
Function to allocate memory needed during the measurement update,.
BFL::ExtendedKalmanFilter::~ExtendedKalmanFilter
virtual ~ExtendedKalmanFilter()
Destructor.
BFL::KalmanFilter
Class representing the family of all Kalman Filters (EKF, IEKF, ...)
Definition:
kalmanfilter.h:49
Generated by
1.8.17