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. In Kalman Folding 2: Tracking,klf2 we reproduce a tracking example from the literature, showing that these advantages extend to time-dependent, linear models. Here, we extend that example further to include aerodynamic drag, making the model nonlinear. We must change the Kalman filter itself to handle such problems. The resulting class of filters are called Extended Kalman Filters or EKFs. Other papers in this series feature applications of EKFs to a variety of problems including navigation and pursuit.
The particular EKF designed here includes integration of non-linear equations of motion. We integrate these equations by folding over a lazy stream that generates, on demand, differential updates to the solution. Folds over lazy streams were introduced in /Kalman Folding 4: Streams and Observables/klf4 where we used them to step a static Kalman filter over observations. They also afford a constant-memory representation for solutions of differential equations, making them suitable for integration components in constant-memory filters.
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 2: Tracking,klf2 we found the following formulation for the accumulator function of a fold that implements the linear dynamic Kalman filter, that is, a filter that can track states that evolve with timetime according to a linear transformation.
\noindent where
\noindent and all quantities are matrices:
-
$\mathbold{z}$ is a${b}×{1}$ column vector containing one multidimensional observation -
$\mathbold{x}$ and $\mathbold{x}2$ are${n}×{1}$ column vectors of model states -
$\mathbold{Z}$ is a${b}×{b}$ matrix, the covariance of observation noise -
$\mathbold{P}$ and$\mathbold{P}_2$ are${n}×{n}$ matrices, the theoretical covariances of$\mathbold{x}$ and$\mathbold{x}_2$ , respectively -
$\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 -
$\mathbold{Ξ}$ is a non-dimensionalized$n×{n}$ integral of process noise$\mathbold{ξ}$ , namely \newline \(∫0δ t\mathbold{Φ}(τ)⋅{\left(\begin{array}{c|c}\mathbold{0}&\mathbold{0}\\hline\mathbold{0}&E\left[\mathbold{ξ}\mathbold{ξ}\intercal\right]\end{array}\right)⋅\mathbold{Φ}(τ)^\intercal\,\textrm{d}τ}\) -
$\mathbold{Φ}$ is a non-dimensionalized$n×{n}$ propagator for$\mathbold{F}$ , namely $e\mathbold{F\,{δ t}}$ -
$\mathbold{Γ}$ is an$n×{dim(\mathbold{u})}$ integral of system response, namely \(∫0δ t{\mathbold{Φ}(τ) ⋅ \mathbold{G}\,\textrm{d}τ}\) -
$\mathbold{u}$ is a vector of external disturbances or control inputs -
$δ{t}$ is an increment of time (or, more generally, the independent variable of the problem)
\noindent and the time-evolving states satisfy the following differential equation in state-space form:
\noindent
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. Both kinds of dimensions are aspects of the type of a quantity. Dimensional arguments, like type-arguments more generally, are invaluable for checking equations.
If the physical and matrix dimensions of
The non-dimensionalization of
\noindent In all examples in this paper, the observations
Let us reproduce Zarchan and Musoff’szarc example of tracking a falling object in one position dimension, with aerodynamic drag. Throughout this series of papers, we refer to the examples in that reference because they provide solid benchmarks against which to check our contribution. The difference between our approach and typical presentations of Kalman-type filters is functional decomposition: writing code in functional style affords the ability to deploy it verbatim, even and especially at the binary level, in both laboratory and field. This ability can make the difference in a successful application because seemingly insignificant changes, even instruction order, can make qualitative differences in filter behavior due to floating-point issues.
To accommodate nonlinearity, we replace equation
\ref{eqn:state-propagation-equation} for time-propagation of the state
To establish a benchmark solution, we solve the differential equations of motion using Wolfram’s built-in numerical integrator. We then introduce our own Euler and Runge-Kutta integrators and show they produce similar results when folded over lazy streams. These integrators do not use special features of the Wolfram language, so they are easy to implement in other languages.
Let
Omitting, for clarity’s sake, explicit dependence of
\noindent where
-
$g$ is the acceleration of Earth’s gravitation, about$32.2\,\textrm{ft}/{\textrm{s}}^2$ -
$ρ(h)$ is the density of airzerr in$\textrm{slug}/{\textrm{ft}}^2$ ;$ρ\,{{\dot{h}}^2}$ has units of pressure, that is,$\textrm{slug}/(\textrm{ft}⋅{\textrm{sec}^2})$ -
$β = 500\,\textrm{slug}/(\textrm{ft}⋅{\textrm{sec}^2})$ is a constant ballistic coefficient of the object in units of pressure (it is possible to estimate this coefficient in the filter; here, we treat it as a known constant).
The positive direction is up and we are only concerned with negative velocities
where the object is approaching the ground. We may provisionally replace the
factor
In scalar form, the equations are
\noindent or
\noindent
with
We integrate these equations for thirty seconds
with the initial conditions NDSolve, the numerical
integrator for differential equations, as follows:
\noindent producing the results in figure fig:ndsolve-falling-with-drag-results. These results are indistinguishable from those in the reference.
We can write the same differential equation as a lazy stream, which uses only
constant memory. Thus, it is suitable for the internals of a Kalman filter. We
implement the integrator as an accumulator function for a foldStream from
paper 3klf3 which produces all intermediate results as a new stream:
The simplest integrator is the Euler integrator, which updates a state with its derivative times a small interval of time:
This is a binary function that takes two compound arguments. The first is an
instance of the accumulation type: a pair of a time t and a (usually compound)
state x. The second is an element of the input stream, a triple of a time
differential dt, the same time t that appears in the first argument, and a
function Dx that computes the derivative of the state given the state and the
time as Dx[x,t].
Folding this integrator over the streamed differential equation produces a
streamed solution. The input stream must produce elements of the form
{dt, t, Dx} and, like all streams, contain a thunk that produces the rest of the
stream.thnk
This bit contains nothing specific to our example, but just threads around the
integration inputs and increments time. It could be much more rich,
manipulating dt and Dx for speed or numerics (adaptive integration).
The kernel of our differential equation is the derivative function Dx, which,
for our example, is
\noindent Integrating the differential equation for thirty seconds looks like this:
The type of the result, here, is a lazy stream produced by takeUntil from the
lazy stream produced by foldStream. Because these streams are lazy, nothing
happens until we demand values for, say, plotting. The results are
indistinguishable from those in figure fig:ndsolve-falling-with-drag-results.
The arguments of takeUntil are a stream and a predicate. The result is a new
stream that pulls values from the original stream, applying the predicate until
it produces True. At that point, the rest of the stream returned by
takeUntil is empty, represented by invocation of the null thunk, Null[],
The implementation of takeUntil is in three overloads:
Given an empty stream and any predicate, produce the empty stream:
Given a stream containing a value v and a tail thunk, return the empty
stream if the predicate evaluates to True:
Otherwise, recurse by invoking the thunk in the stream:
The point of this style of integration is that we can change three aspects of the integration independently of one another, leaving the others verbatim, without even recompilation, because we have un-nested and /decomplected/hick these aspects:
- the integrator
- sophisticated adaptive treatments of the time increment
dtand derivative functionDx - the internals of the derivative function
Dx
For example, should Euler integration prove inadequate, we can easily substitute second- or fourth-order Runge-Kutta integrators. The only requirement is that an integrator must match the integrator’s functional interface:
Decomplecting these bits also makes them easier to review and verify by hand because dependencies are lexically localized, making expressions smaller, easier to memorize and to find on a page.
For gains and covariances, we need the best linear approximation of the
equations of motion so that we have an expression that structurally resembles equation
\ref{eqn:state-space-form}. When there are no disturbances,
\noindent
Thus, with
We need $\mathbold{Φ}=e\mathbold{Ft}$ to propagate solutions forward,
because, if
\noindent First-order expansions turn out to be enough, so
we take
We compute the gains and covariances as in equations \ref{eqn:covariance-propagation-equation}, \ref{eqn:kalman-gain-definition}, and \ref{eqn:kalman-denominator-definition}:
\noindent where
\noindent with matrix element $\mathbold{F}22$ evaluated at the current state
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