In Kalman Folding, Part 1,klfl we present basic, static Kalman filtering as a functional fold, highlighting the unique advantages of this form for deploying test-hardened code verbatim in harsh, mission-critical environments. The examples in that paper are all static, meaning that the states of the model do not depend on the independent variable, often physical time.
Here, we present a dynamic Kalman filter in the same, functional form. This filter can handle many dynamic, time-evolving applications including some tracking and navigation problems, and is easilly extended to nonlinear and non-Gaussian forms, the Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) respectively. Those are subjects of other papers in this Kalman-folding series. Here, we reproduce a tracking example from a well known reference, but in functional form, highlighting the advantages of that form.
In this series of papers, we use the Wolfram languagewolf because it excels at concise expression of mathematical code. All examples in these papers can be directly transcribed to any modern mainstream language that supports closures. For example, it is easy to write them in C++11 and beyond, Python, any modern Lisp, not to mention Haskell, Scala, Erlang, and OCaml. Many can be written without full closures; function pointers will suffice, so they are easy to write in C. It’s also not difficult to add extra arguments to simulate just enough closure-like support in C to write the rest of the examples in that language.
In Kalman Folding,klfl we found the following elegant formulation for the accumulator function of a fold that implements the static Kalman filter:
\begin{equation} \label{eqn:kalman-cume-definition} \text{kalmanStatic} \left( \mathbold{Z} \right) \left( \left\{ \mathbold{x}, \mathbold{P} \right\}, \left\{ \mathbold{A}, \mathbold{z} \right\} \right) = \left\{ \mathbold{x}+ \mathbold{K}\, \left( \mathbold{z}- \mathbold{A}\, \mathbold{x} \right), \mathbold{P}- \mathbold{K}\, \mathbold{D}\, \mathbold{K}^\intercal \right\} \end{equation}
\noindent where
\begin{align}
\label{eqn:kalman-gain-definition}
\mathbold{K}
&=
\mathbold{P}\,
\mathbold{A}^\intercal\,
\mathbold{D}-1
\label{eqn:kalman-denominator-definition}
\mathbold{D}
&= \mathbold{Z} +
\mathbold{A}\,
\mathbold{P}\,
\mathbold{A}^\intercal
\end{align}
\noindent and all quantities are matrices:
-
$\mathbold{z}$ is a${b}×{1}$ column vector containing one multidimensional observation -
$\mathbold{x}$ is an${n}×{1}$ column vector of model states -
$\mathbold{Z}$ is a${b}×{b}$ matrix, the covariance of observation noise -
$\mathbold{P}$ is an${n}×{n}$ matrix, the theoretical covariance of$\mathbold{x}$ -
$\mathbold{A}$ is a${b}×{n}$ matrix, the observation partials -
$\mathbold{D}$ is a${b}×{b}$ matrix, the Kalman denominator -
$\mathbold{K}$ is an${n}×{b}$ matrix, the Kalman gain
In physical or engineering applications, these quantities carry physical
dimensions of units of measure in addition to their matrix dimensions as numbers
of rows and columns.
If the physical and matrix dimensions of
\begin{equation}
\label{eqn:dimensional-breakdown}
\begin{array}{lccccr}
\left[\left[\mathbold{Z}\right]\right] &=& (&\mathcal{Z}^2 & b×{b}&)
\left[\left[\mathbold{A}\right]\right] &=& (&\mathcal{Z}/\mathcal{X} & b×{n}&) \
\left[\left[\mathbold{P}\right]\right] &=& (&\mathcal{X}^2 & n×{n}&) \
\left[\left[\mathbold{A}\,\mathbold{P}\,\mathbold{A}^\intercal\right]\right] &=& (&\mathcal{Z}^2 & b×{b}&) \
\left[\left[\mathbold{D}\right]\right] &=& (&\mathcal{Z}^2 & b×{b}&) \
\left[\left[\mathbold{P}\,\mathbold{A}^\intercal\right]\right] &=& (&\mathcal{X}\,\mathcal{Z} & n×{b}&) \
\left[\left[\mathbold{K}\right]\right] &=& (&\mathcal{X}/\mathcal{Z} & n×{b}&)
\end{array}
\end{equation}
\noindent In all examples in this paper, the observations
The function in equation \ref{eqn:kalman-cume-definition}
lambda-lifts/lmlf
In Wolfram, this function is
\begin{verbatim} kalman[Zeta_][{x_, P_}, {A_, z_}] := Module[{D, K}, D = Zeta + A.P.Transpose[A]; K = P.Transpose[A].Inverse[D]; {x2 + K.(z - A.x), P - K.D.Transpose[K]}] \end{verbatim}
For details about this filter including walkthroughs of small test cases, see the first paper in the series, Kalman Folding, Part 1.klfl In another paper in this series, Kalman Folding 3: Derivations,klde we present a full derivation of this static accumulator function.
Let us reproduce an example from Zarchan and Musoff,zarc to track the height of a falling object, with no aerodynamic drag. Handling drag requires an extended Kalman filter (EKF), subject of part five of this series,klf5 because a model with drag is nonlinear.
We will need a dynamic Kalman filter, which applies an additional, linear dynamic model to the states.
Suppose the states
\begin{equation*} {\dot{\mathbold{x}}}(t)=\mathbold{F}\,\mathbold{x}(t)+\mathbold{G}\,\mathbold{u}(t) \end{equation*}
If the physical dimensions of
We often leave off the explicit denotation of time dependence for improved readability:
\begin{equation*} {\dot{\mathbold{x}}}=\mathbold{F}\,\mathbold{x}+\mathbold{G}\,\mathbold{u} \end{equation*}
Generalize by adding random process noise
\begin{equation} \label{eqn:state-space-form} {\dot{\mathbold{x}}}= \mathbold{F}\,\mathbold{x}+ \mathbold{G}\,\mathbold{u}+ \mathbold{ξ} \end{equation}
This is standard /state-space form/stsp for
differential equations. Solving these equations is beyond the scope of
this paper, but suffice it to say that we need certain time integrals of
\begin{equation}
\label{eqn:definition-of-Phi}
\mathbold{Φ}(δ t)\stackrel{\text{\tiny def}}{}
e^{\mathbold{F}\,{\delta t}}
\mathbold{1}+
\frac{\mathbold{F} {δ t }}{1!}+
\frac{\mathbold{F}^2{δ t^2}}{2!}+
\frac{\mathbold{F}^3{δ t^3}}{3!}+
\cdots
\end{equation}
\noindent where
Like
The second integral,
\begin{equation} \label{eqn:definition-of-Gamma} \mathbold{Γ}(δ t)\stackrel{\text{\tiny def}}{=} ∫0δ t{\mathbold{Φ}(τ) ⋅ \mathbold{G}\,\textrm{d}τ } \end{equation}
\noindent The physical dimensions of
The last integral,
\begin{equation}
\label{eqn:definition-of-Xi}
\mathbold{Ξ}(δ t)\stackrel{\text{\tiny def}}{=}
∫0δ t\mathbold{Φ}(τ)⋅{
\begin{pmatrix}
0 & \cdots & 0
\vdots & \ddots & \vdots \
0 & \cdots & E\left[\mathbold{ ξ }\mathbold{ ξ } \intercal \right]
\end{pmatrix}⋅\mathbold{Φ}(τ)^\intercal\,\textrm{d}τ}
\end{equation}
\noindent The physical dimensions of
Detailed dimensional analysis of these matrices is the subject of another paper in this series.
The transitions of a state (and its covariance) from time
\begin{align}
\label{eqn:transition-of-state}
\mathbold{x}
&←
\mathbold{Φ}\,
\mathbold{x}+
\mathbold{Γ}\,
\mathbold{u}
\mathbold{P}
&←
\mathbold{Ξ}+
\mathbold{Φ}\,
\mathbold{P}\,
\mathbold{Φ}^\intercal
\end{align}
These equations appear plausible on inspection, and equation
\ref{eqn:transition-of-state} has a particularly intuitive explanation. If
\begin{align}
\mathbold{x}(t_2)&=\mathbold{Φ}(t_2-t_1)\,\mathbold{x}(t_1)
\notag
&=e\mathbold{F×(t_2-t_1)}\,e\mathbold{F\,t_1}\mathbold{x}_0=e\mathbold{F\,t_2}\mathbold{x}_0
\end{align}
\noindent This is the first step in verifying that the recurrences satisfy
equation \ref{eqn:state-space-form}. It also explains why we call
These tiny changes are all that is needed to add linear state evolution to the Kalman filter:
\begin{verbatim} kalman[Zeta_][{x_, P_}, {Xi_, Phi_, Gamma_, u_, A_, z_}] := Module[{x2, P2, D, K}, x2 = Phi.x + Gamma.u; P2 = Xi + Phi.P.Transpose[Phi]; (* after this, it’s identical to the static filter *) D = Zeta + A.P2.Transpose[A]; K = P2.Transpose[A].inv[D]; {x2 + K.(z - A.x2), P2 - K.D.Transpose[K]}]\end{verbatim}
Let
\begin{equation*} \mathbold{x} = \begin{bmatrix} { h } (t) \ \dot { h } (t) \end{bmatrix} \end{equation*}
\noindent The system dynamics are elementary:
\begin{equation*}
\begin{bmatrix} \dot { h } (t) \ \ddot { h } (t) \end{bmatrix}
=
\begin{bmatrix}
0 & 1
0 & 0
\end{bmatrix}
\begin{bmatrix} h(t) \ \dot { h } (t) \end{bmatrix}
+
\begin{bmatrix} 0 \ 1 \end{bmatrix}
\begin{bmatrix} g \end{bmatrix}
\end{equation*}
\noindent where
\begin{equation*} \begin{matrix} \mathbold{F} = \begin{bmatrix}0 & 1 \0 & 0\end{bmatrix}, & \mathbold{G} = \begin{bmatrix} 0 \ 1 \end{bmatrix}, & \mathbold{u} = \begin{bmatrix} g \end{bmatrix} \end{matrix} \end{equation*}
\noindent and their integrals from equations \ref{eqn:definition-of-Phi}, \ref{eqn:definition-of-Gamma}, and \ref{eqn:definition-of-Xi}
\begin{equation*}
\begin{matrix}
\mathbold{Φ} =
\begin{bmatrix}
1 & δ t
0 & 1
\end{bmatrix}, &
\mathbold{Γ} =
\begin{bmatrix}
{{δ t}^2}/{2} \
δ t
\end{bmatrix}, &
\mathbold{Ξ} =
E\left[\mathbold{ ξ }\mathbold{ ξ } \intercal \right]
\begin{bmatrix}
\sfrac { { δ t } 3 }{ 3 } & \sfrac { { δ t } 2 }{ 2 } \
\sfrac { { δ t } 2 }{ 2 } & δ t
\end{bmatrix}
\end{matrix}
\end{equation*}
\noindent We test this filter over a sequence of fake
observations tracking an object from an initial height of
The ground truth is
\begin{equation*} h(t) = h_0 + {\dot{h}}_0\,t + g\,t^2/2 \end{equation*}
\noindent where
\begin{equation*} \begin{matrix} h_0 = 400,000\,\textrm{ft}, & {\dot{h}}_0 = -6,000\,\textrm{ft}/\textrm{sec} \end{matrix} \end{equation*}
\noindent and we generate fake noisy observations by sampling a Gaussian
distribution of zero mean and standard deviation
It’s easy to add system dynamics to a static Kalman filter. Expressed as the accumulator function for a fold, the filter is decoupled from the environment in which it runs. We can run exactly the same code, even and especially the same binary, over arrays in memory, lazy streams, asynchronous observables, any data source that can support a fold operator. Such flexibility of deployment allows us to address the difficult issues of modeling, statistics, and numerics in friendly environments where we have large memories and powerful debugging tools, then to deploy with confidence in unfriendly, real-world environments where we have small memories, asynchronous, real-time data delivery, and seldom more than logging for forensics.
affn https://en.wikipedia.org/wiki/Affine_transformation
bars Bar-Shalom, Yaakov, et al. Estimation with applications to tracking and navigation. New York: Wiley, 2001.
bier http://tinyurl.com/h3jh4kt
bssl https://en.wikipedia.org/wiki/Bessel’s_correction
busi https://en.wikipedia.org/wiki/Business_logic
cdot We sometimes use the center dot or the
- <2017-03-01 Wed> Corrected missing linear term in equation 6.