Public Member Functions | Private Attributes
BFL::SRIteratedExtendedKalmanFilter Class Reference

#include <SRiteratedextendedkalmanfilter.h>

Inheritance diagram for BFL::SRIteratedExtendedKalmanFilter:
Inheritance graph
[legend]

List of all members.

Public Member Functions

virtual void CalculateCovariance (MatrixWrapper::Matrix &R_vf, MatrixWrapper::Matrix &H_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &SR)
 Calculate the covariance of the new state estimate (P_k)
virtual void CalculateMatrix (MatrixWrapper::Matrix &H_i, MatrixWrapper::SymmetricMatrix &R_i, MatrixWrapper::Matrix &invS, MatrixWrapper::Matrix &Sr, MatrixWrapper::Matrix &K_i)
 Calculate K_i , invS and Sr.
virtual void CalculateMean (MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i)
 Calculate the new state estimate.
virtual void CalculateMeasUpdate (MatrixWrapper::ColumnVector z, MatrixWrapper::ColumnVector Z, MatrixWrapper::Matrix H, MatrixWrapper::SymmetricMatrix R)
virtual void MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
 Perform a measurement update use a measurement model, measurements z and virutal measurements s.
virtual void MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
 Perform a measurement update use a measurement model, measurements z.
void PriorSet (MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P)
 Set mean and covariance of the state estimation to a specific value.
MatrixWrapper::Matrix SRCovarianceGet () const
 Returns a square root of the covariance of the measurement input u.
void SRCovarianceSet (MatrixWrapper::Matrix JP_new)
 Set the square root covariance to a specific value.
 SRIteratedExtendedKalmanFilter (Gaussian *prior, unsigned int nr_it=1)
virtual void SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
 Perform a system update with the current measurement model ans system model using an input u.
virtual void SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel)
 Perform a system update with the current measurement model and system model.
virtual ~SRIteratedExtendedKalmanFilter ()
 Destructor.

Private Attributes

MatrixWrapper::Matrix JP
 the upper triangular matrix cholesky decompostion of the state covariance ( $ JP*JP^{'}=P $)
unsigned int nr_iterations
 Variable indicating the number of iterations of the filter.

Detailed Description

This is a class implementing the Kalman Filter (KF) class for Square Root Iterated Extended Kalman Filters. this is a possible implementation of a Kalman filter, which in will yield better numerical stable results, since the covariance matrix is defined as the Square root of the Covariance matrix of the state estimation. See

     @Book{   Anderson_auxiliary,
      author    = {Anderson, B.D.O. and Moore, J.B.},
      title     = {Optimal filtering},
      publisher = {Prentice-Hall, Englewood Cliffs, NJ },
      year      = {1979}
    }
     

for more details. Note that this particular implementation:

Definition at line 53 of file SRiteratedextendedkalmanfilter.h.


Constructor & Destructor Documentation

Constructor

Precondition:
you created the prior
Parameters:
priorpointer to the Monte Carlo Pdf prior density
nr_itthe number of iterations in one update

Definition at line 29 of file SRiteratedextendedkalmanfilter.cpp.

Destructor.

Definition at line 36 of file SRiteratedextendedkalmanfilter.cpp.


Member Function Documentation

void BFL::SRIteratedExtendedKalmanFilter::CalculateCovariance ( MatrixWrapper::Matrix &  R_vf,
MatrixWrapper::Matrix &  H_i,
MatrixWrapper::Matrix &  invS,
MatrixWrapper::Matrix &  SR 
) [virtual]

Calculate the covariance of the new state estimate (P_k)

\[ JP = JP-JP*JP^{'}*H_k^{'}*inv(S)^{'}*inv(R+Sr)*H_k*JP \]

\[ P_k=JP*JP^{'} \]

Definition at line 221 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::CalculateMatrix ( MatrixWrapper::Matrix &  H_i,
MatrixWrapper::SymmetricMatrix &  R_i,
MatrixWrapper::Matrix &  invS,
MatrixWrapper::Matrix &  Sr,
MatrixWrapper::Matrix &  K_i 
) [virtual]

Calculate K_i , invS and Sr.

\[ Sr= cholesky(H_i*JP*JP^{'}*H_i^{'}+R_i)^{'} \]

\[ invS=inv(Sr)\]

\[ K_i=JP*JP^{'}*H_i^{'}*invS^{'}*invS \]

Definition at line 188 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::CalculateMean ( MatrixWrapper::ColumnVector &  x_k,
const MatrixWrapper::ColumnVector &  z,
MatrixWrapper::ColumnVector &  Z_i,
MatrixWrapper::Matrix &  K_i 
) [virtual]

Calculate the new state estimate.

\[ x_k = x_{k-} + K.(z - Z) \]

Definition at line 213 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::CalculateMeasUpdate ( MatrixWrapper::ColumnVector  z,
MatrixWrapper::ColumnVector  Z,
MatrixWrapper::Matrix  H,
MatrixWrapper::SymmetricMatrix  R 
) [virtual]

Calculate Kalman filter Measurement Update

\[ x_k = x_{k-} + K.(z - Z) \]

\[ S=(H_k*J_k*J_k^{'}*H_k+D*R*D^{'})^{'} \]

\[ Sr=cholesky(S)^{'}\]

\[ J_k = J_k-J_k*J_k^{'}*H^{'}*inv(S)^{'}*inv(R+Sr)*H_k*J_k \]

with

\[ K = J_{k}*J_{k}^{'}*H'*inv(S)^{'}*inv(S) \]

Definition at line 169 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::MeasUpdate ( MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const  measmodel,
const MatrixWrapper::ColumnVector &  z,
const MatrixWrapper::ColumnVector &  s 
) [virtual]

Perform a measurement update use a measurement model, measurements z and virutal measurements s.

Implements BFL::KalmanFilter.

Definition at line 66 of file SRiteratedextendedkalmanfilter.cpp.

virtual void BFL::SRIteratedExtendedKalmanFilter::MeasUpdate ( MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const  measmodel,
const MatrixWrapper::ColumnVector &  z 
) [virtual]

Perform a measurement update use a measurement model, measurements z.

void BFL::SRIteratedExtendedKalmanFilter::PriorSet ( MatrixWrapper::ColumnVector &  X,
MatrixWrapper::SymmetricMatrix &  P 
)

Set mean and covariance of the state estimation to a specific value.

Definition at line 163 of file SRiteratedextendedkalmanfilter.cpp.

Returns a square root of the covariance of the measurement input u.

Definition at line 153 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::SRCovarianceSet ( MatrixWrapper::Matrix  JP_new)

Set the square root covariance to a specific value.

Definition at line 158 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::SysUpdate ( SystemModel< MatrixWrapper::ColumnVector > *const  sysmodel,
const MatrixWrapper::ColumnVector &  u 
) [virtual]

Perform a system update with the current measurement model ans system model using an input u.

Implements BFL::KalmanFilter.

Definition at line 39 of file SRiteratedextendedkalmanfilter.cpp.

void BFL::SRIteratedExtendedKalmanFilter::SysUpdate ( SystemModel< MatrixWrapper::ColumnVector > *const  sysmodel) [virtual]

Perform a system update with the current measurement model and system model.

Definition at line 59 of file SRiteratedextendedkalmanfilter.cpp.


Member Data Documentation

MatrixWrapper::Matrix BFL::SRIteratedExtendedKalmanFilter::JP [private]

the upper triangular matrix cholesky decompostion of the state covariance ( $ JP*JP^{'}=P $)

Definition at line 118 of file SRiteratedextendedkalmanfilter.h.

Variable indicating the number of iterations of the filter.

Definition at line 116 of file SRiteratedextendedkalmanfilter.h.


The documentation for this class was generated from the following files:


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