Kalman

An extended Kalman filter. Note that this could be generalized further. If you need a Kalman filter and have a state update matrix \(A\) then you can just use const bigA as the function which returns the Jacobian.

Documentation

extKalman

Arguments

:: (KnownNat m, KnownNat n, (1 <=? n) ~ True, (1 <=? m) ~ True)

=> R n

Prior mean \(\boldsymbol{\mu}_0\)

-> Sq n

Prior variance \(\Sigma_0\)

-> L m n

Observation map \(H\)

-> Sq m

Observation noise \(R\)

-> (R n -> R n)

State update function \(\boldsymbol{a}\)

-> (R n -> Sq n)

A function which returns the Jacobian of the state update function at a given point \(\frac{\partial \boldsymbol{a}}{\partial \boldsymbol{x}}\)

-> Sq n

State noise \(Q\)

-> [R m]

List of observations \(\boldsymbol{y}_i\)

-> [(R n, Sq n)]

List of posterior means and variances \((\hat{\boldsymbol{x}}_i, \hat{\Sigma}_i)\)

The model for the extended Kalman filter is given by

\[ \begin{aligned} \boldsymbol{x}_i &= \boldsymbol{a}(\boldsymbol{x}_{i-1}) + \boldsymbol{\psi}_{i-1} \\ \boldsymbol{y}_i &= {H}\boldsymbol{x}_i + \boldsymbol{\upsilon}_i \end{aligned} \]

where

• \(\boldsymbol{a}\) is some non-linear vector-valued state update function.
• \(\boldsymbol{\psi}_{i}\) are independent identically normally distributed random variables with mean 0 representing the fact that the state update is noisy: \(\boldsymbol{\psi}_{i} \sim {\cal{N}}(0,Q)\).
• \({H}\) is a matrix which represents how we observe the hidden state process.
• \(\boldsymbol{\upsilon}_i\) are independent identically normally distributed random variables with mean 0 represent the fact that the observations are noisy: \(\boldsymbol{\upsilon}_{i} \sim {\cal{N}}(0,R)\).

We assume the whole process starts at 0 with our belief of the state (aka the prior state) being given by \(\boldsymbol{x}_0 \sim {\cal{N}}(\boldsymbol{\mu}_0, \Sigma_0)\)