#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) 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 Gaussian *	PostGet ()
	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:

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

◆ SRIteratedExtendedKalmanFilter()

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.

◆ ~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
	)