Base class for Kalman filters. More...

Detailed Description

Base class for Kalman filters.

Definition at line 29 of file kalmanFilter.hpp.

#include <kalmanFilter.hpp>

Classes
struct	CorrectionStack
struct	StackedCorrection
Public Member Functions
	ExtendedKalmanFilterIndirect (size_t _size)
size_t	size ()
jblas::vec &	x ()
jblas::sym_mat &	P ()
double &	x (size_t i)
double &	P (size_t i, size_t j)
void	predict (const ind_array &iax, const mat &F_v, const ind_array &iav, const mat &F_u, const sym_mat &U)
	Predict covariances matrix.
void	predict (const ind_array &iax, const mat &F_v, const ind_array &iav, const sym_mat &Q)
	Predict covariances matrix.
void	initialize (const ind_array &iax, const mat &G_rs, const ind_array &ia_rs, const ind_array &ia_l, const mat &G_y, const sym_mat &R)
	EKF initialization from fully observable info.
void	initialize (const ind_array &iax, const mat &G_v, const ind_array &ia_rs, const ind_array &ia_l, const mat &G_y, const sym_mat &R, const mat &G_n, const sym_mat &N)
	EKF initialization from partially observable info.
void	reparametrize (const ind_array &iax, const mat &J_l, const ind_array &ia_old, const ind_array &ia_new)
	EKF reparametrization.
void	computeKalmanGain (const ind_array &ia_x, Innovation &inn, const mat &INN_rsl, const ind_array &ia_rsl)
	Compute Kalman gain.
void	correct (const ind_array &iax, Innovation &inn, const mat &INN_rsl, const ind_array &ia_rsl, bool correct_P=true)
	EKF correction.
void	stackCorrection (Innovation &inn, const mat &INN_rsl, const ind_array &ia_rsl)
void	correctAllStacked (const ind_array &iax, bool correct_P=true)
void	clearStack ()
Public Attributes
mat	K
mat	PJt_tmp
Protected Types
typedef std::list < StackedCorrection >	CorrectionList
Protected Attributes
vec	stackedInnovation_x
sym_mat	stackedInnovation_P
sym_mat	stackedInnovation_iP
CorrectionStack	corrStack
Private Attributes
size_t	size_
vec	x_
sym_mat	P_

Member Function Documentation

void jafar::rtslam::ExtendedKalmanFilterIndirect::computeKalmanGain	(	const ind_array &	ia_x,
		Innovation &	inn,
		const mat &	INN_rsl,
		const ind_array &	ia_rsl
	)

Compute Kalman gain.

The result is in the class members K and PJt_tmp.

Parameters:

ia_x	ind. array to all states used in the map.
inn	innovation.
INN_rsl	innovation Jacobian.
ia_rsl	ind. array to states in the innovation function.

void jafar::rtslam::ExtendedKalmanFilterIndirect::correct	(	const ind_array &	iax,
		Innovation &	inn,
		const mat &	INN_rsl,
		const ind_array &	ia_rsl,
		bool	correct_P = `true`
	)

EKF correction.

This function uses the Innovation class to extract all useful chunks necessary for EKF correction. In partucular, the following info is recovered from Innovation:

{z, Z} = {inn.x, inn.P}, mean and conv. matrices.

Also, the Jacobian and indirect-array associated to the innovation:

INN_rsl: the Jacobian wrt the states that contributed to the innovation
ia_rsl: the indices to these states

The EKF update is then the following (see Sola etal., IROS 2009):

K = -P * trans(INN_rsl) * inv(Z)
x <-- x + K * z
P <-- P + K * INN_rsl * P

Parameters:

ia_x	the indirect array of used indices in the map.
inn	the Innovation.
INN_rsl,:	the Jacobian wrt the states that contributed to the innovation
ia_rsl,:	the indices to these states

void jafar::rtslam::ExtendedKalmanFilterIndirect::initialize	(	const ind_array &	iax,
		const mat &	G_rs,
		const ind_array &	ia_rs,
		const ind_array &	ia_l,
		const mat &	G_y,
		const sym_mat &	R
	)

EKF initialization from fully observable info.

This function alters the state vector structure to allocate a new element to be filtered.

Parameters:

iax	indirect array of indices to used states
G_rs	Jacobian of the back-projection function wrt vehicle states (robot and sensor).
ia_rs	indirect array of indices to robot and sensor
ia_l	indirect array of indices to the landmark
G_y	Jacobian of back-projection wrt the measurement
R	measurement noise covariances matrix

void jafar::rtslam::ExtendedKalmanFilterIndirect::initialize	(	const ind_array &	iax,
		const mat &	G_v,
		const ind_array &	ia_rs,
		const ind_array &	ia_l,
		const mat &	G_y,
		const sym_mat &	R,
		const mat &	G_n,
		const sym_mat &	N
	)

EKF initialization from partially observable info.

This function alters the state vector structure to allocate a new element to be filtered.

Parameters:

iax	indirect array of indices to used states
G_rs	Jacobian of the back-projection function wrt vehicle states (robot and sensor).
ia_rs	indirect array of indices to robot and sensor
ia_l	indirect array of indices to the landmark
G_y	Jacobian of back-projection wrt the measurement
R	measurement noise covariances matrix
G_n	Jacobian of back-projection wrt the non-measured prior
N	non-measured prior covariances matrix

void jafar::rtslam::ExtendedKalmanFilterIndirect::predict	(	const ind_array &	iax,
		const mat &	F_v,
		const ind_array &	iav,
		const mat &	F_u,
		const sym_mat &	U
	)

Predict covariances matrix.

This function predicts the future state of the covariances matrix. It uses a Jacobian F_v indexed by an indirect array iav into the state vector. The covariances matrix is also indexed by an indirect array iax containing the used states of the filter. It incorporates the process noise U via a second Jacobian F_u, mapping it to the state space via iav (the same as F_v). The formula is:

[Pvv, Pvm = [F_v*Pvv*F_v' + F_u*U*F_u' , F_v*Pvm Pmv, Pmm] Pmv*F_v' , Pmm ]

Parameters:

iax	the ind_array of all used states in the map.
F_v	the Jacobian of the process model.
iav	the ind_array of the process model states.
F_u	the Jacobian of the process model wrt the perturbation.
U	the covariances matrix of the perturbation in control-space.

void jafar::rtslam::ExtendedKalmanFilterIndirect::predict	(	const ind_array &	iax,
		const mat &	F_v,
		const ind_array &	iav,
		const sym_mat &	Q
	)

Predict covariances matrix.

This function predicts the future state of the covariances matrix. It uses a Jacobian F_v indexed by an indirect array iav into the state vector. The covariances matrix is also indexed by an indirect array iax containing the used states of the filter. It incorporates the process noise Q, already mapped to the state space. The formula is:

[Pvv, Pvm; Pmv, Pmm] = [F_v*Pvv*F_v'+Q , F_v*Pvm ; Pmv*F_v' , Pmm]

Parameters:

iax	the ind_array of all used states in the map.
F_v	the Jacobian of the process model.
iav	the ind_array of the process model states.
Q	the covariances matrix of the perturbation in state-space.

void jafar::rtslam::ExtendedKalmanFilterIndirect::reparametrize	(	const ind_array &	iax,
		const mat &	J_l,
		const ind_array &	ia_old,
		const ind_array &	ia_new
	)

EKF reparametrization.

This function alters the state structure to modify an element that is currently being filtered.

Parameters:

iax	indirect array of indices to used states
J_l	Jacobian of reparametrization wrt old landmark
ia_old	indices to old landmark parameters
ia_new	indices to new landmark parameters

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

modules/rtslam/include/rtslam/kalmanFilter.hpp

Detailed Description

Classes

Public Member Functions

Public Attributes

Protected Types

Protected Attributes

Private Attributes

Member Function Documentation