#include <SRiteratedextendedkalmanfilter.h>

Inheritance diagram for BFL::SRIteratedExtendedKalmanFilter:

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:

Currently only works for implicit measurement model (LinearAnalyticMeasurementModelGaussianUncertainty_Implicit) the implicit measurement model is described as :
$0 = f (x,z)$
and internally defined as a linear measurement model with virtual measurement z_k^{virtual}
$z_k^{virtual} = H(x_k,z_k) \times x_k + N(\mu(x_{k},z_k) ,\Sigma(x_k,z_k))$

See also:
KalmanFilter

Definition at line 53 of file SRiteratedextendedkalmanfilter.h.

Constructor & Destructor Documentation

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

Constructor

Precondition:: you created the prior

Parameters:

prior	pointer to the Monte Carlo Pdf prior density
nr_it	the number of iterations in one update

Definition at line 29 of file SRiteratedextendedkalmanfilter.cpp.

BFL::SRIteratedExtendedKalmanFilter::~SRIteratedExtendedKalmanFilter ( ) [virtual]

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.

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.

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]`