Utilities

Corresponds to Eq. (11) from [AZH17] with added W_n.

Corresponds to Eq. (15) from [AZH17] with added L_n.

EKFPropagator <: AbstractKFPropagator

Propagation step of the extended Kalman filter.

EKFUpdater <: AbstractKFUpdater

Update step of the extended Kalman filter.

mutable struct KalmanState end

Manifold Kalman filter state.

The following deterministic dynamical system is discretized:

\[D_t y(t) = f(y(t), q(t), t) \\ z(t) = h(p(t), q(t))\]

where $y(t) \in \mathcal{M}$ is state at time t, $q(t) \in Q$ are control parameters, $f\colon \mathcal{M} × \mathcal{N} × \mathbb{R} \to T\mathbb{M}$ represents state transition function and $h \colon \mathcal{M} \times Q \to \mathcal{M}_{obs}$ is the measurement function.

Notably, when $f$ is given by an action of a Lie group on $y(t)$, i.e. $f(y, q, t) = g(y, q, t) ∘ y(t)$ for some function $gf\colon \mathcal{M} \times \mathbb{R} \to \mathbb{G}$ and an action $\circ$ of group $\mathbb{G}$ on manifold $\mathbb{M}$, the system can be solved using RKMK integrators.

Such system is a generalization of the IEKF from [BB18], where $\mathcal{M} = \mathcal{G}$ and action represents either left or right group operation action.

Discrete equations are as follows:

\[ \begin{align*} \tilde{f}(p, q_n, w_n, t_n) &= \exp_p(\Delta t f(p, q_n, w_n, t_n)) \quad & \text{discretization} \\ p_{n|n-1} &= \tilde{f}(p_{n-1|n-1}, q_n, w_n, t_n) \quad &\text{mean propagation} \\ \hat{f}(p_c, q_n, w_n, t_n) &= \tilde{f}(\phi^{-1}_p(p_c), q_n, w_n, t_n) \quad & \text{Jacobian parametrization} \\ F_n &= D_p \tilde{f}(p_{n-1|n-1}, q_{n}, w_n) \quad& \text{Jacobian of } \hat{f} \text{ wrt. } p_c \text{ at } p_{n-1|n-1} \\ L_n &= D_w \tilde{f}(p_{n-1|n-1}, q_{n}, w_n) \quad& \text{Jacobian of } \tilde{f} \text{ wrt. } w \text{ at } p_{n-1|n-1} \\ P_{n|n-1} &= F_n P_{n-1|n-1} F_n^{T} + L_n Q_n(p_{n|n-1}, q_n) L_n^{T} \quad& \text{covariance propagation} \\ P_{n|n-1} & & \text{a linear operator on }T_{p_{n|n-1}}\mathcal{M} \\ z_n & \quad& \text{actual measurement}\\ y_n &= \log_{h(p_{n|n-1}, q_n)}(z_n) \quad & \text{measurement residual} \\ H_n &= D_p h(p_{n|n-1}, q_n) \quad& \text{Jacobian of } h \text{ wrt. } p \text{ at } p_{n|n-1} \\ H_n & & \text{a linear operator from }T_{p_{n|n-1}}\mathcal{M} \text{ to } T_{h(p_{n|n-1}, q_n)} \mathcal{N} \\ R_n & \quad & \text{covariance matrix of the observation noise} S_n &= H_n P_{n|n-1} H_n^{T} + R_n \quad& \text{innovation covariance}\\ S_n & & \text{a linear operator on }T_{h(p_{n|n-1}, q_n)}\mathcal{N} \\ K_n &= P_{n|n-1} H_n^T S_n^{-1} \quad& \text{Kalman gain}\\ K_n & & \text{a linear operator from }T_{h(p_{n|n-1}, q_n)}\mathcal{N} \text{ to } T_{p_{n|n-1}} \mathcal{M} \\ p_{n|n} &= \exp_{p_{n|n-1}}(K_n y_n) \quad& \text{updated state estimate} \\ P_{n|n} &= \operatorname{PT}_{p_{n|n-1} \to p_{n|n}} P_{n|n-1} - K_n S_n K_n^T \quad& \text{updated covariance estimate} \end{align*}\]

where $p_{n|m}$ denotes estimate of $p$ at time step $n$ given samples up to and including the one at time $m$. $\operatorname{PT}$ represents transporting covaraince from propagated state to the one updated through measurement. It is sometimes known as "covariance reset", see [GGM23] and [MGH22]. $\phi_p$ is a chart around point p, $\phi_p \colon \mathcal{M} \to \mathbb{R}^k$, and $p_c = \phi_p(p)$.

Note: for linear noise, $L_n$ is the identity matrix.

Fields

• p: filter state estimate
• P: state covariance matrix coordinates
• Q: process noise
• R: measurement noise
• f_tilde: discretized transition function
• jacobian_f_tilde: Jacobian of the discretized transition function
• B: basis type in which covariance and Jacobian matrices are computed

Source: E. A. Wan and R. Van Der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373), Lake Louise, Alta., Canada: IEEE, 2000, pp. 153–158. doi: 10.1109/ASSPCC.2000.882463.

UnscentedUpdater

Update step of the unscented Kalman filter.

WanMerweSigmaPoints <: UnscentedSigmaPoints

A structure that represents the sigma points proposed by E. A. Wan and R. Van Der Merwe.

Source: E. A. Wan and R. Van Der Merwe, “The unscented Kalman filter for nonlinear estimation,” in Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No.00EX373), Lake Louise, Alta., Canada: IEEE, 2000, pp. 153–158. doi: 10.1109/ASSPCC.2000.882463.

discrete_kalman_filter_manifold(
    M::AbstractManifold,
    M_obs::AbstractManifold,
    p0,
    f_tilde,
    h,
    P0,
    Q,
    R;
    kwargs...
)

Construct a Kalman filter on manifold M.

Keyword arguments

• control_prototype=nothing: prototype of the control parameter for h and f_tilde. Can be used for constructing caches by some algorithms.

predict!(kalman::KalmanState, prop::EKFPropagator, control)

Perform prediction step of the Kalman filter represented by KalmanState kalman with EKFPropagator propagation rule.

predict!(kalman::KalmanState, prop::UnscentedPropagatorCache, control)

Perform prediction step of the Kalman filter represented by KalmanState kalman with the cached variant of the UnscentedPropagator propagation rule.

update!(kalman::KalmanState, upd::EKFUpdater, control, measurement)

Perform the update step of the Kalman filter represented by KalmanState kalman with update rule EKFUpdater.

update!(kalman::KalmanState, upd::UnscentedUpdaterCache, control, measurement)

Perform the update step of the Kalman filter represented by KalmanState kalman with the cached variant of the update rule UnscentedUpdater.

HPHT is innovation covariance without measurement noise.

get_inits(pfo::AbstractKFOParametrization, p_opt)

Get filter initialization parameters: p0, P0, Q and R, where p_opt is the set of parameters over which optimization is performed.

abstract type AbstractKFOParametrization end

Represent a nonlinear least squares fitting problem for parameters of a Kalman filter. The parameters may refer to initial conditions, noise covariance matrices, process parameters (like weight of a certain factor) or measurement parameters (like sensor bias).

The following functionality is expected:

• get_inits(pfo::AbstractKFOParametrization, p_opt)
• residuals(pfo::AbstractKFOParametrization, p_opt)