no.kartverket.positionorientation

Class KalmanFilter

java.lang.Object

no.kartverket.positionorientation.KalmanFilter

public class KalmanFilter
extends java.lang.Object

Simple implementation for a Kalman filter.

The code is based on the kalman filter example code in the ejml manual

Nested Class Summary

Nested Classes

Modifier and Type	Class and Description
static interface	KalmanFilter.PredictionMatrixUpdater Set model and covariance matrixes for the next Kalman filter prediction step

Field Summary

Fields

Modifier and Type	Field and Description
private org.ejml.equation.Equation	eq
private org.ejml.data.DMatrixRMaj	F
private int	numParameters
private org.ejml.data.DMatrixRMaj	P
private org.ejml.equation.Sequence	predictP
private org.ejml.equation.Sequence	predictX
private org.ejml.data.DMatrixRMaj	Q
private org.ejml.equation.Sequence	updateK
private org.ejml.equation.Sequence	updateP
private org.ejml.equation.Sequence	updateX
private org.ejml.equation.Sequence	updateY
private org.ejml.data.DMatrixRMaj	x

Constructor Summary

Constructors

Constructor and Description
KalmanFilter(int numParameters)

Method Summary

Modifier and Type	Method and Description
org.ejml.data.DMatrixRMaj	getP() Get parameter covariance matrix
org.ejml.data.DMatrixRMaj	getX() Get parameter vector
void	initialize(double[] x_init, double[] P_init) Initialize state
void	predict(KalmanFilter.PredictionMatrixUpdater updater) Prediction step
void	update(org.ejml.data.DMatrixRMaj H, org.ejml.data.DMatrixRMaj z, org.ejml.data.DMatrixRMaj R) Update with new observations

Methods inherited from class java.lang.Object

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

Field Detail

numParameters

private int numParameters

x

private org.ejml.data.DMatrixRMaj x

P

private org.ejml.data.DMatrixRMaj P

Q

private org.ejml.data.DMatrixRMaj Q

F

private org.ejml.data.DMatrixRMaj F

eq

private final org.ejml.equation.Equation eq

predictX

private org.ejml.equation.Sequence predictX

predictP

private org.ejml.equation.Sequence predictP

updateY

private org.ejml.equation.Sequence updateY

updateK

private org.ejml.equation.Sequence updateK

updateX

private org.ejml.equation.Sequence updateX

updateP

private org.ejml.equation.Sequence updateP

Constructor Detail

KalmanFilter

public KalmanFilter(int numParameters)

Method Detail

initialize

public void initialize(double[] x_init,
                       double[] P_init)

Initialize state

Parameters:

x_init - initial parameters

P_init - initial parameter covariances (diagonal elements of covariance matrix)

getX

public org.ejml.data.DMatrixRMaj getX()

Get parameter vector

Returns:

parameter vector

getP

public org.ejml.data.DMatrixRMaj getP()

Get parameter covariance matrix

Returns:

parameter covariance matrix

predict

public void predict(KalmanFilter.PredictionMatrixUpdater updater)

Prediction step

Parameters:

updater - Set parameter transition model matrix and covariance matrix

update

public void update(org.ejml.data.DMatrixRMaj H,
                   org.ejml.data.DMatrixRMaj z,
                   org.ejml.data.DMatrixRMaj R)

Update with new observations

Parameters:

H - observation model matrix

z - vector of observations

R - covariance matrix of observation vector