lib

lib/com.irurueta.android.navigation.inertial/KalmanFilter

KalmanFilter

open class KalmanFilter : Serializable

Implementation of a Kalman filter. This class contains the state of a Kalman kilter. The state of a Kalman filter is updated by predict and correct functions.

The source code, notation and formulae below are borrowed from the JKalman tutorial [Welch95]:


x<sub>k</sub>=A*x<sub>k-1</sub>+B*u<sub>k</sub>+w<sub>k</sub>
z<sub>k</sub>=Hx<sub>k</sub>+v<sub>k</sub>,

where:

x<sub>k</sub> (x<sub>k-1</sub>)
Content copied to clipboard - state of the system at the moment k (k-1)
z<sub>k</sub>
Content copied to clipboard - measurement of the system state at the moment k
u<sub>k</sub>
Content copied to clipboard - external control applied at the moment k
w<sub>k</sub>
Content copied to clipboard
 and v<sub>k</sub>
Content copied to clipboard 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.

Constructors

constructor(dynamParams: Int, measureParams: Int, controlParams: Int)

Allocates a Kalman filter and all its matrices and initializes them.

constructor(dynamParams: Int, measureParams: Int)

Constructor in case of no control parameters.

Properties

open var controlMatrix: Matrix

Control matrix (B) (it is not used if there is no control).

DEFAULT_MEASUREMENT_NOISE_VARIANCE

val DEFAULT_MEASUREMENT_NOISE_VARIANCE: Double = 0.1

Independent measurement noise variance assumed when no measurement noise covariance matrix is provided.

DEFAULT_PROCESS_NOISE_VARIANCE

val DEFAULT_PROCESS_NOISE_VARIANCE: Double = 1.0E-6

Independent process noise variance assumed when no process noise covariance matrix is provided.

open var errorCovPost: Matrix

Posteriori error estimate covariance matrix (P(k)): P(k)=(I-K(k)*H)*P'(k)

open var errorCovPre: Matrix

Priori error estimate covariance matrix (P'(k)): P'(k)=A*P(k-1)*At + Q)

open var gain: Matrix

Kalman gain matrix (K(k)): K(k)=P'(k)*Ht*inv(H*P'(k)*Ht+R)

measurementMatrix

open var measurementMatrix: Matrix

Measurement matrix (H).

measurementNoiseCov

open var measurementNoiseCov: Matrix

Measurement noise covariance matrix (R).

processNoiseCov

open var processNoiseCov: Matrix

Process noise covariance matrix (Q).

open var statePost: Matrix

Corrected state (x(k)): x(k)=x'(k)+K(k)*(z(k)-H*x'(k))

open var statePre: Matrix

Predicted state (x'(k)): x(k)=A*x(k-1)+B*u(k)

transitionMatrix

open var transitionMatrix: Matrix

State transition matrix (A).

Functions

open fun correct(measurement: Matrix): Matrix

Adjusts model state.

getControlParameters

open fun getControlParameters(): Int

Obtains the number of control vector dimensions (control parameters).

getDynamicParameters

open fun getDynamicParameters(): Int

Obtains the number of state vector dimensions (dynamic parameters).

getMeasureParameters

open fun getMeasureParameters(): Int

Obtains the number of measurement vector dimensions (measure parameters).

open fun predict(): Matrix

Estimates subsequent model state without control parameters.

open fun predict(control: Matrix): Matrix

Estimates subsequent model state.

setMeasureParameters

open fun setMeasureParameters(measureParameters: Int)

Sets the number of measurement vector dimensions (measure parameters).