linearkalman 0.1.2

Linear Kalman filtering and smoothing
Documentation

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.