/*******************************************************************************
*
* JKalman - KALMAN FILTER (Java)
*
* Copyright (C) 2007 Petr Chmelar
*
* By downloading, copying, installing or using the software you agree to
* the license in licenseIntel.txt or in licenseGNU.txt
*
**************************************************************************** */
package jhplot.math.kalman;
import java.util.*;
import jhplot.math.kalman.jama.Matrix;
// import Jama.*;
/**
* 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.
*/
public class JKalman {
/** number of measurement vector dimensions */
int mp;
/** number of state vector dimensions */
int dp;
/** number of control vector dimensions */
int cp;
/** predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k) */
Matrix state_pre;
/** corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k)) */
Matrix state_post;
/** state transition matrix (A) */
Matrix transition_matrix;
/** control matrix (B) (it is not used if there is no control)*/
Matrix control_matrix;
/** measurement matrix (H) */
Matrix measurement_matrix;
/** process noise covariance matrix (Q) */
Matrix process_noise_cov;
/** measurement noise covariance matrix (R) */
Matrix measurement_noise_cov;
/** priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q) */
Matrix error_cov_pre;
/** Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R) */
Matrix gain;
/** posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k) */
Matrix error_cov_post;
/** temporary matrices */
Matrix temp1;
Matrix temp2;
Matrix temp3;
Matrix temp4;
Matrix temp5;
/**
* The construstor allocates JKalman filter and all its matrices and
* initializes them somehow.
* @param dynam_params
* @param measure_params
* @param control_params
*
* @exception IllegalArgumentException Kalman filter dimensions exception.
*/
public JKalman(int dynam_params, int measure_params, int control_params)
throws Exception {
if( dynam_params <= 0 || measure_params <= 0 ) {
throw new IllegalArgumentException("Kalman filter: Illegal dimensions.");
}
if( control_params < 0 ) {
control_params = dynam_params;
}
// init
dp = dynam_params;
mp = measure_params;
cp = control_params;
state_pre = new Matrix(dp, 1); // init by transition _matrix*state_post
// following variables must be initialized properly in advance
state_post = new Matrix(dp, 1); // init by the first measurement!!!
transition_matrix = Matrix.identity(dp, dp); // or init the matrix as:
/* double[][] tr = { {1, 0, 1, 0}, // { {1, 1}, // x
{0, 1, 0, 1}, // {0, 1} }; // dx
{0, 0, 1, 0},
{0, 0, 0, 1} };
kalman.transition_matrix = new Matrix(tr);
*/
process_noise_cov = Matrix.identity(dp, dp, 1e-3); // 1e-5 (1 in OpenCV)
measurement_matrix = Matrix.identity(mp, dp); // 1 (0 in OpenCV)
measurement_noise_cov = Matrix.identity(mp, mp, 1e-1); // 1e-1 (1 in OpenCV)
error_cov_pre = new Matrix(dp, dp); // initialized in Predict
error_cov_post = Matrix.identity(dp, dp); // 1 (0 in OpenCV)
gain = new Matrix(dp, mp);
if( cp > 0 )
{
control_matrix = new Matrix(dp, cp);
}
else { // TODO possibly an error in OpenCV
control_matrix = null;
}
temp1 = new Matrix(dp, dp);
temp2 = new Matrix(mp, dp);
temp3 = new Matrix(mp, mp);
temp4 = new Matrix(mp, dp);
temp5 = new Matrix(mp, 1);
}
/**
* Constructor in case of no control.
* @param dynam_params
* @param measure_params
*/
public JKalman(int dynam_params, int measure_params) throws Exception {
this(dynam_params, measure_params, 0);
}
/**
* Alias for prediction with no control.
* @return Predict(no control).
*/
public Matrix Predict() {
return Predict(null);
}
/**
* 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),
*
* @param control Control vector (uk), should be NULL if there
* is no external control (control_params=0).
* @return The function returns the estimated state.
*/
public Matrix Predict(Matrix control) {
// (1) Project the state ahead
// update the state: x'(k) = A*x(k)
state_pre = transition_matrix.times(state_post);
if( control != null && cp > 0 ) {
// x'(k) = x'(k) + B*u(k)
state_pre = control_matrix.gemm(control, state_pre, 1, 1);
}
// (2) Project the error covariance ahead
// update error covariance matrices: temp1 = A*P(k)
temp1 = transition_matrix.times(error_cov_post);
// P'(k) = temp1*At + Q
error_cov_pre = temp1.gemm(transition_matrix.transpose(), process_noise_cov, 1, 1);
return state_pre;
}
/**
* 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.
* @param measurement Matrix containing the measurement vector.
* @return
*/
public Matrix Correct(final Matrix measurement) {
// (1) Compute the Kalman gain
// temp2 = H*P'(k)
temp2 = measurement_matrix.times(error_cov_pre);
// temp3 = temp2*Ht + R
temp3 = temp2.gemm(measurement_matrix.transpose(), measurement_noise_cov, 1, 1);
// temp4 = inv(temp3)*temp2 = Kt(k)
temp4 = temp3.solve(temp2);
// hokus pokus...
// temp4 = temp3.svd().getU().times(temp2);
// K(k)
gain = temp4.transpose();
// (2) Update estimate with measurement z(k)
// temp5 = z(k) - H*x'(k)
temp5 = measurement_matrix.gemm(state_pre, measurement, -1, 1);
// x(k) = x'(k) + K(k)*temp5
state_post = gain.gemm(temp5, state_pre, 1, 1);
// (3) Update the error covariance.
// P(k) = P'(k) - K(k)*temp2
error_cov_post = gain.gemm(temp2, error_cov_pre, -1, 1);
return state_post;
}
/**
* Setter
* @param state_pre
*/
public void setState_pre(Matrix state_pre) {
this.state_pre = state_pre;
}
/**
* Getter
* @return
*/
public Matrix getState_pre() {
return state_pre;
}
/**
* Setter
* @param state_post
*/
public void setState_post(Matrix state_post) {
this.state_post = state_post;
}
public Matrix getState_post() {
return state_post;
}
/**
* Getter
* @param transition_matrix
*/
public void setTransition_matrix(Matrix transition_matrix) {
this.transition_matrix = transition_matrix;
}
public Matrix getTransition_matrix() {
return transition_matrix;
}
/**
* Setter
* @param control_matrix
*/
public void setControl_matrix(Matrix control_matrix) {
this.control_matrix = control_matrix;
}
/**
* Getter
* @return
*/
public Matrix getControl_matrix() {
return control_matrix;
}
/**
* Setter
* @param measurement_matrix
*/
public void setMeasurement_matrix(Matrix measurement_matrix) {
this.measurement_matrix = measurement_matrix;
}
/**
* Getter
* @return
*/
public Matrix getMeasurement_matrix() {
return measurement_matrix;
}
/**
* Setter
* @param process_noise_cov
*/
public void setProcess_noise_cov(Matrix process_noise_cov) {
this.process_noise_cov = process_noise_cov;
}
/**
* Getter
* @return
*/
public Matrix getProcess_noise_cov() {
return process_noise_cov;
}
/**
* Setter
* @param measurement_noise_cov
*/
public void setMeasurement_noise_cov(Matrix measurement_noise_cov) {
this.measurement_noise_cov = measurement_noise_cov;
}
/**
* Getter
* @return
*/
public Matrix getMeasurement_noise_cov() {
return measurement_noise_cov;
}
/**
* Setter
* @param error_cov_pre
*/
public void setError_cov_pre(Matrix error_cov_pre) {
this.error_cov_pre = error_cov_pre;
}
/**
* Getter
* @return
*/
public Matrix getError_cov_pre() {
return error_cov_pre;
}
/**
* Setter
* @param gain
*/
public void setGain(Matrix gain) {
this.gain = gain;
}
/**
* Getter
* @return
*/
public Matrix getGain() {
return gain;
}
/**
* Setter
* @param error_cov_post
*/
public void setError_cov_post(Matrix error_cov_post) {
this.error_cov_post = error_cov_post;
}
/**
* Getter
* @return
*/
public Matrix getError_cov_post() {
return error_cov_post;
}
}
Ads help maintain this website.