# Crate linearkalman [−] [src]

This crate implements a standard linear Kalman filter and smoothing for vectors of arbitrary dimension. Implementation method relies on `rulinalg` library for linear algebra computations. Most inputs and outputs rely therefore on (derived) constructs from rulinalg library, in particular `Vector<f64>` and `Matrix<f64>` structs.

Currently, implementation method assumes that Kalman filter is time-invariant and is based on the equations detailed below. Notations in the below equations correspond to annotations in the source code.

Measurement and state equations:

• `z_{t} = H_{t} x_{t} + v_{t}` where `v_{t} ~ N(0, R_{t})`
• `x_{t} = F_{t} x_{t-1} + B_{t} u_{t} + w_{t}` where `w_{t} ~ N(0, Q_{t})`

Kalman filter equations:

• `P_{t|t-1} = F_{t} P_{t-1|t-1} F'_{t} + Q_{t}`
• `x_{t|t-1} = F_{t} x_{t-1|t-1} + B_{t} u_{t}`
• `K_{t} = P_{t|t-1} H'_{t} * (H_{t} P_{t|t-1} H'_{t} + R_{t})^{-1}`
• `P_{t|t} = (Id - K_{t} H_{t}) * P_{t|t-1}`
• `x_{t|t} = x_{t|t-1} + K_{t} * (z_{t} - H_{t} x_{t|t-1})`

Kalman smoothing equations:

• `J_{t} = P_{t|t} F'_{t} P_{t+1|t}^{-1}`
• `x_{t|T} = x_{t|t} + J_{t} * (x_{t+1|T} - x_{t+1|t})`
• `P_{t|T} = P_{t|t} + J_{t} * (P_{t+1|T} - P_{t+1|t}) * J'_{t}`

Nomenclature:

• `(x_{t+1|t}, P_{t+1|t})` will be referred to as predicted state variables.
• `(x_{t|t}, P_{t|t})` will be referred to as filtered state variables.
• `(x_{t|T}, P_{t|T})` will be referred to as smoothed state variables.

For now, it is assumed here that `B_{t}` matrix is null and that `Q_{t}, R_{t}, H_{t}` and `F_{t}` matrices are constant over time.

Besides real data, algorithm takes as inputs `H`, `R`, `F` and `Q` matrices as well as initial guesses for state mean `x0 = x_{1|0}`and covariance matrix `P0 = P_{1|0}`. Covariance matrix `P0` indicates the uncertainty related to the guess of `x0`.

## Structs

 KalmanFilter Container object with values for matrices used in the Kalman filtering procedure as well as initial values for state variables. KalmanState Container with the value of state variable and its covariance. This struct is used throughout all parts of Kalman procedure and may refer to predicted, filtered and smoothed variables depending on the context.

## Functions

 filter_step Returns a tuple containing updated and predicted estimates (in that order) of the state variable and its covariance. This function might be useful for cases where data is incoming and being updated in real-time so that Kalman filtering is run incrementally. Note that given some initial values for `x` and `P`, `filter_step` makes a prediction and then runs the update step to correct the prediction based on the observed data. predict_step Returns a predicted state variable mean and covariance. If prediction for time `t` is desired, then `KalmanState` object with initial conditions contains state mean and covariance at time `t-1` given information up to time `t-1`. update_step Returns an updated state variable mean and covariance given predicted and observed data. Typically, update step will be called after prediction step, data of which will be consequently used as input in updating.