Public Member Functions | Private Attributes | List of all members
BFL::SRIteratedExtendedKalmanFilter Class Reference

#include <SRiteratedextendedkalmanfilter.h>

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

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) More...
 
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. More...
 
virtual void CalculateMean (MatrixWrapper::ColumnVector &x_k, const MatrixWrapper::ColumnVector &z, MatrixWrapper::ColumnVector &Z_i, MatrixWrapper::Matrix &K_i)
 Calculate the new state estimate. More...
 
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. More...
 
virtual void MeasUpdate (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
 Perform a measurement update use a measurement model, measurements z. More...
 
void PriorSet (MatrixWrapper::ColumnVector &X, MatrixWrapper::SymmetricMatrix &P)
 Set mean and covariance of the state estimation to a specific value. More...
 
MatrixWrapper::Matrix SRCovarianceGet () const
 Returns a square root of the covariance of the measurement input u. More...
 
void SRCovarianceSet (MatrixWrapper::Matrix JP_new)
 Set the square root covariance to a specific value. More...
 
 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. More...
 
virtual void SysUpdate (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel)
 Perform a system update with the current measurement model and system model. More...
 
virtual ~SRIteratedExtendedKalmanFilter ()
 Destructor. More...
 
- Public Member Functions inherited from BFL::KalmanFilter
void AllocateMeasModel (const vector< unsigned int > &meas_dimensions)
 Function to allocate memory needed during the measurement update,. More...
 
void AllocateMeasModel (const unsigned int &meas_dimensions)
 Function to allocate memory needed during the measurement update. More...
 
 KalmanFilter (Gaussian *prior)
 Constructor. More...
 
virtual GaussianPostGet ()
 Get Posterior density. More...
 
virtual ~KalmanFilter ()
 Destructor. More...
 
- Public Member Functions inherited from BFL::Filter< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector >
 Filter (Pdf< MatrixWrapper::ColumnVector > *prior)
 Constructor. More...
 
 Filter (const Filter< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > &filt)
 copy constructor More...
 
virtual void Reset (Pdf< MatrixWrapper::ColumnVector > *prior)
 Reset Filter. More...
 
int TimeStepGet () const
 Get current time. More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
 Full Update (system with inputs/sensing params) More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
 Full Update (system without inputs, with sensing params) More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
 Full Update (system without inputs/sensing params) More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
 Full Update (system with inputs, without sensing params) More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u)
 System Update (system with inputs) More...
 
virtual bool Update (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel)
 System Update (system without inputs) More...
 
virtual bool Update (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
 Measurement Update (system with "sensing params") More...
 
virtual bool Update (MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z)
 Measurement Update (system without "sensing params") More...
 
virtual ~Filter ()
 destructor More...
 

Private Attributes

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

Additional Inherited Members

- Protected Member Functions inherited from BFL::KalmanFilter
void CalculateMeasUpdate (const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &Z, const MatrixWrapper::Matrix &H, const MatrixWrapper::SymmetricMatrix &R)
 
void CalculateSysUpdate (const MatrixWrapper::ColumnVector &J, const MatrixWrapper::Matrix &F, const MatrixWrapper::SymmetricMatrix &Q)
 
void PostMuSet (const MatrixWrapper::ColumnVector &c)
 Set expected value of posterior estimate. More...
 
void PostSigmaSet (const MatrixWrapper::SymmetricMatrix &s)
 Set covariance of posterior estimate. More...
 
virtual bool UpdateInternal (SystemModel< MatrixWrapper::ColumnVector > *const sysmodel, const MatrixWrapper::ColumnVector &u, MeasurementModel< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector > *const measmodel, const MatrixWrapper::ColumnVector &z, const MatrixWrapper::ColumnVector &s)
 Actual implementation of Update, varies along filters. More...
 
- Protected Attributes inherited from BFL::KalmanFilter
Matrix _K
 
std::map< unsigned int, MeasUpdateVariables_mapMeasUpdateVariables
 
std::map< unsigned int, MeasUpdateVariables >::iterator _mapMeasUpdateVariables_it
 
ColumnVector _Mu_new
 
SymmetricMatrix _Sigma_new
 
Matrix _Sigma_temp
 
Matrix _Sigma_temp_par
 
Matrix _SMatrix
 
- Protected Attributes inherited from BFL::Filter< MatrixWrapper::ColumnVector, MatrixWrapper::ColumnVector >
Pdf< MatrixWrapper::ColumnVector > * _post
 Pointer to the Posterior Pdf. More...
 
Pdf< MatrixWrapper::ColumnVector > * _prior
 prior Pdf More...
 
int _timestep
 Represents the current timestep of the filter. More...
 

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

◆ SRIteratedExtendedKalmanFilter()

BFL::SRIteratedExtendedKalmanFilter::SRIteratedExtendedKalmanFilter ( Gaussian prior,
unsigned int  nr_it = 1 
)

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.

◆ ~SRIteratedExtendedKalmanFilter()

BFL::SRIteratedExtendedKalmanFilter::~SRIteratedExtendedKalmanFilter ( )
virtual

Destructor.

Definition at line 36 of file SRiteratedextendedkalmanfilter.cpp.

Member Function Documentation

◆ CalculateCovariance()

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.

◆ CalculateMatrix()

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.

◆ CalculateMean()

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.

◆ CalculateMeasUpdate()

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.

◆ MeasUpdate() [1/2]

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.

◆ MeasUpdate() [2/2]

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.

◆ PriorSet()

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.

◆ SRCovarianceGet()

Matrix BFL::SRIteratedExtendedKalmanFilter::SRCovarianceGet ( ) const

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

Definition at line 153 of file SRiteratedextendedkalmanfilter.cpp.

◆ SRCovarianceSet()

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.

◆ SysUpdate() [1/2]

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.

◆ SysUpdate() [2/2]

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

◆ JP

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.

◆ nr_iterations

unsigned int BFL::SRIteratedExtendedKalmanFilter::nr_iterations
private

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 Mon Feb 28 2022 21:56:34