|
|||||||||
PREV CLASS NEXT CLASS | FRAMES NO FRAMES | ||||||||
SUMMARY: NESTED | FIELD | CONSTR | METHOD | DETAIL: FIELD | CONSTR | METHOD |
java.lang.Objectkalman.Kalman
public class Kalman
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)
|
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 |
---|
public Kalman(int dynam_params, int measure_params, int control_params) throws java.lang.Exception
dynam_params
- measure_params
- control_params
-
java.lang.IllegalArgumentException
- Kalman filter dimensions exception.
java.lang.Exception
public Kalman(int dynam_params, int measure_params) throws java.lang.Exception
dynam_params
- measure_params
-
java.lang.Exception
Method Detail |
---|
public Matrix Predict()
public Matrix Predict(Matrix control)
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),
control
- Control vector (uk), should be NULL if there
is no external control (control_params
=0).
public Matrix Correct(Matrix measurement)
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.
measurement
- Matrix containing the measurement vector.
public static void main(java.lang.String[] args)
|
|||||||||
PREV CLASS NEXT CLASS | FRAMES NO FRAMES | ||||||||
SUMMARY: NESTED | FIELD | CONSTR | METHOD | DETAIL: FIELD | CONSTR | METHOD |