jhplot.math.kalman

Class JKalman

java.lang.Object

jhplot.math.kalman.JKalman

public class JKalman
extends java.lang.Object

Kalman filter (state).

The structure JKalman is used to keep Kalman filter state. It is created by constructor function, updated by Predict and Correct functions.

Normally, the structure is used for standard JKalman filter (notation and the formulae below are borrowed from the JKalman tutorial [Welch95]):

 xk=A*xk-1+B*uk+wk
 zk=Hxk+vk,
 

where:

 xk (xk-1) - state of the system at the moment k (k-1)
 zk - measurement of the system state at the moment k
 uk - external control applied at the moment k
 wk and vk are normally-distributed process and measurement noise, respectively:
 p(w) ~ N(0,Q)
 p(v) ~ N(0,R),
 that is,
 Q - process noise covariance matrix, constant or variable,
 R - measurement noise covariance matrix, constant or variable
 

In case of standard JKalman filter, all the matrices: A, B, H, Q and R are initialized once after JKalman structure is allocated via constructor. However, the same structure and the same functions may be used to simulate extended JKalman filter by linearizing extended JKalman filter equation in the current system state neighborhood, in this case A, B, H (and, probably, Q and R) should be updated on every step.

Constructor Summary

Constructors

Constructor and Description
JKalman(int dynam_params, int measure_params) Constructor in case of no control.
JKalman(int dynam_params, int measure_params, int control_params) The construstor allocates JKalman filter and all its matrices and initializes them somehow.

Method Summary

Modifier and Type	Method and Description
Matrix	Correct(Matrix measurement) Adjusts model state.
Matrix	getControl_matrix() Getter
Matrix	getError_cov_post() Getter
Matrix	getError_cov_pre() Getter
Matrix	getGain() Getter
Matrix	getMeasurement_matrix() Getter
Matrix	getMeasurement_noise_cov() Getter
Matrix	getProcess_noise_cov() Getter
Matrix	getState_post()
Matrix	getState_pre() Getter
Matrix	getTransition_matrix()
Matrix	Predict() Alias for prediction with no control.
Matrix	Predict(Matrix control) Estimates subsequent model state.
void	setControl_matrix(Matrix control_matrix) Setter
void	setError_cov_post(Matrix error_cov_post) Setter
void	setError_cov_pre(Matrix error_cov_pre) Setter
void	setGain(Matrix gain) Setter
void	setMeasurement_matrix(Matrix measurement_matrix) Setter
void	setMeasurement_noise_cov(Matrix measurement_noise_cov) Setter
void	setProcess_noise_cov(Matrix process_noise_cov) Setter
void	setState_post(Matrix state_post) Setter
void	setState_pre(Matrix state_pre) Setter
void	setTransition_matrix(Matrix transition_matrix) Getter

Methods inherited from class java.lang.Object

equals, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait

Constructor Detail

JKalman

public JKalman(int dynam_params,
               int measure_params,
               int control_params)
        throws java.lang.Exception

The construstor allocates JKalman filter and all its matrices and initializes them somehow.

Parameters:

dynam_params -

measure_params -

control_params -

Throws:

java.lang.IllegalArgumentException - Kalman filter dimensions exception.

java.lang.Exception

JKalman

public JKalman(int dynam_params,
               int measure_params)
        throws java.lang.Exception

Constructor in case of no control.

Parameters:

dynam_params -

measure_params -

Throws:

java.lang.Exception

Method Detail

Predict

public Matrix Predict()

Alias for prediction with no control.

Returns:

Predict(no control).

Predict

public Matrix Predict(Matrix control)

Estimates subsequent model state.

The function estimates the subsequent stochastic model state by its current state and stores it at state_pre:

 x'k=A*xk+B*uk
 P'k=A*Pk-1*AT + Q,
 where
 x'k is predicted state (state_pre),
 xk-1 is corrected state on the previous step (state_post)
     (should be initialized somehow in the beginning, zero vector by default),
 uk is external control (control parameter),
 P'k is prior error covariance matrix (error_cov_pre)
 Pk-1 is posteriori error covariance matrix on the previous step (error_cov_post)
     (should be initialized somehow in the beginning, identity matrix by default),
 

Parameters:

control - Control vector (u_k), should be NULL if there is no external control (control_params=0).

Returns:

The function returns the estimated state.

Correct

public Matrix Correct(Matrix measurement)

Adjusts model state. The function KalmanCorrect adjusts stochastic model state on the basis of the given measurement of the model state:

 Kk=P'k*HT*(H*P'k*HT+R)-1
 xk=x'k+Kk*(zk-H*x'k)
 Pk=(I-Kk*H)*P'k
 where
 zk - given measurement (mesurement parameter)
 Kk - JKalman "gain" matrix.
 

The function stores adjusted state at state_post and returns it on output.

Parameters:

measurement - Matrix containing the measurement vector.

Returns:

setState_pre

public void setState_pre(Matrix state_pre)

Setter

Parameters:

state_pre -

getState_pre

public Matrix getState_pre()

Getter

Returns:

setState_post

public void setState_post(Matrix state_post)

Setter

Parameters:

state_post -

getState_post

public Matrix getState_post()

setTransition_matrix

public void setTransition_matrix(Matrix transition_matrix)

Getter

Parameters:

transition_matrix -

getTransition_matrix

public Matrix getTransition_matrix()

setControl_matrix

public void setControl_matrix(Matrix control_matrix)

Setter

Parameters:

control_matrix -

getControl_matrix

public Matrix getControl_matrix()

Getter

Returns:

setMeasurement_matrix

public void setMeasurement_matrix(Matrix measurement_matrix)

Setter

Parameters:

measurement_matrix -

getMeasurement_matrix

public Matrix getMeasurement_matrix()

Getter

Returns:

setProcess_noise_cov

public void setProcess_noise_cov(Matrix process_noise_cov)

Setter

Parameters:

process_noise_cov -

getProcess_noise_cov

public Matrix getProcess_noise_cov()

Getter

Returns:

setMeasurement_noise_cov

public void setMeasurement_noise_cov(Matrix measurement_noise_cov)

Setter

Parameters:

measurement_noise_cov -

getMeasurement_noise_cov

public Matrix getMeasurement_noise_cov()

Getter

Returns:

setError_cov_pre

public void setError_cov_pre(Matrix error_cov_pre)

Setter

Parameters:

error_cov_pre -

getError_cov_pre

public Matrix getError_cov_pre()

Getter

Returns:

setGain

public void setGain(Matrix gain)

Setter

Parameters:

gain -

getGain

public Matrix getGain()

Getter

Returns:

setError_cov_post

public void setError_cov_post(Matrix error_cov_post)

Setter

Parameters:

error_cov_post -

getError_cov_post

public Matrix getError_cov_post()

Getter

Returns: