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}
wherev_{t} ~ N(0, R_{t})
x_{t} = F_{t} x_{t-1} + B_{t} u_{t} + w_{t}
wherew_{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.
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 posterior and prior 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. It is the working horse of the |