Kalman::EKFilter

Kalman::EKFilter< T, BEG, OQ, OVR, DBG > Class Template Reference

Generic Extended Kalman Filter (EKF) template base class. More...

#include <ekfilter.hpp>

Inheritance diagram for Kalman::EKFilter< T, BEG, OQ, OVR, DBG >:

Kalman::KFilter< T, BEG, OQ, OVR, DBG > Collaboration diagram for Kalman::EKFilter< T, BEG, OQ, OVR, DBG >:

Collaboration graph
[legend]
List of all members.

Public Types

typedef T type
 Type of objects contained in matrices and vectors.
typedef KVector< T, BEG, DBG > Vector
 Vector type.
typedef KMatrix< T, BEG, DBG > Matrix
 Matrix type.
enum  { beg = BEG }

Public Member Functions

void init (Vector &x_, Matrix &P_)
 Sets initial conditions for the Kalman Filter.
Constructor and Destructor.
 EKFilter ()
 Default constructor.
 EKFilter (K_UINT_32 n_, K_UINT_32 nu_, K_UINT_32 nw_, K_UINT_32 m_, K_UINT_32 nv_)
 Constructors specifying all necessary matrix and vector dimensions.
virtual ~EKFilter ()
 Virtual destructor.
Dimension Accessor Functions
K_UINT_32 getSizeX () const
 Returns the size of the state vector.
K_UINT_32 getSizeU () const
 Returns the size of the input vector.
K_UINT_32 getSizeW () const
 Returns the size of the process noise vector.
K_UINT_32 getSizeZ () const
 Returns the size of the measurement vector.
K_UINT_32 getSizeV () const
 Returns the size of the measurement noise vector.
Resizing Functions
These functions allow to change the dimensions of all matrices and vectors, thus implementing a Variable-Dimension Extended Kalman Filter. They do nothing if the new size is the same as the old one.
Warning:
setDim() (or the five setSize functions) must be called before any other function, or else, matrices and vectors will not have their memory allocated.


void setDim (K_UINT_32 n_, K_UINT_32 nu_, K_UINT_32 nw_, K_UINT_32 m_, K_UINT_32 nv_)
 Sets all dimensions at once.
void setSizeX (K_UINT_32 n_)
 Sets the size of the state vector.
void setSizeU (K_UINT_32 nu_)
 Sets the size of the input vector.
void setSizeW (K_UINT_32 nw_)
 Sets the size of the process noise vector.
void setSizeZ (K_UINT_32 m_)
 Sets the size of the measurement vector.
void setSizeV (K_UINT_32 nv_)
 Sets the size of the measurement noise vector.
Kalman Filter Functions
These functions allow to get the results from the Kalman filtering algorithm. Before any of these can be called, all dimensions must have been set properly at least once and init() must have been called, also at least once. Each time the user want to resize some vectors, the corresponding resizing functions must be called again before being able to call one of the functions in this section. init() must also be called again if n or nw has changed. init() can also be called solely to reset the filter.

void step (Vector &u_, const Vector &z_)
 Makes one prediction-correction step.
void timeUpdateStep (Vector &u_)
 Makes one prediction step.
void measureUpdateStep (const Vector &z_)
 Makes one correction step.
const Vector & predict (Vector &u_)
 Returns the predicted state vector (a priori state estimate).
const Vector & simulate ()
 Returns the predicted measurement vector.
const Vector & getX () const
 Returns the corrected state (a posteriori state estimate).
const Matrix & calculateP () const
 Returns the a posteriori error covariance estimate matrix.

Protected Member Functions

void NoModification ()
 Allows optimizations on some calculations.
virtual void sizeUpdate ()
 Resizes all vector and matrices. Never call or overload this !
Matrix Pre-Creators
Theses functions have been designed to be overridden by derived classes if necessary. Their role is to fill in the parts of the Kalman matrices that don't change between iterations. That is to say, these functions should only set constant values inside matrices that don't depend on x or u.

They will all be called at least once, before the calls to their corresponding matrix (not pre-) creators. In fact, they are called once per resize (not necessarily at the moment of the resize though), including while the matrices are first allocated.

Note:
Matrices have already been properly resized before these functions are called, so no further resizing is or should be necessary.

If a matrix pre-creator is overridden, but it does not modify in any way the matrix in certain execution paths, then the function NoModification() should be called in each of those execution paths so that the filter can optimize away some calculations. The default versions of the matrix pre-creators only call NoModification() in their bodies.

Warning:
Each matrix pre-creator cannot suppose that any other matrix pre-creator will be called before or after it.


virtual void makeBaseA ()
 Virtual pre-creator of A.
virtual void makeBaseW ()
 Virtual pre-creator of W.
virtual void makeBaseQ ()
 Virtual pre-creator of Q.
virtual void makeBaseH ()
 Virtual pre-creator of H.
virtual void makeBaseV ()
 Virtual pre-creator of V.
virtual void makeBaseR ()
 Virtual pre-creator of R.
Matrix Creators
Theses functions have been designed to be overridden by derived classes if necessary. Their role is to fill in the parts of the Kalman matrices that change between iterations. That is to say, these functions should set values inside matrices that depend on x or u.

These functions can suppose that their corresponding matrix pre-creator has been called at least once before. Also, makeA(), makeW(), makeQ() and makeProcess() can suppose that makeCommonProcess() is called every time just before it being called. Same thing for makeH(), makeV(), makeR() and makeMeasure() about makeCommonMeasure().

Note:
Matrices have already been properly resized before these functions are called, so no further resizing is or should be necessary.

If a matrix creator is overridden, but it does not modify in any way the matrix in certain execution paths, then the function NoModification() should be called in each of those execution paths so that the filter can optimize away some calculations. The default versions of the matrix creators only call NoModification() in their bodies.

Warning:
Each matrix creator cannot suppose that any other matrix creator will be called before or after it. One thing is sure : makeCommon*() is called first, then some of make*() and finally, makeProcess() or makeMeasure().

These functions can access x and u in read-only mode, except makeProcess(), which must modify x.



virtual void makeCommonProcess ()
 Optional function used to precalculate common values for process.
virtual void makeA ()
 Virtual creator of A.
virtual void makeW ()
 Virtual creator of W.
virtual void makeQ ()
 Virtual creator of Q.
virtual void makeProcess ()=0
 Actual process $ f(x, u, 0) $ . Fills in new x by using old x.
virtual void makeCommonMeasure ()
 Optional function used to precalculate common values for measurement.
virtual void makeH ()
 Virtual creator of H.
virtual void makeV ()
 Virtual creator of V.
virtual void makeR ()
 Virtual creator of R.
virtual void makeMeasure ()=0
 Actual measurement function $ h(x, 0) $ . Fills in z.
virtual void makeDZ ()
 Hook-up function to modify innovation vector.

Protected Attributes

Kalman Vectors and Matrices
Vector x
 Corrected state vector.
Vector u
 Input vector.
Vector z
 Predicted measurement vector.
Vector dz
 Innovation vector.
Matrix A
 A jacobian matrix.
Matrix W
 A jacobian matrix.
Matrix Q
 Process noise covariance matrix.
Matrix H
 A jacobian matrix.
Matrix V
 A jacobian matrix.
Matrix R
 Measurement noise covariance matrix.
Kalman Dimensions
Warning:
These values, which are accessible to derived classes, are read-only. The derived classes should use the resizing functions to modify vector and matrix dimensions.


K_UINT_32 n
 Size of the state vector.
K_UINT_32 nu
 Size of the input vector.
K_UINT_32 nw
 Size of the process noise vector.
K_UINT_32 m
 Size of the measurement vector.
K_UINT_32 nv
 Size of the measurement noise vector.

Detailed Description

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
class Kalman::EKFilter< T, BEG, OQ, OVR, DBG >

Generic Extended Kalman Filter (EKF) template base class.

Usage
"The Kalman filter is a set of mathematical equations that provides an efficient computational (recursive) solution of the least-squares method. The filter is very powerful in several aspects: it supports estimations of past, present, and even future states, and it can do so even when the precise nature of the modeled system is unknown." (quoted from [02])
This version of the Kalman filter is in fact a Variable-Dimension Extended Kalman Filter (VDEKF). It supports optimized algorithms (translated from Fortran - see [01]), even in the presence of correlated process or measurement noise.
To use this template class, you must first inherit from it and implement some virtual functions. See the example page for more informations. Note that you can copy freely an EKFilter-derived class freely : this can be useful if you need to branch your filter based on some condition.
Notation
We prefered the notation of [02] : here it is. Assume a state vector $ x $ (to estimate) and a non-linear process function $ f $ (to model) that describes the evolution of this state through time, that is :

\[ x_k = f \left( x_{k-1}, u_{k-1}, w_{k-1} \right) \]

where $ u $ is the (known) input vector fed to the process and $ w $ is the (unknown) process noise vector due to uncertainty and process modeling errors. Further suppose that the (known) process noise covariance matrix is :

\[ Q = E \left( w w^T \right) \]

Now, let's assume a (known) measurement vector $ z $ , which depends on the current state $ x $ in the form of a non-linear function $ h $ (to model) :

\[ z_k = h \left( x_k, v_k \right) \]

where $ v $ is the (unknown) measurement noise vector with a (known) covariance matrix :

\[ R = E \left( v v^T \right) \]

Suppose that we have an estimate of the previous state $ \hat{x}_{k-1} $ , called a corrected state or an a posteriori state estimate. We can build a predicted state (also called an a priori state estimate) by using $ f $ :

\[ \tilde{x}_k = f \left( \hat{x}_{k-1}, u_{k-1}, 0 \right) \]

since the input is known and the process noise, unknown. With this predicted state, we can get a predicted measurement vector by using $ h $ :

\[ \tilde{z}_k = h \left( \tilde{x}_k, 0 \right) \]

since the measurement noise is unknown. To obtain a linear least-squares formulation, we need to linearize those two systems. Here are first-order Taylor series centered on $ \tilde{x}_k $ :

\[ x_k \approx f \left( \hat{x}_{k-1}, u_{k-1}, 0 \right) + \frac{\partial f}{\partial x} \left( \hat{x}_{k-1}, u_{k-1}, 0 \right) \left( \Delta x \right) + \frac{\partial f}{\partial u} \left( \hat{x}_{k-1}, u_{k-1}, 0 \right) \left( \Delta u \right) + \frac{\partial f}{\partial w} \left( \hat{x}_{k-1}, u_{k-1}, 0 \right) \left( \Delta w \right) \]

\[ \phantom{x_k} = \tilde{x}_k + A \left( x_{k-1} - \hat{x}_{k-1} \right) + W w_{k-1} \]

We can do the same for the other system :

\[ z_k \approx h \left( \tilde{x}_k, 0 \right) + \frac{\partial h}{\partial x} \left( \tilde{x}_k, 0 \right) \left( \Delta x \right) + \frac{\partial h}{\partial v} \left( \tilde{x}_k, 0 \right) \left( \Delta v \right) \]

\[ \phantom{z_k} = \tilde{z}_k + H \left( x_k - \tilde{x}_k \right) + V v_k \]

The user of this class must derive from it, and implement all the functions corresponding to A, W, Q, f, H, V, R and h.

References
[01] Bierman, G. J. "Factorization Methods for Discrete Sequential Estimation", Academic Press, 1977.
[02] Welch, G. and Bishop, G. "An Introduction to the %Kalman Filter", http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
Template parameters
Type requirements for T
This means that, if t1, t2 are instances of T, op is an arithmetic operator (+ - * /), is is of type istream and os is of type ostream, the following expressions must be valid : -
 T(); T t1;
Default constructor -
 T(0.0); T t1(1.0);
Constructor from double -
 T t1 = t2; T t1(t2); T(t1);
Copy constructor -
 t1 op t2
Arithmetic operation, convertible to T -
 -t1
Negation operator, convertible to T. Same as :
 T(0.0) - t1;
-
 t1 = t2;
Assignment operator -
 t1 op= t2;
Arithmetic inplace operation. Same as :
 t1 = t1 op t2;
-
 t1 == t2
Equality comparison, convertible to bool -
 is >> t1;
operator>>() -
 os << t1;
operator<<()
Finally, note that operator>>() and operator<<() must be compatible. Also, operator&() must not have been overloaded.

Member Enumeration Documentation

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
anonymous enum
 

Enumerator:
beg  Starting index of matrices and vectors.

Constructor & Destructor Documentation

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::EKFilter (  K_UINT_32  n_,
K_UINT_32  nu_,
K_UINT_32  nw_,
K_UINT_32  m_,
K_UINT_32  nv_
) 
 

Constructors specifying all necessary matrix and vector dimensions.

This constructor simply calls setDim() with all the corresponding arguments.

Member Function Documentation

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::setDim (  K_UINT_32  n_,
K_UINT_32  nu_,
K_UINT_32  nw_,
K_UINT_32  m_,
K_UINT_32  nv_
) 
 

Sets all dimensions at once.

This function simply calls the setSize*() functions for x, u, w, z, v with the corresponding arguments.

Warning:
This function (or the corresponding five setSize*() functions) must be called before any other functions.

init() must always be called after this function and before any other non-dimensioning function.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::setSizeX (  K_UINT_32  n_  ) 
 

Sets the size of the state vector.

Parameters:
n_ New state vector size. Must not be 0.
Warning:
init() must always be called after this function and before any other non-dimensioning function.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::setSizeW (  K_UINT_32  nw_  ) 
 

Sets the size of the process noise vector.

Parameters:
nw_ New process noise vector size.
Warning:
init() must always be called after this function and before any other non-dimensioning function.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::init (  Vector &  x_,
Matrix &  P_
) 
 

Sets initial conditions for the Kalman Filter.

This function allows to set an initial state estimate vector and an initial error covariance matrix estimate. This must be called at least once, after all dimensioning functions and before any other function. However, it can also be called anytime to reset or modify x or P.

Parameters:
x_ State vector estimate. Will be destroyed.
P_ Error covariance matrix estimate. Will be destroyed.
Warning:
If setDim(), setSizeX() or setSizeW() is called, then init() must be called again before any other non-dimensioning function.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::step (  Vector &  u_,
const Vector &  z_
) 
 

Makes one prediction-correction step.

This is the main EKFilter function. First, it resizes any matrix who needs it. Then, it proceeds to the time update phase, using the input vector u_. This means that the following virtual functions should be called : makeCommonProcess(), makeA(), makeW(), makeQ() and makeProcess(). At this stage, x contains a current predicted state instead of an old corrected state. If z_ is empty, that is, if there are no measures in this step, there is no correction and the function stops there. Else, the measure update phase begins. This means that the following virtual functions should be called : makeCommonMeasure(), makeHImpl(), makeVImpl(), makeRImpl(), makeMeasure() and makeDZ().After this phase, x contains the new corrected state.

Parameters:
u_ Input vector. Will not be destroyed. Can be empty.
z_ Measurement vector. Will not be destroyed. Can be empty.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::timeUpdateStep (  Vector &  u_  ) 
 

Makes one prediction step.

This function first resizes any matrix who needs it. Then, it proceeds to the time update phase, using the input vector u_. This means that the following virtual functions should be called : makeCommonProcess(), makeA(), makeW(), makeQ() and makeProcess(). At this stage, x contains a current predicted state instead of an old corrected state.

Parameters:
u_ Input vector. Will not be destroyed. Can be empty.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::measureUpdateStep (  const Vector &  z_  ) 
 

Makes one correction step.

First, this function resizes any matrix who needs it. If z_ is empty, that is, if there are no measures in this step, there is no correction and the function stops there. Else, the measure update phase begins. This means that the following virtual functions should be called : makeCommonMeasure(), makeHImpl(), makeVImpl(), makeRImpl(), makeMeasure() and makeDZ().After this phase, x contains the new corrected state.

Parameters:
z_ Measurement vector. Will not be destroyed. Can be empty.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
const EKFilter< T, BEG, OQ, OVR, DBG >::Vector & Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::predict (  Vector &  u_  ) 
 

Returns the predicted state vector (a priori state estimate).

This function is used to predict a future state. First, it resizes any matrix who needs it. Then, it does a partial time update, in the sense that only x is updated, not P. This also means that only the following virtual functions should be called : makeCommonProcess() and makeProcess().

Parameters:
u_ Input vector. Will not be destroyed. Can be empty.
Note:
The real x is not modified by this function (this is a const function). Only a copy of x is returned.
Warning:
For better efficiency, the prediction is returned by reference. The reference points to an internal member of the filter, which means that a new prediction (and many other functions) will invalidate the contents of this vector. This also means that this vector must be copied (or better yet, swapped) as soon as possible if its data is needed later.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
const EKFilter< T, BEG, OQ, OVR, DBG >::Vector & Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::simulate (   ) 
 

Returns the predicted measurement vector.

This function is used to predict a future measurement. First, it resizes any matrix who needs it. Then, it does a partial measure update, in the sense that only z is calculated : x and P are not updated. This also means that only the following virtual functions should be called : makeCommonMeasure() and makeMeasure().

Note:
This is a const function. It only works on copies of vectors.
Warning:
For better efficiency, the prediction is returned by reference. The reference points to an internal member of the filter, which means that a new prediction (and many other functions) will invalidate the contents of this vector. This also means that this vector must be copied (or better yet, swapped) as soon as possible if its data is needed later.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
const EKFilter< T, BEG, OQ, OVR, DBG >::Matrix & Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::calculateP (   )  const
 

Returns the a posteriori error covariance estimate matrix.

Warning:
This is not a simple return statement. Since P is not kept and updated in the filter (an alternate and more stable representation of P is used), calculations are involved to retrieve P. So, use this function wisely.

For better efficiency, P is returned by reference. The reference points to an internal member of the filter, which means that other functions may invalidate the contents of this matrix. This also means that this matrix must be copied (or better yet, swapped) as soon as possible if its data is needed later.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::NoModification (   )  [protected]
 

Allows optimizations on some calculations.

By default, the EKFilter template class suppose that matrix pre-creators and creators modify all matrices. However, if it could suppose that some of these functions do not modify anything, some calculations could be optimized away. The NoModification() function says that the function in which it has been called has not modified any matrix. For optimization purposes, this means that this function should be called in every non-mutating execution branch of all make*() and makeBase*() functions.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeBaseQ (   )  [protected, virtual]
 

Virtual pre-creator of Q.

Note:
If OQ is true, that is, if Q is always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeBaseV (   )  [protected, virtual]
 

Virtual pre-creator of V.

Note:
If OVR is true, that is, if both V and R are always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeBaseR (   )  [protected, virtual]
 

Virtual pre-creator of R.

Note:
If OVR is true, that is, if both V and R are always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeCommonProcess (   )  [protected, virtual]
 

Optional function used to precalculate common values for process.

If complex calculations are needed for more than one of makeA(), makeW(), makeQ() and makeProcess() functions, then this function can be used to store the results in temporary variables of the derived class.

Warning:
This function must not modify any matrix of the base class.

This function must not be used to store permanent state. In other words, all calculations performed in this function should be temporary. This is because the predict() function will call this function but has no knowledge of how to undo it.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeQ (   )  [protected, virtual]
 

Virtual creator of Q.

Note:
If OQ is true, that is, if Q is always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
virtual void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeProcess (   )  [protected, pure virtual]
 

Actual process $ f(x, u, 0) $ . Fills in new x by using old x.

This function must be overridden, since it is the core of the system process.

Warning:
This function should have no side effects to class members (even members of derived classes) other than x. This is because this function is used by predict(), which does a calculation and then undoes it before returning the result.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeCommonMeasure (   )  [protected, virtual]
 

Optional function used to precalculate common values for measurement.

If complex calculations are needed for more than one of makeH(), makeV(), makeR(), makeMeasure() and makeDZ() functions, then this function can be used to store the results in temporary variables of the derived class.

Warning:
This function must not modify any matrix of the base class.

This function must not be used to store permanent state. In other words, all calculations performed in this function should be temporary. This is because the simulate() function will call this function but has no knowledge of how to undo it.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeV (   )  [protected, virtual]
 

Virtual creator of V.

Note:
If OVR is true, that is, if both V and R are always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeR (   )  [protected, virtual]
 

Virtual creator of R.

Note:
If OVR is true, that is, if both V and R are always diagonal, then it is not necessary to initialize non-diagonal elements with anything meaningful.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
virtual void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeMeasure (   )  [protected, pure virtual]
 

Actual measurement function $ h(x, 0) $ . Fills in z.

This function must be overridden, since it is the core of the measurement system. At the time this will be called, x contains the predicted state (a priori state estimate), which is the one that must be used with the measurement function.

Warning:
This function should have no side effects to class members (even members of derived classes) other than z. This is because this function is used by simulate(), which does a calculation and then undoes it before returning the result.

template<typename T, K_UINT_32 BEG, bool OQ, bool OVR, bool DBG>
void Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::makeDZ (   )  [protected, virtual]
 

Hook-up function to modify innovation vector.

This function should rarely be overridden ; this is more of a hack than anything else. In fact, this is used to perform adjustements on the result of substracting the predicted measurement vector to the real measurement vector. This is needed, for example, when measures include angles. It may be mandatory that the difference of the two angles be in a certain range, like $ [-\pi, \pi] $ .

Member Data Documentation

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Vector Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::x [protected]
 

Corrected state vector.

This is an n-sized vector. Derived classes should modify it only through makeProcess().

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Vector Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::u [protected]
 

Input vector.

This is an nu-sized vector. Derived classes should never modify it.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Vector Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::z [protected]
 

Predicted measurement vector.

This is an m-sized vector. Derived classes should modify it only through makeMeasure().

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Vector Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::dz [protected]
 

Innovation vector.

This is an m-sized vector. Derived classes should modify it only through makeDZ(). The innovation vector is the difference between the real measurement vector and the predicted one.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::A [protected]
 

A jacobian matrix.

This is an n by n jacobian matrix of partial derivatives, defined as follow :

\[ A_{[i,j]} = \frac{\partial f_{[i]}}{\partial x_{[j]}} \]

Derived classes should modify it only through makeBaseA() for the constant part and makeA() for the variable part.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::W [protected]
 

A jacobian matrix.

This is an n by nw jacobian matrix of partial derivatives, defined as follow :

\[ W_{[i,j]} = \frac{\partial f_{[i]}}{\partial w_{[j]}} \]

Derived classes should modify it only through makeBaseW() for the constant part and makeW() for the variable part.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::Q [protected]
 

Process noise covariance matrix.

This is the nw by nw covariance matrix of w, that is :

\[ Q = E\left( w w^T \right) \]

Derived classes should modify it only through makeBaseQ() for the constant part and makeQ() for the variable part. If Q is always diagonal, then you should turn on the OQ optimization.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::H [protected]
 

A jacobian matrix.

This is an m by n jacobian matrix of partial derivatives, defined as follow :

\[ H_{[i,j]} = \frac{\partial h_{[i]}}{\partial x_{[j]}} \]

Derived classes should modify it only through makeBaseH() for the constant part and makeH() for the variable part.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::V [protected]
 

A jacobian matrix.

This is an m by nv jacobian matrix of partial derivatives, defined as follow :

\[ V_{[i,j]} = \frac{\partial h_{[i]}}{\partial v_{[j]}} \]

Derived classes should modify it only through makeBaseV() for the constant part and makeV() for the variable part. If both V and R are always diagonal, then you should turn on the OVR optimization.

template<typename T, K_UINT_32 BEG, bool OQ = false, bool OVR = false, bool DBG = true>
Matrix Kalman::EKFilter< T, BEG, OQ, OVR, DBG >::R [protected]
 

Measurement noise covariance matrix.

This is the nv by nv covariance matrix of v, that is :

\[ R = E\left( v v^T \right) \]

Derived classes should modify it only through makeBaseR() for the constant part and makeR() for the variable part. If both V and R are always diagonal, then you should turn on the OVR optimization.

The documentation for this class was generated from the following files:
Generated on Sat Jan 28 21:02:01 2006 for KFilter by  doxygen 1.4.5