Implementation of unscented kalman filter (UKF) for filtering non-linear processes. More...

#include <UnscentedKalman.h>

Public Member Functions
CvMat *	getState ()
	Returns the process state vector. More...

CvMat *	getStateCovariance ()
	Returns the process state covariance matrix. More...

void	initialize ()
	(Re-)initialize UKF internal state. More...

void	predict (UnscentedProcess *process_model)
	Updated the state by predicting. More...

	UnscentedKalman (int state_n, int obs_n, int state_k=0, double alpha=0.001, double beta=2.0)
	Initializes Unscented Kalman filter. More...

void	update (UnscentedObservation *observation)
	Updates the state by an observation. More...

	~UnscentedKalman ()

Private Attributes
CvMat *	invPredObsCovariance

CvMat *	kalmanGain

CvMat *	kalmanTmp

double	lambda

double	lambda2

int	obs_n

CvMat *	predObs

CvMat *	predObsCovariance

CvMat *	predObsDiff

int	sigma_n

CvMat **	sigma_predObs

CvMat **	sigma_state

bool	sigmasUpdated

CvMat *	sqrtStateCovariance

CvMat *	state

int	state_k

int	state_n

CvMat *	stateCovariance

CvMat *	stateD

CvMat *	stateDiff

CvMat *	statePredObsCrossCorrelation

CvMat *	stateTmp

CvMat *	stateU

CvMat *	stateV

Detailed Description

Implementation of unscented kalman filter (UKF) for filtering non-linear processes.

See http://www.cs.unc.edu/~welch/kalman/media/pdf/Julier1997_SPIE_KF.pdf for more details about UKF.

The UKF estimates a process state (represented by a vector) using observations of the process. Observations are some derivate of the process state as usually the process state cannot be directly observed.

UnscentedProcess models the process by predicting the next filter state based on the current filter state.

UnscentedObservation models the observation by predicting observation results based on the current filter state.

UnscentedKalman holds the estimated process state vector and its covariance matrix. The new process state can be estimated using predict and update methods.

The current implementation does not separate process noise elements from the process state vector. It is therefore the responsibility of the user to include noise terms into process state and state covariance.

class MyUnscentedProcess : public UnscentedProcess {
  void f(CvMat *state) { // compute new state }
  CvMat *getProcessNoise() { return _noise; }
} myProcess;
class MyUnscentedObservation : public UnscentedObservation {
  void h(CvMat *z, cvMat *state) { // compute measurement vector z from state }
  CvMat *getObservation() { return _obs; }
  CvMat *getObservationNoise() { return _noise; }
} myObservation;
int state_n = NUMBER_OF_ELEMENTS_IN_PROCESS_STATE_VECTOR;
int obs_n = NUMBER_OF_ELEMENTS_IN_PROCESS_OBSERVATION_VECTOR;
int state_k = NUMBER_OF_PROCESS_NOISE_ELEMENTS; //TODO: Not supported at the moment.
UnscentedKalman ukf(state_n, obs_n, state_k);
initializeState(ukf.getState(), ukf.getStateCovariance());
ukf.initialize();
while (1) {
  ukf.predict(&myProcess);
  // measure new observation.
  ukf.update(&myObservation);
  CvMat *state = ukf.getState();
  // unpack state information from the state vector and do something with it.
}

Definition at line 96 of file UnscentedKalman.h.

Constructor & Destructor Documentation

alvar::UnscentedKalman::UnscentedKalman	(	int	state_n,
		int	obs_n,
		int	state_k = `0`,
		double	alpha = `0.001`,
		double	beta = `2.0`
	)

Initializes Unscented Kalman filter.

Initializes Unscented Kalman filter. The state vector returned by getState and state covariance matrix returned by getStateCovariance should be initialized before using the filter.

Separate state noise vector is currently unsupported. The user should include noise terms in the state vector directly. Set the noise mean into state vector and noise variance into state covariance matrix.

Parameters

state_n	The number of elements in process state vector.
obs_n	The number of elements in observation vector.
state_k	The number of noise elements used in the process model. TODO: This is currently unsupported.
alpha	Spread of sigma points.
beta	Prior knowlegde about the distribution (2 for Gaussian).

Definition at line 30 of file UnscentedKalman.cpp.

alvar::UnscentedKalman::~UnscentedKalman ( )

Definition at line 71 of file UnscentedKalman.cpp.

Member Function Documentation

CvMat* alvar::UnscentedKalman::getState ( )

inline

Returns the process state vector.

The returned state vector contains the current state of the process. The returned vector may be modified if the current process state is known, for example in initialization phase. If the vector is modified, initialize method must be called before calling either predict or update methods.

Returns: A vector of state_n elements.

Definition at line 173 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::getStateCovariance ( )

inline

Returns the process state covariance matrix.

The returned matrix contains the current state covariance. The matrix may be modified if the covariance is known, for example in initialization phase. If the matrix is modified, initialize method must be called before calling either predict of update methods.

Returns: state_n by state_n covariance matrix.

Definition at line 184 of file UnscentedKalman.h.

void alvar::UnscentedKalman::initialize ( )

(Re-)initialize UKF internal state.

Must be called before predict/update when ever state or state co-variance are changed.

Definition at line 97 of file UnscentedKalman.cpp.

void alvar::UnscentedKalman::predict ( UnscentedProcess * process_model )

Updated the state by predicting.

Updates the process state by predicting new state from the current state. Normally each predict call is followed with a call to update method.

Parameters

process_model The model implementation that is used to predict the next state.

Definition at line 158 of file UnscentedKalman.cpp.

void alvar::UnscentedKalman::update ( UnscentedObservation * observation )

Updates the state by an observation.

Updates the process state by a measurement that indirectly observed the correct process state. The observation implementation needs to hold the current measurement data and implement a transformation from process state into measurement (the UnscentedObservation::h method).

Parameters

observation The observation implementation the is used to update the current state.

Definition at line 218 of file UnscentedKalman.cpp.

Member Data Documentation

CvMat* alvar::UnscentedKalman::invPredObsCovariance

private

Definition at line 116 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::kalmanGain

private

Definition at line 120 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::kalmanTmp

private

Definition at line 121 of file UnscentedKalman.h.

double alvar::UnscentedKalman::lambda

private

Definition at line 103 of file UnscentedKalman.h.

double alvar::UnscentedKalman::lambda2

private

Definition at line 103 of file UnscentedKalman.h.

int alvar::UnscentedKalman::obs_n

private

Definition at line 100 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::predObs

private

Definition at line 114 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::predObsCovariance

private

Definition at line 115 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::predObsDiff

private

Definition at line 117 of file UnscentedKalman.h.

int alvar::UnscentedKalman::sigma_n

private

Definition at line 101 of file UnscentedKalman.h.

CvMat** alvar::UnscentedKalman::sigma_predObs

private

Definition at line 124 of file UnscentedKalman.h.

CvMat** alvar::UnscentedKalman::sigma_state

private

Definition at line 123 of file UnscentedKalman.h.

bool alvar::UnscentedKalman::sigmasUpdated

private

Definition at line 102 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::sqrtStateCovariance

private

Definition at line 107 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::state

private

Definition at line 105 of file UnscentedKalman.h.

int alvar::UnscentedKalman::state_k

private

Definition at line 99 of file UnscentedKalman.h.

int alvar::UnscentedKalman::state_n

private

Definition at line 98 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateCovariance

private

Definition at line 106 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateD

private

Definition at line 108 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateDiff

private

Definition at line 112 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::statePredObsCrossCorrelation

private

Definition at line 119 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateTmp

private

Definition at line 111 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateU

private

Definition at line 109 of file UnscentedKalman.h.

CvMat* alvar::UnscentedKalman::stateV

private

Definition at line 110 of file UnscentedKalman.h.

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

Public Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation