kalman-folding-03-008-derivations.tex

% Created 2016-09-09 Fri 21:03
\documentclass[10pt,oneside,x11names]{article}
\usepackage[utf8]{inputenc}
\usepackage[T1]{fontenc}
\usepackage{fixltx2e}
\usepackage{graphicx}
\usepackage{grffile}
\usepackage{longtable}
\usepackage{wrapfig}
\usepackage{rotating}
\usepackage[normalem]{ulem}
\usepackage{amsmath}
\usepackage{textcomp}
\usepackage{amssymb}
\usepackage{capt-of}
\usepackage{hyperref}
\usepackage{geometry}
\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{amsfonts}
\usepackage{palatino}
\usepackage{siunitx}
\usepackage{esdiff}
\usepackage{xfrac}
\usepackage{nicefrac}
\usepackage{faktor}
\usepackage[euler-digits,euler-hat-accent]{eulervm}
\author{Brian Beckman}
\date{\textit{<2016-05-03 Tue>}}
\title{Kalman Folding 3: Derivations (Review Draft)\\\medskip
\large Extracting Models from Data, One Observation at a Time}
\hypersetup{
 pdfauthor={Brian Beckman},
 pdftitle={Kalman Folding 3: Derivations (Review Draft)},
 pdfkeywords={},
 pdfsubject={},
 pdfcreator={Emacs 24.5.1 (Org mode 8.3.4)}, 
 pdflang={English}}
\begin{document}

\maketitle
\setcounter{tocdepth}{3}
\tableofcontents


\section{Abstract}
\label{sec:orgheadline1}

In \emph{Kalman Folding, Part 1},\footnote{B. Beckman, \emph{Kalman Folding, Part 1}, \url{http://vixra.org/abs/1606.0328}.} 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 mathematical derivations of the basic, static filter. These are
semi-formal sketches that leave many details to the reader, but highlight all
important points that must be rigorously proved. These derivations have several
novel arguments and we strive for much higher clarity and simplicity than is
found in most treatments of the topic.

\section{Kalman Folding}
\label{sec:orgheadline2}

In \emph{Kalman Folding, Part 1},\footnotemark[1]{} we found the following small 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:

\begin{itemize}
\item \(\mathbold{z}\) is a  \({b}\times{1}\) column vector containing one multidimensional observation
\item \(\mathbold{x}\) is an \({n}\times{1}\) column vector of \emph{model states}
\item \(\mathbold{Z}\) is a  \({b}\times{b}\) matrix, the covariance of
observation noise
\item \(\mathbold{P}\) is an \({n}\times{n}\) matrix, the theoretical
covariance of \(\mathbold{x}\)
\item \(\mathbold{A}\) is a  \({b}\times{n}\) matrix, the \emph{observation partials}
\item \(\mathbold{D}\) is a  \({b}\times{b}\) matrix, the Kalman denominator
\item \(\mathbold{K}\) is an \({n}\times{b}\) matrix, the Kalman gain
\end{itemize}

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 
\(\mathbold{x}\) 
are
\(\left[\left[\mathbold{x}\right]\right]
\stackrel{\text{\tiny def}}{=}
(\mathcal{X}, n\times{1})\)
and of 
\(\mathbold{z}\) 
are
\(\left[\left[\mathbold{z}\right]\right]
\stackrel{\text{\tiny def}}{=}
(\mathcal{Z}, b\times{1})\), then

\begin{equation}
\label{eqn:dimensional-breakdown}
\begin{array}{lccccr}
\left[\left[\mathbold{Z}\right]\right]                                       &=& (&\mathcal{Z}^2            & b\times{b}&) \\
\left[\left[\mathbold{A}\right]\right]                                       &=& (&\mathcal{Z}/\mathcal{X}  & b\times{n}&) \\
\left[\left[\mathbold{P}\right]\right]                                       &=& (&\mathcal{X}^2            & n\times{n}&) \\
\left[\left[\mathbold{A}\,\mathbold{P}\,\mathbold{A}^\intercal\right]\right] &=& (&\mathcal{Z}^2            & b\times{b}&) \\
\left[\left[\mathbold{D}\right]\right]                                       &=& (&\mathcal{Z}^2            & b\times{b}&) \\
\left[\left[\mathbold{P}\,\mathbold{A}^\intercal\right]\right]               &=& (&\mathcal{X}\,\mathcal{Z} & n\times{b}&) \\
\left[\left[\mathbold{K}\right]\right]                                       &=& (&\mathcal{X}/\mathcal{Z}  & n\times{b}&)
\end{array}
\end{equation}

Dimensional arguments, regarding both matrix dimensions and physical dimensions,
are invaluable for checking the derivations that follow.

\section{Derivations}
\label{sec:orgheadline17}

Below, we derive equations \ref{eqn:kalman-cume-definition},
\ref{eqn:kalman-gain-definition} and \ref{eqn:kalman-denominator-definition}.
Again, these derivations are just sketches designed for clarity as opposed to
rigorous proofs.
These derivations only cover the
static Kalman filter, where \(\mathbold{x}\) are
fixed, constant, static states of the model. See Bar-Shalom\footnote{Bar-Shalom, Yaakov, \emph{et al}. Estimation with applications to tracking and navigation. New York: Wiley, 2001.} for
derivations of the Kalman filter with time-dependent states and part 2 of this series\footnote{B. Beckman, \emph{Kalman Folding 2: Tracking and System Dynamics}, \url{http://vixra.org/abs/1606.0348}.} for
an example.

The plan is first to develop expressions for the prior estimate
\(\tilde{\mathbold{x}}\) and prior covariance \(\tilde{\mathbold{P}}\), and then expressions
for the posterior versions \(\hat{\mathbold{x}}\) and \(\hat{\mathbold{P}}\),
defining the Kalman gain \(\mathbold{K}\) matrix and the denominator matrix
\(\mathbold{D}\) along the way. Finally, we derive the particular, convenient expressions for \(\mathbold{K}\)
and \(\mathbold{D}\) that appear in equations \ref{eqn:kalman-cume-definition},
\ref{eqn:kalman-gain-definition}, and \ref{eqn:kalman-denominator-definition}.
Bierman laid out this strategy for the derivation in his classic book
\emph{Factorization Methods for Discrete Sequential Estimation}.\footnote{\url{http://tinyurl.com/h3jh4kt}} We follow
his plan, unpacking many of his elided steps for greater clarity.

\subsection{Notation}
\label{sec:orgheadline4}

The word \emph{vector} alone means \emph{column vector} by default. If a quantity is a row
vector, we explicitly say so. In general, lower-case boldface symbols like
\(\mathbold{x}\) denote column vectors. Row vectors include a superscript
\emph{transpose} symbol, as in \(\mathbold{a}^\intercal\). We write literal vectors in
square brackets, as in \(\left[a, b, \ldots\right]^\intercal\) for a column vector
or \(\left[a, b, \ldots\right]\) for a row vector or for cases where we don't care
whether it's a column or row.


Upper-case
boldface symbols like \(\mathbold{M}\) denote matrices. Our matrices are not
always square. Because vectors are
special cases of matrices, some matrices are also vectors. We may use an
upper-case symbol to denote a vector, but we do not use a lower-case symbol to
denote a non-vector matrix.

Juxtaposition, as in
\(\mathbold{A}\,\mathbold{x}\) or \(\mathbold{A}\,\mathbold{B}\), means matrix
multiplication. 
When we write a product like
\(\mathbold{A}\,\mathbold{B}\), we assume that the number of columns of
\(\mathbold{A}\) matches the number of rows of \(\mathbold{B}\). 

Matrix multiplication is non-commutative, meaning that
\(\mathbold{A}\,\mathbold{B}\) does not, in general, equal
\(\mathbold{B}\,\mathbold{A}\). However, if a matrix \(\mathbold{D}\) is square and
diagonal,
meaning that it has non-zero entries only along its main diagonal from upper
left to lower right, then \(\mathbold{A}\,\mathbold{D}\) does always equal
\(\mathbold{D}\,\mathbold{A}\). We may freely use this fact without mentioning it
explicitly.

Symmetric matrices do not always
commute, even with other symmetric matrices. In particular, the product of two
symmetric matrices is not always symmetric, as witnessed by the following
counterexample:

\begin{equation*}
\left(
\begin{array}{cc}
 1 & 2 \\
 2 & 3 \\
\end{array}
\right)\cdot\left(
\begin{array}{cc}
 4 & 5 \\
 5 & 6 \\
\end{array}
\right)
=
\left(
\begin{array}{cc}
 14 & 17 \\
 23 & 28 \\
\end{array}
\right)
\end{equation*}


Matrix multiplication is associative, meaning that the order in which pairwise
multiplications is carried out does not matter. Thus 

\[(\mathbold{A}\,\mathbold{B})\,\mathbold{C}=\mathbold{A}\,(\mathbold{B}\,\mathbold{C})=\mathbold{A}\,\mathbold{B}\,\mathbold{C}\]

\noindent and we don't need to write parentheses. That means
some expressions of long products can be hard to read. We occasionally use
a center dot or \(\times\) symbol to make multiplication easier to read, as in
\(\mathbold{A}\cdot\mathbold{x}\) or \(\mathbold{A}\times\mathbold{x}\). We also use
the \(\times\) symbol when discussing the numbers of rows and columns of a matrix,
as in ``\(\mathbold{A}\) is an \(m\times n\) matrix,'' meaning that \(\mathbold{A}\)
has \(m\) rows and \(n\) columns.


We may freely  exploit the following facts without mentioning them explicitly:
\begin{itemize}
\item For any matrix \(\mathbold{M}\), \(\left(\mathbold{M}^\intercal\right)^\intercal = \mathbold{M}\)
\item For any invertible matrix \(\mathbold{M}\), \(\left(\mathbold{M}^{-1}\right)^{-1} = \mathbold{M}\)
\item For any two matrices \(\mathbold{A}\) and
\(\mathbold{B}\),
\(\left(\mathbold{A}\,\mathbold{B}\right)^\intercal=\mathbold{B}^\intercal\mathbold{A}^\intercal\)
\item \(\left(\mathbold{A}\,\mathbold{B}\right)^{-1}=\mathbold{B}^{-1}\mathbold{A}^{-1}\)
  when the matrices are invertible
\item \(\mathbold{P}^\intercal\) = \(\mathbold{P}\) if and only if \(\mathbold{P}\) is
symmetric
\end{itemize}

For any matrix \(\mathbold{M}\), \(\mathbold{M}^2\) means
\(\mathbold{M}^\intercal\mathbold{M}\), the transpose of the matrix times the
matrix. Such squared matrices are always square and symmetric.
This notation pertains to vectors, as well, because they are just
special cases of matrices. Thus,
\(\mathbold{x}^2=\mathbold{x}^\intercal\mathbold{x}\), the square of the Euclidean
\(\mbox{2-\textrm{norm}}\) of \(\mathbold{x}\), a scalar; and
\((\mathbold{x}^\intercal)^2 =
(\mathbold{x}^\intercal)^\intercal\cdot
\mathbold{x}^\intercal=
\mathbold{x}\,\mathbold{x}^\intercal\)
is the outer product of \(\mathbold{x}\) with itself; that outer product is an
\(n\times{n}\) square, symmetric matrix, where \(n\) is the dimensionality of \(\mathbold{x}\). 



When \(\mathbold{M}^2\) is invertible, \(\mathbold{M}^{-2}\)
means the inverse of \(\mathbold{M}^2\), namely
\(\left(\mathbold{M}^\intercal\mathbold{M}\right)^{-1}\).

We use the term \emph{tall} to mean a matrix with more rows than columns, that is, an
\(m\times{n}\)
matrix when
\(m>n\). When discussing
\(m\times{n}\)
matrices, we  usually assume that
\(m>n\).
We use the term \emph{wide} to mean a matrix with
more columns than rows, as in an \(n\times{m}\) matrix. We use the term \emph{small} to
mean \(n\times{n}\), and \emph{large} to mean \(m\times{m}\). 

\subsubsection{Probability and Statistics}
\label{sec:orgheadline3}

We use the terms \emph{distribution} and \emph{expectation value} without definition in
this paper. If \(\mathbold{x}\) is a random variable, then we denote the
expectation value of some function \(f\) of \(\mathbold{x}\) as \(E[f(\mathbold{x})]\).

\subsection{Definitions}
\label{sec:orgheadline5}

\begin{description}
\item[{\(t\)}] is the independent variable. In many applications, \(t\) represents physical
time, or an integer index mapped to physical time. It is known and
non-random. We treat it as a scalar, here, though it is possible to extend
the theory to a vector \(t\).

\item[{\(\mathbold{x}\)}] is the (column) vector of \(n\) unknown, constant \emph{states}
of the model. It's a random variable, and we compute estimates and
covariances \emph{via} expectation values over its distribution. This symbol
also means an algebraic variable standing for some particular estimate of
the states.

\item[{\(\mathbold{A}\,\mathbold{x}\)}] is the \emph{model}; it predicts an observation at
time \(t\) given an estimate of the states \(\mathbold{x}\) and a current
partials matrix \(\mathbold{A}\) that may depend on \(t\). The model is a
column vector of dimensionality \(b\times{1}\), the same as the dimensionality of an
observation \(\mathbold{z}\).

\item[{\(\mathbold{A}\)}] is the \emph{current partials matrix}, the partial derivative of
the model with respect to the unknown states \(\mathbold{x}\), evaluated
at the current value of the independent variable \(t\). We could write
\(\mathbold{A}\) as \(\mathbold{A}(t)\); it's an
aesthetic judgment to omit explicit \(t\) dependence because it
would make the derivations longer and harder to read.  Because the
model is \emph{linear}, the partials do not depend on \(\mathbold{x}\). 
\(\mathbold{A}\) is known, non-random, and may depend on \(t\). Generally,
its dimensionality
is \(b\times{n}\), where \(b\) is the dimensionality of an 
observation \(\mathbold{z}\).

\item[{\(\tilde{\mathbold{A}}\)}] is the \emph{prior partials matrix}, a matrix that stacks
all the prior rows of \(\mathbold{A}\) that precede the current row. It is
known, non-random, and \(m b\times{n}\), where \(m\) is the number of prior
observations, \(b\) is the dimensionality of a single
observation \(\mathbold{z}\), and \(n\) is the dimensionality of the states
\(\mathbold{x}\).  Thus
\(\tilde{\mathbold{A}}\) is tall in the typical \emph{overdetermined} case where
\(m>n\), more observations than states. We do not actually
realize \(\tilde{\mathbold{A}}\) in computer memory because Kalman keeps
\emph{all information} in the running covariance matrix. \(\tilde{\mathbold{A}}\)
is just a
useful abstraction for the derivations below.

\item[{\(\mathbold{z}\)}] is the \emph{current observation}. It is known and non-random.
Its dimensionality is \(b\times{1}\).

\item[{\(\tilde{\mathbold{z}}\)}] is a stack of all prior observations. It
is known, non-random, \(m b\times{1}\). It's a useful abstraction in the
derivations below. It's not necessary to actually realize it in computer
memory because we use all its information incrementally by folding.

\item[{\({\tilde{\mathbold{x}}}\)}] the \emph{prior estimate}, the estimate of
\(\mathbold{x}\) given all information we have prior to the current
observation. It is known, non-random, \(n\times{1}\).

\item[{\({\hat{\mathbold{x}}}\)}] the \emph{posterior estimate}, the estimate of
\(\mathbold{x}\) given (1) the prior estimate \({\tilde{\mathbold{x}}}\), (2)
the current partials \(\mathbold{A}\), and (3) the current observation
\(\mathbold{z}\). It is known, non-random, \(n\times{1}\). It satisfies
\emph{the Kalman update equation}:
\end{description}

\begin{equation}
\label{eqn:kalman-update-equation}
{\hat{\mathbold{x}}} =
{\tilde{\mathbold{x}}} +
\mathbold{K}
\left(
\mathbold{z}-
\mathbold{A}\,
{\tilde{\mathbold{x}}}
\right)
\end{equation}

\noindent which is equivalent to  the recurrence
\(\mathbold{x}\leftarrow\mathbold{x}+\mathbold{K}\,(z-\mathbold{A}\,\mathbold{x})\)
used in part 1 of this series.

\begin{description}
\item[{\({\tilde{\mathbold{P}}}\)}] \emph{covariance of the priors}, equals
\({\tilde{\mathbold{A}}}^{-2}\)
(de-dimensionalized; proof sketch
below). This is called just \(\mathbold{P}\) in part one of this series.
It is known, non-random, \(n\times{n}\).

\item[{\({\hat{\mathbold{P}}}\)}] \emph{posterior covariance}, satisfies
\({\hat{\mathbold{P}}}\,
     {\mathbold{A}}^\intercal=
     \mathbold{K}=
     {\tilde{\mathbold{P}}}\,\mathbold{A}^\intercal\,\mathbold{D}^{-1}\)
(de-dimensionalized; proof sketch below). We calculate it from the prior covariance
\(\tilde{\mathbold{P}}\) and the new
partials matrix \(\mathbold{A}\). 
It is known, non-random, \(n\times{n}\).

\item[{\(\mathbold{A}\,{\tilde{\mathbold{x}}}\)}] the \emph{predicted observation} given
the prior estimate \({\tilde{\mathbold{x}}}\) and the current partials matrix
\(\mathbold{A}\). It is a particular evaluation of the model. It is known,
non-random, \(b\times{1}\).

\item[{\(\mathbold{z}-\mathbold{A}\,{\tilde{\mathbold{x}}}\)}] the measurement
\emph{residual}, the difference between the current observation \(\mathbold{z}\) and the
predicted observation \(\mathbold{A}\,{\tilde{\mathbold{x}}}\).

\item[{\(\mathbold{\zeta}\)}] \emph{observation noise}: random column-vector with
zero mean and covariance \(\mathbold{Z}\) (unity, \(\mathbold{1}\), after
de-dimensionalization).
It has \(b\) rows and \(1\) column, like \(\mathbold{z}\).

\item[{\(\mathbold{Z}\)}] covariance of the observation noise, \(E
     \left[
     \mathbold{\zeta}\,
     \mathbold{\zeta}^\intercal
     \right]\): known, non-random \(b\times{b}\).

\item[{\(\tilde{\mathbold{z}} = \tilde{\mathbold{A}}\,{\mathbold{x}} + \mathbold{\zeta}\)}] the
\emph{observation equation}, which equates \(\tilde{\mathbold{z}}\), the stack of
all prior observations, to the product of \(\tilde{\mathbold{A}}\), the stack
of all prior partials matrices, and an unknown random vector of states,
\(\mathbold{x}\), plus some unknown random observation noise
\(\mathbold{\zeta}\).  The stack of prior observations
\(\tilde{\mathbold{z}}\) is known, non-random, \(m b\times{1}\); the stack of prior
partials matrices
\(\tilde{\mathbold{A}}\) is known, non-random, \(m b\times{n}\); the state vector \({\mathbold{x}}\)
is unknown, random, \(n\times{1}\); The noise vector \(\mathbold{\zeta}\) is unknown, random,
\(m b\times{1}\). The observation equation looks similar to the expression for the residual
above. It's worthwhile to take a little time to examine the notations carefully and make sure
that you have a good mental picture of the meanings of these notations. The
observation equation looks tall in the typical, overdetermined case, where
as the residual is usually equivalent to a scalar expression.

\item[{\(\mathbold{K}\)}] \emph{Kalman gain}
\(=
     {\tilde{\mathbold{P}}}\,
     \mathbold{A}^\intercal\,
     {\mathbold{D}}^{-1}\) (proof
sketch below).
Non-random, \(n\times{b}\).

\item[{\(\mathbold{D}\)}] \emph{Kalman denominator}
\(=
     \mathbold{Z}+
     \mathbold{A}\,
     {\tilde{\mathbold{P}}}\,
     \mathbold{A}^\intercal\),
or 
\(\mathbold{1}+
     \mathbold{A}\,
     {\tilde{\mathbold{P}}}\,
     \mathbold{A}^\intercal\)
de-dimensionalized.
(proof sketch below). Non-random, \(b\times{b}\).
\end{description}

\subsection{Demonstration that Prior Covariance \({\tilde{\mathbold{P}}} = \tilde{\mathbold{A}}^{-2}\)}
\label{sec:orgheadline11}

The fact that the prior covariance, \(\tilde{\mathbold{P}}\), equals the
the inverse square of
the stack of prior partials matrices (de-dimensionalized), \(\tilde{\mathbold{A}}^{-2}\), is the secret
to Kalman's efficient, in fact constant, use of computer memory. The stack of
prior partials matrices \(\tilde{\mathbold{A}}\) can be very tall and impractical
to store. But its square, \(\tilde{\mathbold{A}}^{2}\) is only \(n\times{n}\), and
its inverse square is also just \(n\times{n}\). Kalman packs all statistical
information about the model into this small matrix of constant size, and
incrementally improves the statistics as observations accumulate, without
increasing the size of the matrix, and thus without increasing the amount of
computer memory needed to keep all important information. The Kalman filter is
\emph{optimal}, meaning that the small covariance matrices keep all available
information. No other method would be able to squeeze more information out of
the observations and the model --- at least when the noise is Gaussian. A
rigorous optimality proof is out of scope for this paper, but the least-squares
derivation below contains the central idea: Kalman tracks the estimate and
covariance that minimize the sum of squared residuals. Kalman is optimal in the
sense that no other method would find a smaller sum of squared residuals.


\subsubsection{Covariance of Any Random Vector Variable}
\label{sec:orgheadline6}

The covariance of any random column vector \(\mathbold{y}\) is defined as the
expectation value
\(E
\left[
\mathbold{y}\,
\mathbold{y}^\intercal
\right]
=
E
\left[
({\mathbold{y}^\intercal})^2
\right]\)
\noindent This is the expectation value of an outer product of a column vector
\(\mathbold{y}\) and its transpose, \(\mathbold{y}^\intercal\). Therefore, it is a
\(q\times{q}\) matrix, where \(q\times{1}\) is the dimensionality of \(\mathbold{y}\).

\subsubsection{Prior Estimate \({\tilde{\mathbold{x}}}\)}
\label{sec:orgheadline7}

One of our random variables is \(\mathbold{x}\), the column \mbox{$n$-vector} of unknown
states. To calculate its estimate, assume we know the values of all \(m\) past
partials \({\tilde{\mathbold{A}}}\) (tall, \(m b\times{n}\)) and observations
\(\tilde{\mathbold{z}}\) (tall, \(m b\times{1}\)).

Relate \(\mathbold{x}\) to the known observations \({\tilde{\mathbold{z}}}\) and the known
partials \({\tilde{\mathbold{A}}}\) through the normally distributed random noise column
vector \(\mathbold{\zeta}\) and the \emph{observation equation}:

\begin{equation}
\label{eqn:observation-equation}
{\tilde{\mathbold{z}}}={\tilde{\mathbold{A}}}\,\mathbold{x}+\mathbold{\zeta}
\end{equation}

\subsubsection{Sum of Squared Residuals}
\label{sec:orgheadline8}

Consider the
following \emph{performance functional}, computed over the population of
\(\mathbold{x}\).

\begin{equation*}
J(\mathbold{x})
\stackrel{\text{\tiny def}}{=}
\zeta^2=
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^2=
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^\intercal
\cdot
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
\end{equation*}

\noindent \(J(\mathbold{x})\) is a scalar: the sum of squared residuals. A
\emph{residual} is a difference between an actual observation \(\mathbold{z}\) and a
predicted observation \(\mathbold{A}\,\mathbold{x}\). An \emph{actual observation}
\(\mathbold{z}\) is a known,
concrete \mbox{$b$-vector} of numbers, and the partials matrix \(\mathbold{A}\)
is a known, concrete \mbox{$(b\times n)$-matrix} of numbers
corresponding to that observation. The observation equation

\begin{itemize}
\item stacks all prior observations (known, concrete numbers) into
\(\tilde{\mathbold{z}}\)
\item stacks all prior 
values of the partials matrix \(\mathbold{A}\) into \(\tilde{\mathbold{A}}\) (known,
concrete numbers)
\item multiplies by the unknown random state estimate
\(\mathbold{x}\) to get the (unknown, random)
predicted observations \({\tilde{\mathbold{A}}}\,\mathbold{x}\)
\item finally adds some
unknown random noise \(\mathbold{\zeta}\) (column vector of height \(m b\))
\end{itemize}

The performance functional collapses all that
information into a scalar random variable \(J(\mathbold{x})\) with the same (Gaussian) distribution
as the noise \(\mathbold{\zeta}\). Recall that any \emph{random variable} is, in fact,
always a
function, even if only the identity function, as when we say that \(\mathbold{x}\)
is a random variable. This is the standard nomenclature of probability and
statistics established by Kolmogorov, and it admittedly can be confusing.

The job of finding the optimal estimate of the state vector \(\mathbold{x}\) is
the job of finding the concrete, numerical value of \(\mathbold{x}\) that minimizes the
performance functional \(J(\mathbold{x})\), which depends on all the known,
non-random, concrete numbers in \(\tilde{\mathbold{z}}\) and \(\tilde{\mathbold{A}}\).

To
find the \(\mathbold{x}\) that minimizes \(J(\mathbold{x})\), we could take the
classic, school approach of setting to zero the partial derivatives of
\(J(\mathbold{x})\) with respect to \(\mathbold{x}\) and solving the resulting
equations for \(\mathbold{x}\). The following is an easier way. Multiply the
residuals across by the wide matrix \({\tilde{\mathbold{A}}}^\intercal\):

\begin{equation*}
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}} - 
{\tilde{\mathbold{A}}}^2\,
\mathbold{x}
\end{equation*}

\noindent producing an \mbox{$n$-vector}, and then construct a
modified performance functional:

\begin{equation*}
J'(\mathbold{x})
\stackrel{\text{\tiny def}}{=}
\left(
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}} -
{\tilde{\mathbold{A}}}^2\,
\mathbold{x}
\right)^2
=
\left(
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}} -
{\tilde{\mathbold{A}}}^2\,
\mathbold{x}
\right)^\intercal
\cdot
\left(
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}} -
{\tilde{\mathbold{A}}}^2\,
\mathbold{x}\right)
\end{equation*}

\noindent \(J(\mathbold{x})\) is minimum with respect to \(\mathbold{x}\) if and
only if (iff) \(J'(\mathbold{x})\) is minimum (this assertion needs a rigorous
proof; as warned, we present only sketches in this paper). Because
\(J'(\mathbold{x})\) is non-negative, when \(J'(\mathbold{x})\) \emph{can} be zero, its
minimum \emph{must} be zero. \(J'(\mathbold{x})\) is zero iff
\({\tilde{\mathbold{A}}}^2\), an \(n\times{n}\) square matrix, is invertible
(non-singular), in which case

\begin{equation*}
\mathbold{x}=
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}}
\end{equation*}

\noindent produces that minimum value of \(J'(\mathbold{x})\), because then

\begin{equation*}
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}}=
{\tilde{\mathbold{A}}}^2\,
\mathbold{x}
\end{equation*}

We call such a solution for \(\mathbold{x}\) the \emph{least-squares estimate} of
\(\mathbold{x}\): the estimate of
\(\mathbold{x}\) based on all prior observations.
From now on, we write it as \({\tilde{\mathbold{x}}}\)

\begin{equation}
\label{eqn:least-squares-estimate}
\tilde{\mathbold{x}}
\stackrel{\text{\tiny def}}{=}
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
{\tilde{\mathbold{z}}} 
\end{equation}

With this solution, we get a new expression for the performance functional
\(J(\mathbold{x})\) that is  useful below. First note that 

\begin{alignat}{6}
\notag
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{A}}}^{-2}
&=
\mathbold{1}
&& \text{}
\\
\notag
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
&=
{\tilde{\mathbold{A}}}^\intercal
&& 
\quad\text{Multiply on right by }\tilde{\mathbold{A}}^\intercal
\\
\notag
({\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{A}}})\,
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
&=
{\tilde{\mathbold{A}}}^\intercal
&&
\quad\text{Expand definition of }{\tilde{\mathbold{A}}}^2
\\
\label{eqn:aa2at-is-one}
\mathrm{therefore}\quad
{\tilde{\mathbold{A}}}\,
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
&=
\mathbold{1}
&&
\quad\text{Arbitrariness of }\tilde{\mathbold{A}}^\intercal\text{on left}
\end{alignat}



\noindent Equation \ref{eqn:aa2at-is-one} is another assertion that requires a
rigorous proof, out of scope for this paper of sketches. But, assuming it is
true, we have

\begin{alignat}{6}
\notag
J(\mathbold{x})
&=
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^\intercal
\cdot
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
\\
\notag
&=
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^\intercal
{\tilde{\mathbold{A}}}\,
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
&&
\quad\text{insert }\mathbold{1}\text{ from equation \ref{eqn:aa2at-is-one}}
\\
\notag
&=
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^\intercal
{\tilde{\mathbold{A}}}\,
({\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^2)\,
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
&&
\quad\text{insert }\mathbold{1} = {\tilde{\mathbold{A}}}^{-2}\,{\tilde{\mathbold{A}}}^{2}
\\
\notag
&=
\left[
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)^\intercal
{\tilde{\mathbold{A}}}\,
{\tilde{\mathbold{A}}}^{-2}
\right]
{\tilde{\mathbold{A}}}^2
\left[
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
\right]
&&
\quad\text{Regroup}
\\
\notag
&=
\left[
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
\right]^\intercal
{\tilde{\mathbold{A}}}^2
\left[
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal
\left(
{\tilde{\mathbold{z}}}-
{\tilde{\mathbold{A}}}\,
\mathbold{x}
\right)
\right]
&&
\quad\text{Symmetry of $\tilde{\mathbold{A}}$ and $\tilde{\mathbold{A}}^{-2}$}
\\
\label{eqn:performance-functional-reformed}
&=
(\tilde{\mathbold{x}}-\mathbold{x})^\intercal\,
{\tilde{\mathbold{A}}^2}\,
(\tilde{\mathbold{x}}-\mathbold{x})
&&
\quad\text{Definition of }{\tilde{\mathbold{x}}}\text{ from equation \ref{eqn:least-squares-estimate}}
\end{alignat}

\noindent
This has
physical dimensions \(\mathcal{Z}^2\) where \(\mathcal{Z}\) are the physical
dimensions of the observations \(\mathbold{z}\).

\subsubsection{Prior Covariance \(\tilde{\mathbold{P}}\)}
\label{sec:orgheadline9}

We now want the covariance of the residuals between
our least-squares estimate \(\tilde{\mathbold{x}}\) and the random vector
\(\mathbold{x}\):

\begin{align}
\label{eqn:covariance-of-x}
\tilde{\mathbold{P}}
\stackrel{\text{\tiny def}}{=}
E
\left[
(\tilde{\mathbold{x}}-\mathbold{x})
(\tilde{\mathbold{x}}-\mathbold{x})^\intercal
\right]
\end{align}

\noindent  Get \(\tilde{\mathbold{x}}-\mathbold{x}\)
from the observations and partials at hand as follows:

\begin{alignat}{6}
\notag
{\tilde{\mathbold{z}}}
&=
{\tilde{\mathbold{A}}}\,
\mathbold{x} + 
\mathbold{\zeta}
&&
\quad\text{the observation equation, Equation \ref{eqn:observation-equation}}
\\
\notag
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}}
&=
\mathbold{x} + 
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal\,
\mathbold{\zeta}
&&
\quad\text{Multiply on left by }{\tilde{\mathbold{A}}}^{-2}\,\tilde{\mathbold{A}}^\intercal
\\
\notag
\tilde{\mathbold{x}}
&=
\mathbold{x} +
{\tilde{\mathbold{A}}}^{-2}\,
{\tilde{\mathbold{A}}}^\intercal\,
\mathbold{\zeta}
&&
\quad\text{Definition of }{\tilde{\mathbold{x}}}\text{ from equation \ref{eqn:least-squares-estimate}}
\\
\notag
\text{therefore}\quad
\tilde{\mathbold{x}} -
\mathbold{x} &=
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
\mathbold{\zeta}
\end{alignat}

\noindent
Now rewrite equation \ref{eqn:covariance-of-x}, the definition of the prior
covariance \(\tilde{\mathbold{P}}\):

\begin{align}
\notag
E
\left[
(\tilde{\mathbold{x}}-\mathbold{x})
(\tilde{\mathbold{x}}-\mathbold{x})^\intercal
\right] &=
E
\left[
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
\mathbold{\zeta}\,
\mathbold{\zeta}^\intercal
({\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal
\mathbold{\zeta})^\intercal
\right] \\
\label{eqn:almost-final-covariance}
&=
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal\,
E\left[
\mathbold{\zeta}\,
\mathbold{\zeta}^\intercal
\right]
({\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal)^\intercal
\end{align}

\noindent We can collapse the expectation value inwards because the stack of
observation partials \(\tilde{\mathbold{A}}\) is a matrix of concrete, non-random
numbers. 

Noise \(\mathbold{\zeta}\) is Gaussian, normal, with diagonal covariance
matrix \(\mathbold{Z}\), by hypothesis. Equation \ref{eqn:almost-final-covariance}
becomes

\begin{align} 
\notag
\tilde{\mathbold{P}} =
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal\,
E\left[
\mathbold{\zeta}\,\mathbold{\zeta}^\intercal
\right]
({\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal)^\intercal 
&= 
\notag
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal\,
\mathbold{Z}\,
({\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal)^\intercal 
\\
&= 
\notag
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal\,
\mathbold{Z}\,
{\tilde{\mathbold{A}}}
({\tilde{\mathbold{A}}}^{-2})^\intercal
\\
&= 
\label{eqn:prior-covariance-convenient-form}
{\tilde{\mathbold{A}}}^{-2}
{\tilde{\mathbold{A}}}^\intercal\,
\mathbold{Z}\,
{\tilde{\mathbold{A}}}
({\tilde{\mathbold{A}}}^{-2})
\end{align}

\noindent because \(\tilde{\mathbold{A}}^{-2}\) is symmetric.
At this point, no further simplification is possible, in general, because
\(\mathbold{Z}\) is \(b\times b\) and can only be sandwiched between
\({\tilde{\mathbold{A}}}^\intercal\), \(n\times b\), and 
\({\tilde{\mathbold{A}}}\), \(b\times n\). However, we can greatly simplify this and
all subsequent computations by de-dimensionalizing. There are numerical
benefits, as well, to be discussed in the next section.

\subsubsection{De-Dimensionalizing the Observation Equation}
\label{sec:orgheadline10}

Fully spelled out, and in the general case of \mbox{$b$-vector} observations
\(\mathbold{z}\), one block of height \(b\) of the observation equation is

\begin{equation*}
\left(
\begin{array}{c}
 z_1 \\
 z_2 \\
 \vdots  \\
 z_b \\
\end{array}
\right)=\left(
\begin{array}{cccc}
 A_{11} & A_{12} & \cdots  & A_{1 n} \\
 A_{21} & A_{22} & \cdots  & A_{2 n} \\
 \vdots  & \vdots  & \ddots & \vdots  \\
 A_{\text{b1}} & A_{\text{b2}} & \cdots  & A_{b n} \\
\end{array}
\right) \left(
\begin{array}{c}
 x_1 \\
 x_2 \\
 \vdots  \\
 x_n \\
\end{array}
\right)+\left(
\begin{array}{c}
 \zeta _1 \\
 \zeta _2 \\
 \vdots  \\
 \zeta _b \\
\end{array}
\right)
\end{equation*}

If we divide each row \(i\) by the standard deviation \(\sigma_{z_i}\) of the \mbox{$i$-th}
component \(z_i\) of the observation \(\mathbold{z}\), we get

\begin{equation}
\label{eqn:de-dimensionalized-observation-equation}
\left(
\begin{array}{c}
 \frac{z_1}{\sigma _{z_1}} \\
 \frac{z_2}{\sigma _{z_2}} \\
 \vdots  \\
 \frac{z_b}{\sigma _{z_b}} \\
\end{array}
\right)=\left(
\begin{array}{cccc}
 \frac{A_{11}}{\sigma _{z_1}} & \frac{A_{12}}{\sigma
   _{z_1}} & \cdots  & \frac{A_{1 n}}{\sigma _{z_1}} \\
 \frac{A_{21}}{\sigma _{z_2}} & \frac{A_{22}}{\sigma
   _{z_2}} & \cdots  & \frac{A_{2 n}}{\sigma _{z_2}} \\
 \vdots  & \vdots  & \ddots & \vdots  \\
 \frac{A_{\text{b1}}}{\sigma _{z_b}} &
   \frac{A_{\text{b2}}}{\sigma _{z_b}} & \cdots  &
   \frac{A_{b n}}{\sigma _{z_b}} \\
\end{array}
\right) \left(
\begin{array}{c}
 x_1 \\
 x_2 \\
 \vdots  \\
 x_n \\
\end{array}
\right)+\left(
\begin{array}{c}
 \frac{\zeta _1}{\sigma _{z_1}} \\
 \frac{\zeta _2}{\sigma _{z_2}} \\
 \vdots  \\
 \frac{\zeta _b}{\sigma _{z_b}} \\
\end{array}
\right)
\end{equation}

The covariance of the noise \(\mathbold{\zeta}\), so normalized, is non-dimensional
unity and equation \ref{eqn:prior-covariance-convenient-form} collapses
completely to just

\begin{equation}
\label{eqn:prior-covariance-most-convenient-form}
\tilde{\mathbold{P}}={\tilde{\mathbold{A}}}^{-2}
\end{equation}

\noindent and the estimate of the priors, equation
\ref{eqn:least-squares-estimate} now becomes

\begin{equation}
\label{eqn:estimate-of-the-priors}
\tilde{\mathbold{x}}
\stackrel{\text{\tiny def}}{=}
\tilde{\mathbold{P}}\,
{\tilde{\mathbold{A}}}^\intercal
{\tilde{\mathbold{z}}} 
\end{equation}


This is remarkable. All information about the covariance of the noise is pulled
into the (new, normalized) observation partials. 

I remember, when working in the early 1980's at the Deep Space Network at JPL on
direct measurement of tectonic drift,\footnote{JPL Geodynamics Program \url{http://www.jpl.nasa.gov/report/1981.pdf}} one difficulty was the wide
disparity between uncertainties of horizontal measurments (right ascension and
declination) and uncertainties in range. For instance, we knew the RA-dec
position of the centroid of Saturn within 75 meters but its distance to no
better than a million kilometers. That's a disparity of seven orders of
magnitude (the situation is greatly improved, now, due to the accumulation of
range data for multiple spacecraft coupled with decades of orbital
mechanics\footnote{\url{http://ipnpr.jpl.nasa.gov/progress_report/42-178/178C.pdf}}). At the time, this meant that we had to deal with error
ellipsoids that were long, thin needles, covariance matrices with components
differing by up to fourteen orders of magnitude. That's not practical with
floating-point computer arithmetic. One mitigation was de-dimensionalizing or
normalizing, as described here, which brings the uncertainties of all components
of an observation into the same numerical range, near unity. Another mitigation
was Square Root Information Filtering (SRIF), the subject of another paper in
this series.

In any event, for all subsequent calculations in this paper, we assume that the
observation equation has been normalized and that \(\mathbold{Z}=\mathbold{1}\). 


\subsection{Posterior Estimate \(\hat{\mathbold{x}}\) and Covariance \(\hat{\mathbold{P}}\)}
\label{sec:orgheadline16}

To effect incremental updates of \(\mathbold{x}\) and \(\mathbold{P}\), we need the
posterior estimate \(\hat{\mathbold{x}}\) and covariance \(\hat{\mathbold{P}}\) in
terms of the priors \(\tilde{\mathbold{x}}\), \(\tilde{\mathbold{P}}\), and the new
partials \(\mathbold{A}\) and new observation \(\mathbold{z}\), all  of which are
matrices of known, concrete, non-random numbers. This is exactly what our
\emph{kalmanStatic} function from equation \ref{eqn:kalman-cume-definition} does, of course,
in functional form.  We derive the posteriors from scratch to seek
opportunities to define \(\mathbold{K}\) and \(\mathbold{D}\) and to radically shorten
the expressions. 

First, define a new performance functional \(J_1(\mathbold{x})\) as the sum of the 
performance of the priors \(\tilde{J}(\mathbold{x})\) from equation
\ref{eqn:performance-functional-reformed}, now written with tildes overhead,
and a new term
\(J_2(\mathbold{x})\) for the
performance of the new data:

\begin{alignat}{6}
J_1(\mathbold{x})
& \stackrel{\text{\tiny def}}{=}
{\tilde{J}}(\mathbold{x}) +
J_2(\mathbold{x})
\\
\notag
{\tilde{J}}(\mathbold{x})
&\stackrel{\text{\tiny def}}{=}
(\tilde{\mathbold{x}}-\mathbold{x})^\intercal\,
{\tilde{\mathbold{A}}^2}\,
(\tilde{\mathbold{x}}-\mathbold{x})
&&
\quad\text{Equation \ref{eqn:performance-functional-reformed}}
\\
\label{eqn:performance-of-new-data}
J_2(\mathbold{x})
&\stackrel{\text{\tiny def}}{=}
\left(
\mathbold{z}-
\mathbold{A}\,
\mathbold{x}
\right)^2
\\
\notag
&=
\left(
\mathbold{z}-
\mathbold{A}\,
\mathbold{x}
\right)^\intercal
\cdot
\left(
\mathbold{z}-
\mathbold{A}\,
\mathbold{x}
\right)
\\
\notag
&=
\mathbold{z}^2 -
\mathbold{z}^\intercal\,
\mathbold{A}\,
\mathbold{x} -
\mathbold{z}\,
\mathbold{x}^\intercal\,
\mathbold{A}^\intercal +
\left(
\mathbold{A}\,
\mathbold{x}
\right)^2
\\
\notag
&=
\mathbold{z}^2 -
2\,
\mathbold{z}^\intercal\,
\mathbold{A}\,
\mathbold{x} +
\left(
\mathbold{A}\,
\mathbold{x}
\right)^2
\end{alignat}

This time, I don't have a handy trick for minimizing the performance functional.
Let's find the minimizing \(\mathbold{x}\) the classic way: by solving
\(d\,J_1(\mathbold{x})/d\,\mathbold{x}=0\). The usual way to write a vector
derivative is with the \emph{nabla} operator \(\nabla\), which produces \emph{gradient}
vectors from scalar functions.

\begin{align*}
\nabla{}\,f(\mathbold{x}) &\stackrel{\text{\tiny def}}{=}
\begin{bmatrix}
df(\mathbold{x})/dx_0\\
df(\mathbold{x})/dx_1\\
\vdots\\
df(\mathbold{x})/dx_{n-1}
\end{bmatrix}
\end{align*}

The particular scalar function we're differentiating is, of course, the new
performance functional
\(J_1(\mathbold{x})=
{\tilde{J}}(\mathbold{x})+
J_2(\mathbold{x})\). Because
\({\tilde{\mathbold{A}}^2}\) is symmetric,

\begin{align*}
\nabla{}\,
{\tilde{J}}(\mathbold{x}) &=
\nabla{}
\left(
(\tilde{\mathbold{x}}-\mathbold{x})^\intercal\,
{\tilde{\mathbold{A}}^2}\,
(\tilde{\mathbold{x}}-\mathbold{x})
\right) \\ &=
-2\,
{\tilde{\mathbold{A}}^2}\,
(\tilde{\mathbold{x}}-\mathbold{x})
\end{align*}

\noindent an \mbox{$n$-vector}, and we similarly compute the gradient of
\(J_2(\mathbold{x})\), which contains the new observation and partials:

\begin{align*}
\nabla\,
J_2(\mathbold{x})
&=
\nabla
\left(
\mathbold{z}^2 -
2\,
\mathbold{z}^\intercal\,
\mathbold{A}\,
\mathbold{x} +
\left(
\mathbold{A}\,
\mathbold{x}
\right)^2
\right)
\\
&=
2\,
\mathbold{A}^\intercal
\left(
\mathbold{A}\,
\mathbold{x} -
\mathbold{z}
\right)
\\
&=
2\,
\left(
\mathbold{A}^2\,
\mathbold{x}-
\mathbold{A}^\intercal\,
\mathbold{z}
\right)
\end{align*}

\noindent another \mbox{$n$-vector}. We can solve the resulting equation for
\(\mathbold{x}\) on sight, writing the new solution --- the new estimate ---
with an overhat. Be aware that
that \(\mathbold{A}\) is a wide matrix, in fact   an \mbox{$n$-row} when \(b=1\), a
common case, and 
\(\mathbold{A}^2\) is thus an outer product and an \(n\times{n}\) matrix.


\begin{align}
\notag
\nabla{}\,
J_1(\mathbold{x}) 
&= 
\nabla{}\,
{\tilde{J}}
(\mathbold{x}) + 
\nabla{}\,
J_2(\mathbold{x}) 
= 0
\\
\notag
&=
{\tilde{\mathbold{A}}}^2\,
\mathbold{x} -
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{x}}} +
\mathbold{A}^2\,
\mathbold{x} - 
\mathbold{A}^\intercal{}\,
\mathbold{z}
\\
\label{eqn:def-of-posterior-estimate}
&
\text{if and only if}\quad
\mathbold{x}=\hat{\mathbold{x}}
\stackrel{\text{\tiny def}}{=}
\left(
{\tilde{\mathbold{A}}}^2 + 
\mathbold{A}^2
\right)^{-1}
\cdot
\left(
\mathbold{A}^\intercal\,
\mathbold{z} + 
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{x}}}
\right)
\end{align}

Look how pretty this is. Equation \ref{eqn:estimate-of-the-priors} for the
priors gave us the form
\(\tilde{\mathbold{x}}= \tilde{\mathbold{P}}\,
\tilde{\mathbold{A}}^\intercal\,\tilde{\mathbold{z}}\), a covariance 
\(\tilde{\mathbold{P}}\)
times  the prior observations 
\(\tilde{\mathbold{z}}\)
scaled by the prior partials, transposed, 
\(\tilde{\mathbold{A}}^\intercal\). 
The new estimate \(\hat{\mathbold{x}}\) has exactly
the same form if we regard the first matrix factor
\(\left({\tilde{\mathbold{A}}}^2 + \mathbold{A}^2 \right)^{-1}\) 
as a  covariance
\(\hat{\mathbold{P}}\)  and if
we regard \emph{all} the priors \({\tilde{\mathbold{A}}}^2\,{\tilde{\mathbold{x}}}\) as a \emph{single}
scaled observation
to add to the current scaled observation \(\mathbold{A}^\intercal\,\mathbold{z}\).
We may regard \({\tilde{\mathbold{A}}^2}\,\tilde{\mathbold{x}}\) as a scaled
observation because
equations
\ref{eqn:prior-covariance-most-convenient-form}
and
\ref{eqn:estimate-of-the-priors}
imply that
\({\tilde{\mathbold{A}}^\intercal}\,\tilde{\mathbold{z}}={\tilde{\mathbold{A}}^2}\,\tilde{\mathbold{x}}\). 
We may view the second term above,
\(\mathbold{A}^\intercal\,
\mathbold{z} + 
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{x}}}\), 
as
\(\mathbold{A}^\intercal\,
\mathbold{z} + 
{\tilde{\mathbold{A}}}^\intercal\,
{\tilde{\mathbold{z}}}\).

\subsubsection{Posterior estimate, \(\hat{\mathbold{x}}\)}
\label{sec:orgheadline12}

We must wrangle 
equation
\ref{eqn:kalman-update-equation}
from
equation
\ref{eqn:def-of-posterior-estimate}.
Equation 
\ref{eqn:kalman-update-equation}
is the recurrence we want,
namely 
\(\hat{\mathbold{x}}=\tilde{\mathbold{x}}+\mathbold{K}(\mathbold{z}-\mathbold{A}\,\tilde{\mathbold{x}})\),
and equation
\ref{eqn:def-of-posterior-estimate}
is the recurrence we have, namely\\
\(\hat{\mathbold{x}}
=
\left(
{\tilde{\mathbold{A}}}^2 + 
\mathbold{A}^2
\right)^{-1}\,
\left(
\mathbold{A}^\intercal\,
\mathbold{z} + 
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{x}}}
\right)\).

First, formally define the new, posterior covariance.

\begin{equation}
\label{eqn:new-p-hat-definition}
{\hat{\mathbold{P}}}
\stackrel{\text{\tiny def}}{=}
\left(
{\tilde{\mathbold{A}}}^2 + \mathbold{A}^2
\right)^{-1}
\end{equation}

\noindent Now write 
equation
\ref{eqn:def-of-posterior-estimate}
as

\begin{align*}
\hat{\mathbold{x}}
&=
\hat{\mathbold{P}}\,
\left(
\mathbold{A}^\intercal\,
\mathbold{z} + 
{\tilde{\mathbold{A}}}^2\,
{\tilde{\mathbold{x}}}
\right)
\\
\\
&=
\hat{\mathbold{P}}\,\mathbold{A}^\intercal\,
\mathbold{z}
+
\hat{\mathbold{P}}\,
\tilde{\mathbold{A}}^2\,\tilde{\mathbold{x}}
\\
\end{align*}


The form above strongly suggests that we define

\begin{equation}
\label{eqn:kalman-gain-new-definition}
\mathbold{K}
\stackrel{\text{\tiny def}}{=}
{\hat{\mathbold{P}}}\,
\mathbold{A}^\intercal
\end{equation}

\noindent yielding

\begin{align}
\label{eqn:first-part-of-gain-proof}
\hat{\mathbold{x}}
&=
\mathbold{K}\,
\mathbold{z}
+
{\hat{\mathbold{P}}}\,
\tilde{\mathbold{A}}^2\,\tilde{\mathbold{x}}
\end{align}

\noindent Now, to get the recurrence we want

\begin{align}
\notag
\hat{\mathbold{x}}
&=
\tilde{\mathbold{x}}+
\mathbold{K}
\left(
\mathbold{z}-
\mathbold{A}\,
{\tilde{\mathbold{x}}}
\right)
\\
\label{eqn:second-part-of-gain-proof}
&=
\tilde{\mathbold{x}}+
\mathbold{K}\,
\mathbold{z}-
\mathbold{K}\,
\mathbold{A}\,
{\tilde{\mathbold{x}}}
\end{align}

\noindent we need only set equation \ref{eqn:first-part-of-gain-proof} equal to
equation \ref{eqn:second-part-of-gain-proof}.  Cancelling terms and rearranging,
we get

\begin{align}
\label{eqn:recurrence-to-prove}
(\mathbold{1}-\mathbold{K}\,\mathbold{A})\,
\tilde{\mathbold{x}}
&=
{\hat{\mathbold{P}}}\,
\tilde{\mathbold{A}}^2\,\tilde{\mathbold{x}} 
=
{\hat{\mathbold{P}}}\,
\tilde{\mathbold{P}}^{-1}\,\tilde{\mathbold{x}} 
\end{align}

\noindent by definition of the prior covariance, equation
\ref{eqn:prior-covariance-most-convenient-form}. For arbitrary
\(\tilde{\mathbold{x}}\), this will be true if 

\begin{align*}
(\mathbold{1}-\mathbold{K}\,\mathbold{A})
&=
{\hat{\mathbold{P}}}\,
\tilde{\mathbold{P}}^{-1}
\end{align*}

\noindent Rearrange and right-multiply by \(\tilde{\mathbold{P}}\) to get

\begin{align}
\label{eqn:p-is-l-p}
\hat{\mathbold{P}}
&=\left(
\mathbold{1}-
\mathbold{K}\,
\mathbold{A}
\right)\,
\tilde{\mathbold{P}}
=
\hat{\mathbold{P}}\,{\tilde{\mathbold{A}}}^2\,\tilde{\mathbold{P}}
\end{align}

\noindent showing that equations \ref{eqn:recurrence-to-prove} and
\ref{eqn:kalman-update-equation} are just alternative expressions for the same
thing.


Let's write this more compactly

\begin{align}
\label{eqn:derivation-of-p-is-l-p}
{\hat{\mathbold{P}}} &=
\mathbold{L}\,
{\tilde{\mathbold{P}}}
\end{align}

\noindent where

\begin{equation}
\label{eqn:definition-of-l}
\mathbold{L}\stackrel{\text{\tiny def}}{=}
(\mathbold{1}-
\mathbold{K}\,
\mathbold{A})
=
\hat{\mathbold{P}}\,{\tilde{\mathbold{A}}}^2
\end{equation}

\noindent and we have
one of the three equivalent recurrences
for the posterior covariance
from the first paper in this series

\begin{equation}
{{\mathbold{P}}} \leftarrow
\mathbold{L}\,
{{\mathbold{P}}}
\end{equation}

\subsubsection{A Gain Matrix \(\mathbold{K}\) We Can Actually Compute}
\label{sec:orgheadline13}



Of course, the gain matrix \(\mathbold{K}\) is formally defined in terms of the
posterior covariance, that is, as \(\hat{\mathbold{P}}\,\mathbold{A}^\intercal\),
but we don't have the posterior covariance \(\hat{\mathbold{P}}\) by equation
\ref{eqn:p-is-l-p} until we have the gain matrix \(\mathbold{K}\). To get out of
this fix, we note that 

\begin{equation*}
\mathbold{K}
=
\hat{\mathbold{P}}\,
\mathbold{A}^\intercal
=
\mathbold{L}\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
=
(\mathbold{1}-
\mathbold{K}\,
\mathbold{A})\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
\end{equation*}

\noindent and solve for \(\mathbold{K}\):

\begin{align}
\notag
\mathbold{K}
&=
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
-
\mathbold{K}\,
\mathbold{A}\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
\\
\notag
\mathbold{K}\,
(\mathbold{1}+
\mathbold{A}\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal)
&=
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
\\
\label{eqn:for-k}
\mathbold{K}
&=
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal\,
(\mathbold{1}+
\mathbold{A}\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal)^{-1}
\end{align}

Defining the Kalman denominator matrix \(\mathbold{D}\) as follows:

\begin{equation}
\label{eqn:definition-of-d}
\mathbold{D}
\stackrel{\text{\tiny def}}{=}
\mathbold{1}+
\mathbold{A}\,
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal
\end{equation}

\noindent we finally get a form for the Kalman gain matrix \(\mathbold{K}\)
entirely in terms of priors and the new observation partials (sometimes called
the \emph{innovation}):

\begin{align}
\label{eqn:kalman-gain-definition-2}
\mathbold{K}
&=
\tilde{\mathbold{P}}\,
\mathbold{A}^\intercal\,
\mathbold{D}^{-1} 
\\
\label{eqn:kalman-denominator-definition-2}
\text{where}\quad
\mathbold{D}
&= \mathbold{1} +
\mathbold{A}\,
\mathbold{P}\,
\mathbold{A}^\intercal
\end{align}

\noindent These are almost the same as the original definitions, equations
\ref{eqn:kalman-gain-definition} and \ref{eqn:kalman-denominator-definition},
which were written in dimensional form. We leave it to the reader to show that
the dimensional form for \(\mathbold{D}\) is
\(\mathbold{Z}+
\mathbold{A}\,
\mathbold{P}\,
\mathbold{A}^\intercal\).

\subsubsection{Two More Recurrences}
\label{sec:orgheadline14}

There remain
two more recurrences to derive, namely

\begin{equation}
\label{eqn:p-is-lplt-plus-kzkt}
\mathbold{P}\leftarrow
\mathbold{L}\,
\mathbold{P}\,
\mathbold{L}^\intercal +
\mathbold{K}\,
\mathbold{Z}\,
\mathbold{K}^\intercal
\end{equation}

\noindent and the canonical form,

\begin{equation}
\label{eqn:p-is-p-minus-kdkt}
\mathbold{P}\leftarrow
\mathbold{P} -
\mathbold{K}\,
\mathbold{D}\,
\mathbold{K}^\intercal
\end{equation}

\subsubsection{Minimizing \(J_1({\mathbold{x}})\)}
\label{sec:orgheadline15}

The posterior covariance is, from the statistical viewpoint,

\begin{equation*}
{\hat{\mathbold{P}}} =
E
\left[
({\hat{\mathbold{x}}}-\mathbold{x})
({\hat{\mathbold{x}}}-\mathbold{x})^\intercal
\right]
\end{equation*}

\noindent Get our new expression for \({\hat{\mathbold{x}}}\):

\begin{equation*}
{\hat{\mathbold{x}}} =
{\tilde{\mathbold{x}}}+
\mathbold{K}\,
(\mathbold{z}-
\mathbold{A}\,
{\tilde{\mathbold{x}}}) =
\mathbold{K}\,
\mathbold{z} +
\mathbold{L}\,
{\tilde{\mathbold{x}}}
\end{equation*}

\noindent where, again

\begin{equation*}
\mathbold{L}
=
(\mathbold{1}-
\mathbold{K}\,
\mathbold{A})
=
{\hat{\mathbold{P}}}\,
{\tilde{\mathbold{A}}}^2
\end{equation*}

\noindent
Remembering the observation equation
(\ref{eqn:observation-equation}), write a single instance of it
\(\mathbold{z} =
\mathbold{A}\,
\mathbold{x}+
\mathbold{\zeta}\) and find

\begin{align}
\notag
{\hat{\mathbold{x}}}
&=
\mathbold{K}\,
\mathbold{A}\,
\mathbold{x} +
\mathbold{K}\,
\mathbold{\zeta} +
\mathbold{L}\,
{\tilde{\mathbold{x}}}
\\
\notag
&=
\left(
\mathbold{1}-
\mathbold{L}
\right)\,
\mathbold{x} +
\mathbold{K}\,
\mathbold{\zeta} +
\mathbold{L}\,
{\tilde{\mathbold{x}}}
\end{align}

\noindent implying that
\(\left(
{\hat{\mathbold{x}}}-
\mathbold{x}
\right)=
\mathbold{L}\,
\left(
{\tilde{\mathbold{x}}}-
\mathbold{x}
\right) +
\mathbold{K}\,
\mathbold{\zeta}\).

Remembering that
\(E
\left[
\mathbold{\zeta}
\right]=\mathbold{0}\), 
\(E
\left[
\mathbold{\zeta}\,
\mathbold{\zeta}^\intercal
\right]=\mathbold{Z}\), glibly re-dimensionalizing and skipping
intermediate steps, we find that 

\begin{equation}
{\hat{\mathbold{P}}} = 
\mathbold{L}\,
{\tilde{\mathbold{P}}}\,
\mathbold{L}^\intercal + 
\mathbold{K}\,
\mathbold{Z}\,
\mathbold{K}^\intercal
\end{equation}

\noindent We leave it to the reader to check, with reference to equations
\ref{eqn:dimensional-breakdown}, that the physical dimensions work out. This
completes the derivation of the recurrence equation \ref{eqn:p-is-lplt-plus-kzkt}. 

The last form,
\(\hat{\mathbold{P}}
=
\tilde{\mathbold{P}}-
\mathbold{K}\,
\mathbold{D}\,
\mathbold{K}^\intercal\),
is easy to show from what we already know, that 
\(\hat{\mathbold{P}}
=
\mathbold{L}\,
\tilde{\mathbold{P}}
=
(\mathbold{1}-
\mathbold{K}\,
\mathbold{A})\,
\tilde{\mathbold{P}}\).
We just need to show that 
\(\mathbold{K}\,
\mathbold{A}\,
\tilde{\mathbold{P}} 
=
\mathbold{K}\,
\mathbold{D}\,
\mathbold{K}^\intercal\). 
Substitute 
\(\mathbold{D}^{-\intercal}\,
\mathbold{A}\,
\tilde{\mathbold{P}}^\intercal\)
for \(\mathbold{K}^\intercal\) by transposing
equation \ref{eqn:kalman-gain-definition-2}.
Note that for square matrices, the inverse of the
transpose is the transpose of the inverse. Therefore
\(\mathbold{D}^{-\intercal}
= \mathbold{D}^{-1}\) because \(\mathbold{D}\) is symmetric. Likewise 
\(\tilde{\mathbold{P}}^\intercal=\tilde{\mathbold{P}}\).  The result follows:

\begin{equation*}
\mathbold{K}\,
\mathbold{D}\,
\mathbold{K}^\intercal
=
\mathbold{K}\,
\mathbold{D}\,
\mathbold{D}^{-\intercal}\,
\mathbold{A}
\tilde{\mathbold{P}}
=
\mathbold{K}\,
\mathbold{A}
\tilde{\mathbold{P}}
\end{equation*}



\section{Concluding Remarks}
\label{sec:orgheadline18}

These derivations are helpful for gaining intuition into the underlying
statistics and dimensional structures of the Kalman filter and its many
variants. They are a bit involved, but it is worthwhile to ingest these
fundamentals, especially for those who need to research new filters and
applications. For more rigorous proofs built on a Bayesian perspective, see
Bar-Shalom.\footnotemark[2]{} For more careful dimensional analysis of the present
derivations, see part 6 of this series.\footnote{B. Beckman, \emph{Kalman Folding 6: Dimensional Analysis}, to appear.}
\end{document}