This class implements an extended Kalman filter to be used for example to solve the SLAM problem. More...

Detailed Description

This class implements an extended Kalman filter to be used for example to solve the SLAM problem.

The update stage is optimized using the fact that the observation model only apply to two blocks of the state vector, the complexity of this step is reduced to O(n^2) (instead of O(n^3) for the usual implementation). Also the prediction model can be apply only on a given part of the state vector (other states are supposed to be constant). Also the state can be efficiently resized using the softResize() method.

Definition at line 325 of file kalmanFilter.hpp.

#include <kalmanFilter.hpp>

Inheritance diagram for jafar::filter::BlockExtendedKalmanFilter:

[legend]

List of all members.

Public Member Functions
	BlockExtendedKalmanFilter (std::size_t sizeMax_, std::size_t size_)
std::size_t	sizeStateMax () const
virtual std::size_t	sizeState () const
void	softResize (std::size_t size_)
void	predict (std::size_t index1_, std::size_t index2_, LinearPredictModel const &m_)
	Prediction over a substate.
void	predict (std::size_t index1_, std::size_t index2_, JacobianBlockPredictModel &m_)
	Prediction over a substate.
void	predict (std::size_t index1_, std::size_t index2_, JacobianBlockCommandPredictModel &m_, jblas::vec const &u_)
	Prediction over a substate.
void	update (LinearObserveModel &m_, std::size_t index, const jblas::vec &z_)
void	update (JacobianObserveModel &m_, std::size_t index, jblas::vec const &z_)
	This function computes the observation step.
void	update (JacobianObserveModel &m_, std::size_t index_, jblas::vec_range &x_, jblas::sym_mat_range &P_, jblas::vec const &z_)
void	update (BlockObserveModel &m_, std::size_t index1_, std::size_t index2_, jblas::vec_range &x1_, jblas::vec_range &x2_, jblas::sym_mat_range &P1_, jblas::sym_mat_range &P2_, const jblas::vec &z_, jmath::GaussianVector &zPred)
void	applyConstraint (JacobianStrongConstraintModel &model, std::size_t index_)
void	applyConstraint (JacobianStrongConstraintModel &model, std::size_t index_, jblas::vec_range &x_, jblas::sym_mat_range &P_)
double	computeMahalanobisDistance (LinearObserveModel &m_, std::size_t index_, const jblas::vec &z_)
double	computeMahalanobisDistance (LinearObserveModel &m_, const jblas::vec_range &x_, const jblas::sym_mat_range &P_, const jblas::vec &z_)
double	computeMahalanobisDistance (BlockObserveModel &m_, std::size_t index1_, std::size_t index2_, const jblas::vec_range &x1_, const jblas::vec_range &x2_, const jblas::sym_mat_range &P1_, const jblas::sym_mat_range &P2_, const jblas::vec &z_)
Public Attributes
jblas::vec_range *	x_r
jblas::sym_mat_range *	P_r
Protected Member Functions
void	updateBlockExtended (std::size_t index_, const jblas::sym_mat_range &P_, const jblas::sym_mat_range &Pcol_, const jblas::mat_range &H_, double y_, double R_)
	update one block.
void	updateBlockExtended (std::size_t index_, const jblas::sym_mat_range &P_, const jblas::sym_mat_range &Pcol_, const jblas::mat &H_, jblas::vec const &y, jblas::sym_mat const &R_)
void	updateBlockExtended (std::size_t index1_, std::size_t index2_, const jblas::sym_mat_range &P1_, const jblas::sym_mat_range &P2_, const jblas::sym_mat_range &P12_, const jblas::sym_mat_range &P21_, const jblas::sym_mat_range &Pcol1_, const jblas::sym_mat_range &Pcol2_, const jblas::mat_range &H1_, const jblas::mat_range &H2_, double y_, double R_)
	update two blocks.
void	updateBlockExtended (std::size_t index1_, std::size_t index2_, const jblas::sym_mat_range &P1_, const jblas::sym_mat_range &P2_, const jblas::sym_mat_range &P12_, const jblas::sym_mat_range &P21_, const jblas::sym_mat_range &Pcol1_, const jblas::sym_mat_range &Pcol2_, const jblas::mat &H1_, const jblas::mat &H2_, jblas::vec const &y_, jblas::sym_mat const &R_)
Protected Attributes
jblas::mat_range *	K_r
jblas::mat_range *	KH_tmp_r
Friends
std::ostream &	operator<< (std::ostream &s, const BlockExtendedKalmanFilter &f_)

Member Function Documentation

double jafar::filter::BlockExtendedKalmanFilter::computeMahalanobisDistance	(	LinearObserveModel &	m_,
		std::size_t	index_,
		const jblas::vec &	z_
	)

Table of the values of the mahalanobis distance and associated probability (area) for diffenrent dimensions (df).

df\area .995 .990      .975      .950      .900      .750      .500      .250      .100      .050      .025      .010      .005
1  0.00004   0.00016  0.00098   0.00393   0.01579   0.10153   0.45494   1.32330   2.70554   3.84146   5.02389   6.63490   7.87944
2  0.01003   0.02010  0.05064   0.10259   0.21072   0.57536   1.38629   2.77259   4.60517   5.99146   7.37776   9.21034   10.59663
3  0.07172   0.11483  0.21580   0.35185   0.58437   1.21253   2.36597   4.10834   6.25139   7.81473   9.34840   11.34487   12.83816
4  0.20699   0.2971   0.48442   0.71072   1.06362   1.92256   3.35669   5.38527   7.77944   9.48773   11.14329   13.27670   14.86026
5  0.41174   0.55430  0.83121   1.14548   1.61031   2.67460   4.35146   6.62568   9.23636   11.07050   12.83250   15.08627   16.74960 
6  0.67573   0.87209  1.23734   1.63538   2.20413   3.45460   5.34812   7.84080   10.64464   12.59159   14.44938   16.81189   18.54758
7  0.98926   1.23904  1.68987   2.16735   2.83311   4.25485   6.34581   9.03715   12.01704   14.06714   16.01276   18.47531   20.27774
8  1.34441   1.64650  2.17973   2.73264   3.48954   5.07064   7.34412   10.21885   13.36157   15.50731   17.53455   20.09024   21.95495
9  1.73493   2.08790  2.70039   3.32511   4.16816   5.89883   8.34283   11.38875   14.68366   16.91898   19.02277   21.66599   23.58935
10 2.15586   2.55821  3.24697   3.94030   4.86518   6.73720   9.34182   12.54886   15.98718   18.30704   20.48318   23.20925   25.18818

	 (values found on http://www.statsoft.com/textbook/sttable.html)

void jafar::filter::BlockExtendedKalmanFilter::predict	(	std::size_t	index1_,
		std::size_t	index2_,
		LinearPredictModel const &	m_
	)

Prediction over a substate.

Warning:: only substate starting from 0 is supported.

void jafar::filter::BlockExtendedKalmanFilter::predict	(	std::size_t	index1_,
		std::size_t	index2_,
		JacobianBlockPredictModel &	m_
	)

Prediction over a substate.

Warning:: only substate starting from 0 is supported.

void jafar::filter::BlockExtendedKalmanFilter::predict	(	std::size_t	index1_,
		std::size_t	index2_,
		JacobianBlockCommandPredictModel &	m_,
		jblas::vec const &	u_
	)

Prediction over a substate.

Warning:: only substate starting from 0 is supported.

virtual std::size_t jafar::filter::BlockExtendedKalmanFilter::sizeState ( ) const [inline, virtual]

Returns:: size of the filter state.

Reimplemented from jafar::filter::BaseKalmanFilter.

Definition at line 337 of file kalmanFilter.hpp.

void jafar::filter::BlockExtendedKalmanFilter::update	(	JacobianObserveModel &	m_,
		std::size_t	index,
		jblas::vec const &	z_
	)

This function computes the observation step.

Parameters:

m_	observation model
z_	observation

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

modules/filter/include/filter/kalmanFilter.hpp

Detailed Description

Public Member Functions

Public Attributes

Protected Member Functions

Protected Attributes

Friends

Member Function Documentation