kalman
Class Kalman

java.lang.Object
  extended by kalman.Kalman

public class Kalman
extends java.lang.Object

Kalman filter (state).

The structure Kalman 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 Kalman filter (notation and the formulae below are borrowed from the Kalman 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 Kalman filter, all the matrices: A, B, H, Q and R are initialized once after Kalman structure is allocated via constructor. However, the same structure and the same functions may be used to simulate extended Kalman filter by linearizing extended Kalman 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
Kalman(int dynam_params, int measure_params)
          Constructor in case of no control.
Kalman(int dynam_params, int measure_params, int control_params)
          The construstor allocates Kalman filter and all its matrices and initializes them somehow.
 
Method Summary
 Matrix Correct(Matrix measurement)
          Adjusts model state.
static void main(java.lang.String[] args)
          Test function
 Matrix Predict()
          Alias for prediction with no control.
 Matrix Predict(Matrix control)
          Estimates subsequent model state.
 
Methods inherited from class java.lang.Object
clone, equals, finalize, getClass, hashCode, notify, notifyAll, toString, wait, wait, wait
 

Constructor Detail

Kalman

public Kalman(int dynam_params,
              int measure_params,
              int control_params)
       throws java.lang.Exception
The construstor allocates Kalman filter and all its matrices and initializes them somehow.

Parameters:
dynam_params - number of state parameters.
measure_params - number of measurement parameters.
control_params - number of control parameters.
Throws:
java.lang.IllegalArgumentException - Kalman filter dimensions exception.
java.lang.Exception

Kalman

public Kalman(int dynam_params,
              int measure_params)
       throws java.lang.Exception
Constructor in case of no control.

Parameters:
dynam_params - number of state parameters.
measure_params - number of measurement parameters.
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 - Kalman "gain" matrix.
 

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

Parameters:
measurement - Matrix containing the measurement vector.
Returns:
The function returns the corrected state.

main

public static void main(java.lang.String[] args)
Test function

Parameters:
args -