Filtering and Smoothing

predict(m, C, A, b, Q)

Predict the next state estimate for an affine Gaussian transition.

Given an affine transition model $x_{n+1} \mid x_n \sim \mathcal{N}(x_{n+1}; A x_n + b, Q)$ and a current state estimate $x_n \sim \mathcal{N} (m, C)$, predict computes the prediction estimate

\[\begin{aligned} x_{n+1} \sim \mathcal{N} (A m + b, A C A^\top + Q). \end{aligned}\]

sqrt_predict(m, CL, A, b, QL)

Predict the next state estimate for an affine Gaussian transition, in square-root form.

In principle, this function does the same as predict, but the covariances are given as and returned in square-root form. That is, the equivalent call to predict(m, C, A, b, Q) would be sqrt_predict(m, CL, A, b, QL), where C = CL * CL' and Q = QL * QL'.

update(m, C, y, H, b, R)

Update / correct the state based on the affine observation.

Given a Gaussian $x \sim \mathcal{N}(m, C)$ an affine observation model

\[\begin{aligned} y \mid x \sim \mathcal{N}(y; H x + b, R), \end{aligned}\]

and a data point $y$, update computes the posterior $x \mid y$.

sqrt_update(m, CL, y, H, b, RL)

Update / correct the state based on the affine observation, in square-root form.

In principle, this function does the same as update, but the covariances are given as and returned in square-root form. That is, the equivalent call to update(m, C, y, H, b, R) would be sqrt_update(m, CL, y, H, b, RL) where C = CL * CL' and R = RL * RL'.

smooth(mcurr, Ccurr, mnext, Cnext, A, Q)

Smoothing step to update the "current" state estimate on the "next" one.

This implementation internally calls predict to (re-)compute the prediction estimate, and then perform the backwards smoothing.

In most cases, pre-computing the backwards transitions directly via get_backward_transition and then just predicting backwards might be the preferred strategy.

get_backward_transition(m, C, mpred, Cpred, A)

Compute the affine backward transition model used for smoothing.

Returns parameters for a transition $x_n^S \mid x_{n+1}^S \sim \mathcal{N}(G x_{n+1}^S + b, Λ)$, computed with

\[\begin{aligned} G &= C * A^\top C_p^{-1} \\ b &= m - G m_p \\ Λ &= C - G C_p G^\top. \end{aligned}\]

To smooth, just predict backwards.

linearize(h::Function, m::AbstractVector)

Linearize the nonlinear function h at the location m.

Approximate h with $h(x) \approx H x + b$, where

\[\begin{aligned} H &= J_h(m), \\ b &= h(m) - H * m. \end{aligned}\]

The Jacobian is computed with automatic differentiation via ForwardDiff.jl.

ekf_update(m, C, y, h, R)

Update / correct the state based on a nonlinear observation.

This function does two things:

1. it linearizes the observation function h at the mean m, with linearize, and
2. it calls update to perform an update on the now-linear model.