JKalman

jkalman
Class JKalman

java.lang.Object
  extended by jkalman.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
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
 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
clone, equals, finalize, 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 (uk), 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:

JKalman