%\documentstyle[onecolumn]{/usr/sol/ohallaro/tex/spie}
\documentstyle[reduced]{/usr/sol/ohallaro/tex/spie}
%\documentstyle[titleonly]{/usr/sol/ohallaro/tex/spie}
%\documentstyle{article}
\title{Fast Mapping of a Kalman Filter on Warp}
\author{David R. O'Hallaron and Radhakisan S. Baheti\\GE Research and Development Center, Schenectady, NY 12301}
\begin{document}
\bibliographystyle{plain}
\maketitle

\begin{abstract} 
A parallel algorithm for solving an $n$-state Kalman filter on an
$(n+2)$-cell linear array is described. The algorithm is the basis for
the mapping of a 9-state target tracking filter on the Warp computer.
The Warp implementation is written in a high-level language and
achieves a measured speedup of almost 300 over the same filter running
on a Sun workstation.
\end{abstract}

\section{Introduction}
Kalman filtering is a general and popular technique for recursively
estimating the state variables of a dynamic system from noisy
measurements. The solution of the Kalman equations is expensive, in
general requiring $O(n^3)$ floating-point computations. This
time-complexity has motivated the design of a number of parallel
algorithms, \cite{andrews81,chen86,jover86,shaffer87b,sung87}, 
all of which assume 2-D arrays of computational cells. Such arrays
require numerous external (and expensive) high-speed data transmission
channels, and cannot always be synchronized with a single global clock
\cite{fisher83}. In general, 2-D arrays of cells are more costly and
difficult to construct than linear arrays of cells. 

This paper describes a parallel algorithm for solving an $n$-state
Kalman filter using an $(n+2)$-cell linear array.  The algorithm is
the basis for the mapping of a 9-state target tracking filter on the
Warp computer \cite{annaratone87e}, a linear array of 10 or more
identical, powerful, and programmable cells connected to a
general-purpose host.  The Warp implementation is written in a
high-level language and achieves a measured sample time of 282 $\mu
s$, which is a speedup of almost 300 over the same filter running on a
Sun 3/60 workstation.

Section 2 describes a Kalman filter for target tracking applications.
Section 3 discusses some high-level design issues for the parallel
algorithm, which is presented in Section 4. Section 5 describes the
mapping of the algorithm on Warp and its performance.

\section{Target Tracking Kalman Filter}

In this section we describe a Kalman filter for target tracking applications.
Dimensions and description of symbols used are given in Figure \ref{symbols},
where $n$ is the number of elements in the state vector and $m$ is the
number of observations in the measurement vector.  In this paper, we assume
a target tracking filter with $n=9$ states and $m=3$ measurements.
The filter is used to estimate from noisy measurements the position,
velocity and acceleration of a maneuvering target.  The target may be 
surface, subsonic aerial, or supersonic aerial, such as a ship, an 
aircraft, or a missile.  The radar, sonar, or optical instruments measure
the target range and azimuth and elevation angles at high data rates.  

The tracking filter is based on the laws of motion and a stochastic
acceleration model.  It is assumed that the target moves at constant
velocity if there is no maneuver or atmospheric turbulence.  Evasive
maneuvers and other disturbances are modelled as perturbations upon the
constant-velocity trajectory.

The target acceleration is correlated in time.  For example, a lazy turn
will use a highly correlated acceleration.  On the other hand, evasive
maneuvers will be less correlated in time.  The correlation function
$ \gamma ( \tau ) $ for the target acceleration $ a(t) $ can be 
represented by 
\begin{displaymath}
\gamma ( \tau ) = E\{ a(t)a(t+ \tau ) \} = \sigma _ m  ^ 2 e ^{ - \alpha \mid \tau \mid }
\end{displaymath}
where $E$ denotes the expected value, $ \sigma _ m  ^ 2 $ denotes the variance
of the target acceleration, and $ 1 / \alpha $ is the maneuver time constant.
The target dynamic model in one dimension  has the form
\begin{displaymath}
d/dt  \left [
       \begin{array}{c}
        x \\
        \dot x \\
        \ddot x
       \end{array} \right ] =
\left [ \begin{array}{c c c}
      0 & 1 & 0 \\
      0 & 0 & 1 \\
      0 & 0 & - \alpha
     \end{array}
      \right ]  \left [ \begin{array}{c}
         x \\
         \dot x \\
         \ddot x \end{array}
    \right ]
 + \left [ \begin{array}{c}
    0 \\
    0 \\
    1 
 \end{array}
 \right ]   
 w(t)    
\end{displaymath}
where $ x,  \dot x, $ and $ \ddot x $ denote the target position, velocity,
and acceleration at time $ t $, and  $ \{ w(t) \} $ is a white Gaussian
noise process with zero mean and variance $ \sigma _ m ^ 2 $.  Note that
the uncertainty in the target acceleration is represented by a first-order
Gauss-Markov process.

The filtering may be performed in polar coordinates with range, azimuth, 
and elevation angles, and their derivatives as the state variables.  In 
polar coordinates the measurement system model is linear and uncoupled.
However, even for a target moving in a straight line at constant speed,
the dynamic model is nonlinear, and the linearization leads to
large errors. In Cartesian coordinates, a constant-speed target does obey
a a linear law, but the measurement equations are nonlinear.  Both 
coordinate systems have been successfully used in many target tracking
applications.  Target state estimation is normally part of a major goal
to predict the target position and improve the accuracy of a fire control 
system.

\begin{figure}
\scriptsize
\begin{center}
\begin{tabular}{|l|l|l|}
\hline
Symbol & Dimension & Description \\
\hline
\hline
$\Phi$ & $n \times n$ & state transition matrix \\
$H$ & $m \times n$ & Jacobian matrix \\
$H_i$ & $1 \times n$ & $ith$ row of $H$ \\
$K(t)$ & $n \times m$ & Kalman gain matrix \\
$K^{(k)}$ & $n \times 1$ & gain vector for $kth$ scalar update \\
$P(t|t)$ & $n \times n$ &estimation error covariance \\
$P_j$ & $n \times 1$ & $jth$ column of $P$ matrix\\
$P^{(k)}$ & $n \times n$ & $P$ after $kth$ scalar update \\
$P(t+1|t)$ & $n \times n$ &prediction error covariance \\
$Q(t)$ & $n \times n$ & process noise covariance \\
$R(t)$ & $m \times m$ & measurement noise covariance \\
$R_k$ & $1 \times 1$ & $kth$ diagonal element of $R$ \\
${\hat{x}}(t|t)$ & $n \times 1$ & estimated state vector \\
${\hat{x}}^{(k)}$ & $n \times 1$ & ${\bf \hat{x}}$ after $kth$ scalar update\\
${y(t)}$ & $m \times 1$ & measurement vector \\
${y}_{k}(t)$ & $m \times 1$ & $kth$ element of ${\bf  y}$ \\
${h(\hat{x})}$ & $m \times 1$ & coordinate transform vector \\
\hline
\end{tabular}
\end{center}
\caption{Symbols}
\label{symbols}
\end{figure}

A discrete-time equation of target motion in Cartesian coordinates
can be expressed by a state variable model given by \cite{baheti86}. 
\begin{eqnarray}
{\bf x}(t+1) & = & \Phi ( \Delta t, {\bf \Lambda })  {\bf x}(t) + {\bf w}(t) \nonumber \\
{\bf y}(t)   & = & {\bf h(x}(t)) + {\bf v}(t), ~~t=0, 1, \cdots \label{model}
\end{eqnarray}
where $t$ is the normalized discrete time, $ \Delta t $ denotes the
sampling, $ {\bf x} (t) $ is a nine-dimensional state vector with position,
velocity, and acceleration in each of the Cartesian coordinates axes $x,y,z$.
The first-order Markov parameters of discretized stochastic acceleration models in
$ x,y,$ and $ z $ axes are denoted by $ ( 3 \times 3 ) $ matrix
$ {\Lambda } $ = diag. $[ \lambda _ x , \lambda  _ y , \lambda _ z ]$,
where $ \lambda  = (1 - e ^ {- \alpha \Delta t}).  $
The matrix $ \Phi ( \Delta t, {\Lambda } )$ is given by
\begin{equation}
\Phi ( \Delta t, {\Lambda } ) = \left [
\begin{array}{c c c}
    I & (\Delta t) I & 1/2 (\Delta t) ^ 2 I \\
    0 &  I          & ( \Delta t) I \\
    0 &  0          &  {\Lambda }I
\end{array}
\right ]
\label{eqnphi}
\end{equation}
where $ I $ denotes $(3 \times 3) $ identity matrix.  The state vector
$ {\bf x}(t) $ is represented by

\begin{displaymath}
{\bf x}(t)= [ x,y,z, \dot x, \dot y, \dot z, \ddot x,
\ddot y, \ddot z ] ^ T
\end{displaymath}
where $ T $ is a transpose operation.  The vector $ {\bf h(x}(t)) $
is the transformation from the Cartesian coordinates to the polar
coordinates defined by   
\begin{eqnarray}
 r & = & \sqrt{x ^ 2 + y ^ 2 + z ^ 2} \nonumber \\
 \theta _ T & = & \arctan (y/x) \nonumber \\
 \theta _ E & = & \arcsin (z/r) \nonumber
\end{eqnarray}
where $ r, \theta _ T, $ and $ \theta _ E $ denote the target range,
azimuth angle, and elevation angle, respectively.  The noisy 
measurements of range, azimuth, and elevation angles are denoted by
$ {\bf y}(t) $.
\begin{displaymath}
{\bf y}(t) = [r, \theta _ T, \theta _ E ] ^ T.
\end{displaymath}
It is assumed that the state and measurement noise sequences $ \{ {\bf w}(t) \}$ and $\{ {\bf v}(t) \} $ are zero mean gaussian random vector sequences
with covariance matrices $ Q (t) $ and $ R (t) $ respectively.

The  measurement errors in the target range, azimuth angle, and
elevation angle are assumed uncorrelated and may depend on the target
range.  Let $ \sigma ^ 2 _ R , \sigma ^ 2 _ {\theta T }, $ and
$ \sigma ^ 2 _ {\theta E} $ denote the variance of the measurement
noise in range, azimuth angle, and elevation angle, respectively.  The
measurement noise covariance matrix $ R(t) $ is given by
\begin{displaymath}
R(t)= diag [ \sigma ^ 2 _ R, \sigma ^ 2 _ {\theta T}, \sigma ^ 2 _ {\theta E} ].
\end{displaymath}
The dynamics and measurement models presented above are commonly used in
many tracking and navigation applications.  An extended Kalman filter
can be used to recursively estimate the state vector $ {\bf x}(t) $ 
using the dynamics and the measurement model. 
The  extended Kalman filter uses linearization
of nonlinear equations about the current state estimates.  The structure
of the covariance and gain computation equations in the extended Kalman
filter is similar to the structure of the Kalman filter.  However, it is 
important to note that the extended Kalman filter depends on the current state
estimate and thus cannot be computed off-line. 

A block diagram of a target tracking extended Kalman filter is given
in Figure \ref{kfblock}.  The measurements ${\bf y}(t) $ include the effect of sensor
noise, reference input, disturbance, and the target dynamics.  The 
filter consists of time update, Jacobian, and measurement update equations
summarized in the following.

\begin{figure}
\setlength{\unitlength}{0.155in}
\begin{center}
\begin{picture}(19,16)(0,0)
\scriptsize
\put(7,11){\framebox(5,2){\shortstack{Target\\Dynamics}}}
\put(2,7){\framebox(5,2){\shortstack{Time\\Update}}}
\put(12,7){\framebox(5,2){\shortstack{Measurement\\Update}}}
\put(12,4){\framebox(5,1){Jacobian}}
\put(8,6){\framebox(3,1){delay}}
\put(8,1){\framebox(3,1){delay}}
\put(14.5,12){\circle{1}}
\put(14.5,12){\makebox(0,0){+}}
% main loop
\put(1,12){\vector(1,0){6}}
\put(4,12.2){\makebox(0,0)[b]{Reference Input}}
\put(12,12){\vector(1,0){2}}
\put(15,12){\line(1,0){3}}
\put(17.8,10.5){\makebox(0,0)[r]{Measurement}}
\put(18,12){\line(0,-1){3.5}}
\put(18,8.5){\vector(-1,0){1}}
\put(12,8){\vector(-1,0){5}}
\put(9.5,8.2){\makebox(0,0)[b]{\shortstack{$\hat{x}(t|t)$\\$P(t|t)$}}}
\put(2,8){\line(-1,0){1}}
\put(1,8){\line(0,-1){6.5}}
\put(1,1.5){\vector(1,0){7}}
\put(4.5,1.7){\makebox(0,0)[b]{\shortstack{$\hat{x}(t+1|t)$\\$P(t+1|t)$}}}
\put(11,1.5){\line(1,0){7}}
\put(14.5,1.7){\makebox(0,0)[b]{\shortstack{$\hat{x}(t|t-1)$\\$P(t|t-1)$}}}
\put(18,1.5){\line(0,1){6}}
\put(18,7.5){\vector(-1,0){1}}
%jacobian loop
\put(9.5,8){\vector(0,-1){1}}
\put(9.5,6){\line(0,-1){1.5}}
\put(9.3,5){\makebox(0,0)[r]{$\hat{x}(t-1|t-1)$}}
\put(9.5,4.5){\vector(1,0){2.5}}
\put(14.5,5){\vector(0,1){2}}
\put(15,6){\makebox(0,0)[l]{$H$}}
%disturbance and sensor noise
\put(9.5,14){\vector(0,-1){1}}
\put(9.5,14.2){\makebox(0,0)[b]{Disturbance}}
\put(14.5,14){\vector(0,-1){1.5}}
\put(14.5,14.2){\makebox(0,0)[b]{Sensor Noise}}
\end{picture}
\end{center}
\caption{Tracking Filter}
\label{kfblock}
\end{figure}

\subsection{Time Update}

The time-update predicts the state and estimation error covariance matrix using the
following equations:
\begin{eqnarray}
{\bf {\hat{x}}}(t+1|t) & = & \Phi {\bf \hat{x}}(t|t) \label{eqntux} \\
P(t+1|t) & = & \Phi P(t|t) {\Phi} ^ T + Q(t) \label{eqntup}
\end{eqnarray}
where $ {\bf \hat{x}}(t+1|t) $ denotes one-step state prediction
at time $ (t+1) $
based on  the measurements at time $t$. The state estimate at time $t$
denoted by ${\bf {\hat{x}}}(t|t) $ represents the estimate based on the
measurements at time $t$. The state transition matrix $ \Phi $ 
is defined in Equation (\ref{eqnphi}) . The state estimation error covariance
matrix is given by
$ P(t|t) = E \{ {{\bf \hat{x}}(t|t)}{{\bf \hat{x}} ^ T (t|t) } \} $.
The state prediction covariance matrix is defined as $ P(t+1|t) = 
E \{ {{\bf \hat{x}}(t+1|t)}{{\bf \hat{x}} ^ T (t+1|t)} \} $.  The 
time update of the covariance matrix $P(t+1|t)$ accounts
for modeling errors and disturbances by a suitable choice of $Q(t)$
in Equation (\ref{eqntup}) . 


\subsection{Jacobian Update}

The measurement model in Equation (\ref{model}) is nonlinear and is 
linearized  at the optimal state estimate $ {\bf \hat{x}}(t-1|t-1) $ at
time $t-1$ so that it can be used at $t$ in the measurement update. 
Let $H$ denote the Jacobian matrix of ${\bf h(x)}$ evaluated at 
${\bf \hat{x}}(t-1|t-1)$.
\begin{eqnarray}
H & = & \frac { \partial {\bf h(x)}}  { \partial {\bf x}} 
|  {{\bf{x}}  = {{\bf \hat{x}}(t-1|t-1)}} \label{eqnjac}
\end{eqnarray}

\subsection{Measurement Update}

The  measurement update corrects the predicted state and error
covariance using the measurement ${\bf y}(t)$ and the measurement noise
covariance matrix $R(t)$ as follows:
The prediction error known as {\em innovations}
is denoted by ${\bf e}(t)$ and defined as
\begin{displaymath}
{\bf e}(t) = {\bf y}(t)-{\bf h}({\bf \hat{x}}(t|t-1)
\end{displaymath}
The state estimate is given as
\begin{equation}
{\bf \hat{x}}(t|t)={\bf \hat{x}}(t|t-1)+K(t){\bf e}(t) \label{eqnmux}
\end{equation}
The covariance of the innovations is denoted by $R_e(t)$ and defined by
\begin{equation}
R_e(t)=HP(t|t-1)H ^ T +R(t) \label{eqninnovcov}
\end{equation}
The Kalman gain matrix $K(t)$ and the covariance matrix $P(t|t)$ are
given by
\begin{eqnarray}
K(t) & = & P(t|t-1) H^T  { R_e(t)} ^ {-1} \label{eqngain} \\
P(t|t) & = & P(t|t-1) - K(t)HP(t|t-1) \label{eqnmup}
\end{eqnarray}
The initial conditions to start the recursive computations are given by
$ {\bf \hat{x}}(0|0) = {\bf x}(0) $ and $P(0|0)=P(0)$, the {\em a priori}
estimates.  In practice, the first few measurements are used to determine
the initial conditions. 


\begin{figure}
\setlength{\unitlength}{0.14in}
\begin{center}
\begin{picture}(25,10)(1,0)
\scriptsize
\put(4,6){\framebox(5,3){\shortstack{Measurement\\Update\\(range)}}}
\put(11,6){\framebox(5,3){\shortstack{Measurement\\Update\\(azimuth)}}}
\put(18,6){\framebox(5,3){\shortstack{Measurement\\Update\\(elevation)}}}
\put(11,2){\framebox(5,2){Jacobian}}
% x
\multiput(2,8)(7,0){4}{\vector(1,0){2}}
\put(2.3,8.3){\makebox(0,0)[b]{$\hat{x}(t|t-1)$}}
\put(10,8.3){\makebox(0,0)[b]{$\hat{x}^{(1)}$}}
\put(17,8.3){\makebox(0,0)[b]{$\hat{x}^{(2)}$}}
\put(24,8.3){\makebox(0,0)[b]{$\hat{x}(t|t)$}}
% p
\multiput(2,7)(7,0){4}{\vector(1,0){2}}
\put(2.3,6.4){\makebox(0,0){$P(t|t-1)$}}
\put(10,6.4){\makebox(0,0){$P^{(1)}$}}
\put(17,6.4){\makebox(0,0){$P^{(2)}$}}
\put(24,6.4){\makebox(0,0){$P(t|t)$}}
% jac
\put(6.5,3){\vector(0,1){3}}
\put(13.5,4){\vector(0,1){2}}
\put(20.5,3){\vector(0,1){3}}
\put(6.5,3){\line(1,0){4.5}}
\put(16,3){\line(1,0){4.5}}
\put(13.5,1){\vector(0,1){1}}
\put(13.5,0){\makebox(0,0)[b]{$\hat{x}(t-1|t-1)$}}
\put(7,5){\makebox(0,0)[l]{$H_1$}}
\put(14,5){\makebox(0,0)[l]{$H_2$}}
\put(21,5){\makebox(0,0)[l]{$H_3$}}
\end{picture}
\end{center}
\caption{Measurement Update}
\label{measblock}
\end{figure}

\begin{figure}
\setlength{\unitlength}{0.155in}
\begin{center}
\begin{picture}(20,13)(1,0)
\scriptsize
\put(1,8){\framebox(5,3){\shortstack{Jacobian\\(eq. \ref{eqnjac})}}}
\put(8,8){\framebox(5,3){\shortstack{Innovation\\Covariance\\(eq. \ref{eqngaink})}}}
\put(15,8){\framebox(5,3){\shortstack{Innovation\\Error\\(eq. \ref{eqnmuxk})}}}
\put(1,3){\framebox(5,3){\shortstack{Covariance\\Update\\(eq. \ref{eqnmupk})}}}
\put(8,3){\framebox(5,3){\shortstack{Gain\\Computation\\(eq. \ref{eqngaink})}}}
\put(15,3){\framebox(5,3){\shortstack{State\\Update\\(eq. \ref{eqnmuxk})}}}
%top
\put(3.5,12){\vector(0,-1){1}}
\put(3.5,12.2){\makebox(0,0)[b]{$\hat{x}(t-1|t-1)$}}
\put(9,12){\vector(0,-1){1}}
\put(9,12.2){\makebox(0,0)[b]{$y_{i}(t)$}}
\put(12,12){\vector(0,-1){1}}
\put(12,12.2){\makebox(0,0)[b]{$P^{(i-1)}$}}
\put(17.5,12){\vector(0,-1){1}}
\put(17.5,12.2){\makebox(0,0)[b]{$\hat{x}^{(i-1)}$}}
%middle
\put(6,9.5){\vector(1,0){2}}
\put(7,9.7){\makebox(0,0)[b]{$H_i$}}
\put(10.5,8){\vector(0,-1){2}}
\put(11,7){\makebox(0,0)[l]{$R_e^{(i)}$}}
\put(17.5,8){\vector(0,-1){2}}
\put(18,7){\makebox(0,0)[l]{$e^{(i)}$}}
\put(8,4.5){\vector(-1,0){2}}
\put(7,4.7){\makebox(0,0)[b]{$K^{(i)}$}}
\put(13,4.5){\vector(1,0){2}}
\put(14,4.7){\makebox(0,0)[b]{$K^{(i)}$}}
%bottom
\put(3.5,3){\vector(0,-1){1}}
\put(3.5,1.8){\makebox(0,0)[t]{$P^{(i)}$}}
\put(17.5,3){\vector(0,-1){1}}
\put(17.5,1.8){\makebox(0,0)[t]{$\hat{x}^{(i)}$}}
\end{picture}
\end{center}
\caption{$ith$ Scalar Measurement Update}
\label{scalarblock}
\end{figure}

\subsection{Scalar Measurement Processing}
In the measurement update, Equation (\ref{eqngain}) requires the inverse
of an $(m \times m)$  matrix that can be avoided by processing 
each measurement individually \cite{bierman77}.  Let $y_i(t)$ and
$h_i({\bf \hat{x}})$ denote the $ith$ element of vectors ${\bf {y}}(t)$ and
${\bf h(\hat{x})}$ respectively. Let $H_i$ denote the $ith$ row of
the measurement partial derivative matrix $H$. Then Equation (\ref{eqnmux})
can be written as
\begin{eqnarray}
{\bf \hat{x}}^{(0)} & = & {\bf \hat{x}}(t|t-1) \nonumber \\
e ^{(i)} &      = & y_{i}(t) - h_{i}({\bf \hat{x}}(t|t-1)) \nonumber \\
{\bf \hat{x}}^{(i)} & = & {\bf \hat{x}}^{(i-1)} + K^{(i)}e^{(i)},
~i = 1,..., m~~ \label{eqnmuxk} \\
{\bf \hat{x}}(t|t) & = & {\bf \hat{x}}^{(m)} \nonumber
\end{eqnarray}
Equation (\ref{eqnmup}) can be written as 
\begin{eqnarray}
P^{(0)} & = & P(t|t-1) \nonumber \\ 
P^{(i)} & = & P^{(i-1)} - K^{(i)}H_{i}P^{(i-1)}, ~i = 1,...,m \label{eqnmupk} \\
P(t|t) & = & P^{(m)} \nonumber
\end{eqnarray}
Equation (\ref{eqninnovcov}) can be written as 
\begin{displaymath}
R_e^{(i)} = H_iP^{(i-1)}H_i^T+R_i(t)
\end{displaymath}
And Equation (\ref{eqngain}) can be written as 
\begin{eqnarray}
K^{(i)} &=& P^{(i-1)}H_i^T (R_e^{(i)})^{-1} \label{eqngaink} 
\end{eqnarray}			
The computations for the range, azimuth angle, and elevation angle
measurements using the scalar measurement algorithm for target
tracking are summarized in Figure \ref{measblock}.  A block diagram of
the $ith$ scalar update is given in Figure \ref{scalarblock}.

\section{Design Issues}
The Kalman equations can be characterized at a course level by the
{\em computation graph} in Figure \ref{compgraph}.  For our purposes, a
computation graph is a directed acyclic graph consisting of a starting
vertex, $s$, which has no input edges, an ending vertex, $e$, which
has no output edges, and a collection of computation vertices, which
represent a partial ordering of computations.  Vertices are labeled
with types of computations; edges are labeled with input data items.
An edge $(u,v)$ labeled with $A$ indicates that $u$ produces $A$ and
$v$ consumes $A$. Edge $(u,v)$ also represents a weak precedence
relation between $u$ and $v$ in that $v$ is guaranteed not to complete
its computation until $u$ has produced all the elements of $A$.

\begin{figure}
\setlength{\unitlength}{0.15in}
\begin{center}
\begin{picture}(25,45)(0,0)
\scriptsize
%nodes
\put(15,45){\makebox(0,0){$\bullet$}}
\put(14.55,46){\fbox{s}}

\put(10,40){\makebox(0,0){$\bullet$}}
\put(7.5,39){\fbox{\em tux}}

\put(15,40){\makebox(0,0){$\bullet$}}
\put(12.75,40){\fbox{\em jac}}

\put(20,40){\makebox(0,0){$\bullet$}}
\put(20.5,39){\fbox{\em tup}}

\put(5,35){\makebox(0,0){$\bullet$}}
\put(5.5,35){\fbox{\em trans}}

\put(15,35){\makebox(0,0){$\bullet$}}
\put(12,35){\fbox{\em gain}}

\put(5,30){\makebox(0,0){$\bullet$}}
\put(3.75,30){\fbox{s}}

\put(10,30){\makebox(0,0){$\bullet$}}
\put(7.25,29){\fbox{\em mux}}

\put(20,30){\makebox(0,0){$\bullet$}}
\put(20.5,29){\fbox{\em mup}}

\put(5,25){\makebox(0,0){$\bullet$}}
\put(5.5,25){\fbox{\em trans}}

\put(15,25){\makebox(0,0){$\bullet$}}
\put(12,25){\fbox{\em gain}}

\put(5,20){\makebox(0,0){$\bullet$}}
\put(3.75,20){\fbox{s}}

\put(10,20){\makebox(0,0){$\bullet$}}
\put(7.25,19){\fbox{\em mux}}

\put(20,20){\makebox(0,0){$\bullet$}}
\put(20.5,19){\fbox{\em mup}}

\put(5,15){\makebox(0,0){$\bullet$}}
\put(5.5,15){\fbox{\em trans}}

\put(15,15){\makebox(0,0){$\bullet$}}
\put(12,15){\fbox{\em gain}}

\put(5,10){\makebox(0,0){$\bullet$}}
\put(3.75,10){\fbox{s}}

\put(10,10){\makebox(0,0){$\bullet$}}
\put(7.25,9){\fbox{\em mux}}

\put(20,10){\makebox(0,0){$\bullet$}}
\put(20.5,9){\fbox{\em mup}}

\put(15,5){\makebox(0,0){$\bullet$}}
\put(13.5,3.75){\fbox{\em e(xout)}}

%circles
\put(5,40){\circle{.75}}
\put(5,40){\makebox(0,0){3}}

\put(13,38){\circle{.75}}
\put(13,38){\makebox(0,0){2}}

\put(17,38){\circle{.75}}
\put(17,38){\makebox(0,0){1}}

\put(5,27.5){\circle{.75}}
\put(5,27.5){\makebox(0,0){3}}

\put(15,30){\circle{.75}}
\put(15,30){\makebox(0,0){1}}

\put(5,17.5){\circle{.75}}
\put(5,17.5){\makebox(0,0){3}}

\put(15,20){\circle{.75}}
\put(15,20){\makebox(0,0){2}}

%labeled edges
\put(15,45){\vector(-1,-1){5}}
\put(10.25,43){\makebox(0,0){$\hat{x}(t-1|t-1)$}}

\put(15,45){\vector(0,-1){5}}
\put(15.5,42){\makebox(0,0){$\hat{x}$}}

\thicklines
\put(15,45){\vector(1,-1){5}}
\thinlines
\put(19.75,43){\makebox(0,0){$P(t-1|t-1)$}}

\put(10,40){\line(-1,0){4.5}}
\put(7.5,40.5){\makebox(0,0){$\hat{x}^{(0)}$}}

\put(5,39.5){\vector(0,-1){4.5}}
\put(15,40){\vector(-1,-1){1.75}}

\put(15,40){\vector(0,-1){5}}
\put(14.5,37){\makebox(0,0){$H_1$}}

\put(15,40){\vector(1,-2){5}}
\put(18.5,34.5){\makebox(0,0){$H_1$}}
\put(15,40){\vector(1,-1){1.75}}

\multiput(20,40)(0,-10){3}{\vector(-1,-1){5}}
\thicklines
\put(20,10){\vector(-1,-1){5}}
\thinlines
\put(18,37){\makebox(0,0){$P^{(0)}$}}
\put(18,27){\makebox(0,0){$P^{(1)}$}}
\put(18,17){\makebox(0,0){$P^{(2)}$}}
\put(18.35,7){\makebox(0,0){$P(t|t)$}}

\thicklines
\multiput(20,40)(0,-10){3}{\vector(0,-1){10}}
\thinlines
\put(21,35){\makebox(0,0){$P^{(0)}$}}
\put(21,25){\makebox(0,0){$P^{(1)}$}}
\put(21,15){\makebox(0,0){$P^{(2)}$}}

\multiput(10,40)(0,-10){3}{\vector(0,-1){10}}
\put(10.85,35){\makebox(0,0){$\hat{x}^{(0)}$}}
\put(10.85,25){\makebox(0,0){$\hat{x}^{(1)}$}}
\put(10.85,15){\makebox(0,0){$\hat{x}^{(2)}$}}

\multiput(5,35)(0,-10){3}{\vector(1,-1){5}}
\put(8,33){\makebox(0,0){$h_1$}}
\put(8,23){\makebox(0,0){$h_2$}}
\put(8,13){\makebox(0,0){$h_3$}}

\multiput(5,30)(0,-10){3}{\vector(1,0){5}}
\put(7.5,30.5){\makebox(0,0){$y_1$}}
\put(7.5,20.5){\makebox(0,0){$y_2$}}
\put(7.5,10.5){\makebox(0,0){$y_3$}}

\multiput(15,35)(0,-10){3}{\vector(-1,-1){5}}
\put(13,32){\makebox(0,0){$K^{(1)}$}}
\put(13,22){\makebox(0,0){$K^{(2)}$}}
\put(13,12){\makebox(0,0){$K^{(3)}$}}

\multiput(15,35)(0,-10){3}{\vector(1,-1){5}}
\put(16.75,32){\makebox(0,0){$K^{(1)}$}}
\put(16.75,22){\makebox(0,0){$K^{(2)}$}}
\put(16.75,12){\makebox(0,0){$K^{(3)}$}}

\multiput(5,27)(0,-10){2}{\vector(0,-1){2}}

\multiput(15,29.5)(0,-10){2}{\vector(0,-1){4.6}}
\put(14,27.5){\makebox(0,0){$H_2$}}
\put(14,17.5){\makebox(0,0){$H_3$}}

\multiput(15.15,29.65)(0,-10){2}{\vector(1,-2){4.85}}
\put(18.5,24.25){\makebox(0,0){$H_2$}}
\put(18.5,14.25){\makebox(0,0){$H_3$}}

\put(10,10){\vector(1,-1){5}}
\put(11,7){\makebox(0,0){$\hat{x}(t|t)$}}

\end{picture}

\scriptsize
\begin{tabular}{|l|l|l|l|}
\hline
Vertex & Description & Eqn \# & Complexity \\
\hline
\hline
gain & Kalman gain computation & \ref{eqngaink} & $O(n^2)$ \\
jac & Jacobian transform & \ref{eqnjac} & $O(mn)$ \\
mup & scalar measurement update of $P$ & \ref{eqnmupk} & $O(n^2)$ \\
mux & scalar measurement update of $\hat{x}$ & \ref{eqnmuxk} & $O(n)$ \\
trans & coordinate transform & \ref{eqnmuxk} & $O(m)$ \\
tup & time update of $P$ & \ref{eqntup} & $O(n^3)$ \\
tux & time update of $\hat{x}$ & \ref{eqntux} & $O(n^{2})$ \\
\hline
\end{tabular}
\vspace{.25in}
\end{center}
\caption{Kalman Filter Computation Graph}
\label{compgraph}
\end{figure}

The problem to be solved is the design of a fast parallel Kalman
filtering algorithm for a linear array of computational cells.
Inspection of Figure \ref{compgraph} shows that the Kalman filter
computations are an eclectic mix of matrix operations ranging in
complexity from linear to cubic in the number of states.  But linear
arrays have traditionally been applied to regular and uniform
computations.  Thus, in a more general sense, the problem to be solved
is the mapping of an irregular and non-uniform computation onto a
linear array.
 
It is easy to see that the covariance updates 
(Equations \ref{eqntup}) and (\ref{eqnmupk})) require most
of the computations.  This motivates the first and rather obvious
design goal:
\begin{quote}
{\em Focus on covariance matrix updates.}
\end{quote}
Along with their large computational requirements, the covariance
updates in Figure \ref{compgraph} have the largest input and output
requirements. This motivates the second design goal:
\begin{quote}
{\em Minimize data transfers involving covariance.}
\end{quote}
On systems like the Warp computer, the data transfer rate between the
external host and the array of cells is an order of magnitude less
than the transfer rate between cells.  This fact, along with the fact
that many of the inputs in Figure \ref{compgraph} are small vectors
and matrices motivates the third design goal:
\begin{quote}
{\em Minimize data transfers between host and array.}
\end{quote}
The next section describes a parallel algorithm for the Kalman
filter that achieves these goals.

\section{The Algorithm}
The algorithm solves the $n$-state Kalman filter equations using $n+2$
computational cells. The algorithm is appropriate for any $n$-state
filter with a constant, sparse, and upper-triangular state transition
matrix, and constant process noise and measurement noise covariance
matrices. 

A high-level view of the computational cells used by the algorithm is
given in Figure \ref{arch}. Boxes represent computational cells.
Arrows represent unidirectional buffered communication channels. Data
flows from left to right along the upper channel and from right to
left along the lower channel. The measurement vector ${\bf y}(t)$ arrives
from an unspecified host along the upper channel. The updated state
vector ${\bf \hat{x}}(t|t)$ is sent back to the host along the lower channel.

\begin{figure}
\setlength{\unitlength}{0.175in}
\begin{center}
\begin{picture}(20,5)(1,0)
\scriptsize
\put(5,1){\framebox(2,2){\em x-cell}}
\put(8,1){\framebox(2,2){\em k-cell}}
\put(11,1){\framebox(2,2){\shortstack{{\em p-cell}\\{\em 1}}}}
\put(14,1){\makebox(2,2){$\cdots$}}
\put(17,1){\framebox(2,2){\shortstack{{\em p-cell}\\{\em n}}}}
\put(3,2.5){\vector(1,0){2}}
\put(5,1.5){\vector(-1,0){2}}
\put(4,3.1){\makebox(0,0){$y(t)$}}
\put(4,1.1){\makebox(0,0){$\hat{x}(t|t)$}}
\multiput(7,2.5)(3,0){4}{\vector(1,0){1}}
\multiput(8,1.5)(3,0){4}{\vector(-1,0){1}}
\end{picture}
\end{center}
\caption{High-level View of Algorithm}
\label{arch}
\end{figure}

The algorithm uses $n+2$ computational cells: $n$ {\em p-cells}, a
{\em k-cell}, and an {\em x-cell}. The $kth$ {\em p-cell} contains the
$kth$ column of the covariance matrix $P$, which it updates in place
(Equations (\ref{eqntup}) and (\ref{eqnmupk})). The {\em k-cell} computes
Kalman gain vectors (Equation (\ref{eqngaink})) which are distributed to
the {\em x-cell} and the {\em p-cells}. The {\em x-cell} contains the
state vector ${\bf \hat{x}}$, which it updates in place (Equations
(\ref{eqntux}) and (\ref{eqnmuxk}).  Figure \ref{detail} summarizes the assignment of the
vertices in the computation graph onto the cells. Notice that vertices
along the right hand side of the computation graph are assigned to the
{\em p-cells}, vertices down the middle of the graph are assigned to
the {\em k-cell}, and vertices along the left hand side of the graph
are assigned to the {\em x-cell}.

\subsection{P-cell Algorithm}
The $kth$ {\em p-cell} contains the $kth$ column of
the covariance matrix and is responsible for performing
the time and measurement update of that column. 

\paragraph{Covariance Time Update:}
The key computation in the covariance time update (Equation
(\ref{eqntup})) is the solution of
\begin{equation}
A = \Phi P \Phi^T \label{eqnmap1}.
\end{equation}
The state transition matrix for the tracking filter is of the form
\begin{displaymath}
\Phi = 
\left[
\begin{array}{ccccccccc}
1 &  &  &\delta&  &  &\sigma&  & \\
  &1 &  &  &\delta&  &  &\sigma& \\
  &  &1 &  &  &\delta&  &  &\sigma\\
  &  &  &1 &  &  &\delta&  & \\
  &  &  &  &1 &  &  &\delta& \\
  &  &  &  &  &1 &  &  &\delta\\
  &  &  &  &  &  &\alpha&  & \\
  &  &  &  &  &  &  &\alpha& \\
  &  &  &  &  &  &  &  &\alpha\\
\end{array}
\right]
\end{displaymath}
where $\alpha$, $\delta$, and $\sigma$ are constants. From linear algebra 
we know that for each $a_{ij}$ in Equation (\ref{eqnmap1})
\begin{equation}
a_{ij} = \sum_{k=1}^{n} \sum_{\zeta = 1}^{n} p_{k\zeta}\phi_{ik}\phi_{j\zeta}.
\label{eqnmap2}
\end{equation}
Now let 
\begin{displaymath}
M^{(ij)} = (\Phi_{i})^{T}(\Phi_{j}^T)^T \label{eqnmap4}
\end{displaymath}
where $\Phi_i$ is the $ith$ row of $\Phi$, and $\Phi_j^T$ is the $jth$
column of $\Phi^T$. Then Equation (\ref{eqnmap2}) can be rewritten as
\begin{equation}
a_{ij} = \sum_{k=1}^{n} \sum_{\zeta = 1}^{n} p_{k\zeta}m_{k\zeta}^{(ij)}.
\label{eqnmap3}
\end{equation}
Matrix $M^{(ij)}$ is an outer product that defines a 1-1 mapping
between the elements of $P$ and product pairs from $\Phi$.  This
mapping suggests a technique, called the {\em outer product method}
\cite{ohallaron88c}, for generating efficient expressions that solve
each $a_{ij}$ in Equation (\ref{eqnmap1}): 1) For each $i$ and
$j$, compute $M^{(ij)}$. 2) Use the non-zero elements of
$M^{(ij)}$ to form the expression for $a_{ij}$ according to Equation
(\ref{eqnmap3}).
\begin{figure}
\setlength{\unitlength}{0.1in}
\begin{center}
\begin{picture}(30,25)(3,0)
\put(5,4){\framebox(5,18)}
\put(15,10){\framebox(5,12)}
\put(25,10){\framebox(5,12)}
\thinlines
\scriptsize
%p-cells
\multiput(25,19)(0,-3){3}{\line(1,0){5}}
\put(25,19){\makebox(5,3){\em tup}}
\put(25,16){\makebox(5,3){\em mup}}
\put(25,13){\makebox(5,3){\em mup}}
\put(25,10){\makebox(5,3){\em mup}}
%k-cell
\multiput(15,19)(0,-3){3}{\line(1,0){5}}
\put(15,19){\makebox(5,3){\em jac}}
\put(15,16){\makebox(5,3){\em gain}}
\put(15,13){\makebox(5,3){\em gain}}
\put(15,10){\makebox(5,3){\em gain}}
%x-cell
\multiput(5,19)(0,-3){5}{\line(1,0){5}}
\put(5,19){\makebox(5,3){\em tux}}
\put(5,16){\makebox(5,3){\em trans}}
\put(5,13){\makebox(5,3){\em mux}}
\put(5,10){\makebox(5,3){\em mux}}
\put(5,7){\makebox(5,3){\em mux}}
\put(5,4){\makebox(5,3){\em xout}}
\put(7.5,2){\makebox(0,0){(a) {\em x-cell}}}
\put(17.5,2){\makebox(0,0){(b) {\em k-cell}}} 
\put(27.5,2){\makebox(0,0){(c) {\em p-cell}}} 
\end{picture}
\end{center}
\caption{Assignment of Vertices to Cells}
\label{detail}
\end{figure}

For example, consider the expression for $a_{12}$. First, we compute
matrix $M^{(12)}$.
\begin{displaymath}
M^{(12)} = 
\left[
\begin{array}{ccccccccc}
0  &1 &0  &0  &\delta&0  &0  &\sigma&0 \\
0  &0  &0  &0  &0  &0  &0  &0  &0 \\
0  &0  &0  &0  &0  &0  &0  &0  &0 \\
0  &\delta&0  &0  &\delta^2&0  &0&\delta\sigma&0 \\
0  &0  &0  &0  &0  &0  &0  &0  &0 \\
0  &0  &0  &0  &0  &0 &0  &0  &0 \\
0  &\sigma&0  &0  &\delta\sigma&0&0&\sigma^2&0 \\
0  &0  &0  &0  &0  &0  &0  &0&0 \\
0  &0  &0  &0  &0  &0  &0  &0  &0\\
\end{array}
\right]
\label{phistruct}
\end{displaymath}
Next, we use $M^{(12)}$ to derive the expression for $a_{12}$:
\begin{eqnarray}
a_{12} &=& p_{12} + p_{42}\delta + p_{72}\sigma + p_{15}\delta + 
p_{45}\delta^2 + p_{75}\delta\sigma + \nonumber \\
& & p_{18}\sigma + p_{48}\delta\sigma + p_{78}\sigma^2 \nonumber
\end{eqnarray}
Let $s(s_i)$ be the number of non-zero elements in $\Phi(\Phi_i)$.
Let $u(u_i)$ be the number of unit elements in $\Phi(\Phi_i)$, where $\Phi_i$
is the $ith$ row of $\Phi$. Using the outer product method, each
$a_{ij}$ requires $s_{i}s_{j} - u_{i}u_{j}$ scalar multiplications, and the 
total number of scalar multiplications to solve Equation (\ref{eqnmap1}) is
\begin{displaymath}
\sum_{i}\sum_{j} (s_{i}s_{j} - u_{i}u_{j}) = s^2 - u^2.
\end{displaymath}
In the worst case, where $s=n^2$ and $u=0$, the number of scalar
multiplications to solve Equation (\ref{eqnmap1}) using the outer
product method is $n^4$, compared with $2n^3$ using conventional
matrix multiplication. Thus, the outer product method is appropriate
when $s \leq 2n^{3/2}$. Besides being useful for exploiting the
sparsity of $\Phi$, the method enjoys the advantages of being simple,
algorithmic, and non-symbolic; using the outer product method, it
would be trivial to write a C program that inputs the transition
matrix and generates the C code that solves Equation (\ref{eqnmap1}).

The expressions for updating the $kth$ column of $P$ (Equation
(\ref{eqntup})) are easily derived using the outer product method
described above. Let $p=P_k$ be the $kth$ column of $P$ residing at
the $kth$ {\em p-cell}. Let $q=P_{k+3}$ and let $r=P_{k+6}$. Then for
$k=1,2,3$:
\begin{equation}
p_i =
\left\{
\begin{array}{ll}
p_i + (p_{i+3}+q_{i})\delta + (p_{i+6} + r_{i})\sigma + & \\
~~(q_{i+6}+r_{i+3})\delta\sigma + q_{i+3}\delta^2 + & \\
~~~~r_{i+6}\sigma^2 & ~i=1,2,3 \\
& \\
p_i + (p_{i+3}+q_{i})\delta + r_{i}\sigma +  & \\
~~r_{i+3}\delta\sigma + q_{i+3}\delta^2  & ~i=4,5,6 \\
& \\
p_i\alpha + q_{i}\alpha\delta + r_{i}\alpha\delta & 
~i=7,8,9
\end{array}
\right.
\label{eqntup1}
\end{equation}
And for $k=4,5,6$:
\begin{equation}
p_i =
\left\{
\begin{array}{ll}
p_i + (p_{i+3}+q_{i})\delta + p_{i+6}\sigma + & \\
~~q_{i+6}\delta\sigma + q_{i+3}\delta^2 & ~i=1,2,3 \\
& \\
p_i + (p_{i+3}+q_{i})\delta + q_{i+3}\delta^2  & ~i=4,5,6 \\
& \\
p_i\alpha + q_{i}\alpha\delta & ~i=7,8,9
\end{array}
\right.
\label{eqntup2}
\end{equation}
And for $k=7,8,9$:
\begin{equation}
p_i =
\left\{
\begin{array}{ll}
p_i\alpha + p_{i+3}\alpha\delta + p_{i+6}\alpha\sigma & ~i=1,2,3 \\
& \\
p_i\alpha + p_{i+3}\alpha\delta & ~i=4,5,6 \\
& \\
p_i\alpha^2 & ~i=7,8,9
\end{array}
\right.
\label{eqntup3}
\end{equation}
The parallel algorithm for the time update is straightforward.  Before
the time update, the $kth$ {\em p-cell} contains the $kth$ column of $P$.
\begin{center}
\begin{tabular}{rccccccccc}
cell:&1&2&3&4&5&6&7&8&9\\ 
p:&$P_1$&$P_2$&$P_3$&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$\\ 
\end{tabular}
\end{center}
First, the columns of $P$ are shifted three cells to the left and
the result is stored in vector $q$.
\begin{center}
\begin{tabular}{rccccccccc}
cell:&1&2&3&4&5&6&7&8&9\\ 
p:&$P_1$&$P_2$&$P_3$&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$ \\
q:&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$&&& \\
\end{tabular}
\end{center}
Next, the columns of $P$ are shifted three more cells to the left and
the result is stored in vector $r$.
\begin{center}
\begin{tabular}{rccccccccc}
cell:&1&2&3&4&5&6&7&8&9\\ 
p:&$P_1$&$P_2$&$P_3$&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$ \\
q:&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$&&& \\
r:&$P_7$&$P_8$&$P_9$&&&&&& \\
\end{tabular}
\end{center}
With the appropriate columns in place, each cell updates its column of
$P$ independently. Cells 1--3 use Equation (\ref{eqntup1}); cells 4--6
use Equation (\ref{eqntup2}); cells 7--9 use Equation (\ref{eqntup3}). 

Compare the algorithm in this section with a known systolic algorithm
for performing matrix multiplication on $n$ linearly-connected
cells\cite{annaratone87e}. The algorithm in \cite{annaratone87e} works
for arbitrary matrices, requires $2n^{3} = 1458$ scalar
multiplications to perform the pair of matrix multiplications in
Equation (\ref{eqnmap1}), and does not exploit the constant and sparse
nature of $\Phi$.  Further, for the rightmost matrix multiplication in
Equation (\ref{eqnmap1}), all $n^2=81$ elements of $(\Phi P)$ must be
streamed through each of the $n$ cells in the array by some external host.

The algorithm described above, on the other hand, is appropriate for
filters with constant, sparse, upper-triangular state transition
matrices.  For the example state transition matrix, where $s=2n=18$,
the algorithm requires only $s^{2}-u^{2} = 288$ scalar multiplications
because it exploits the sparse and constant nature of $\Phi$. Further,
for the target tracking filter, only $2/3 (n^{2})$ elements of $P$
need to be streamed through the array, and there is no interaction
with an external host.

\paragraph{Covariance Scalar Measurement Update:}
The $ith$ scalar measurement update of $P$ (Equation \ref{eqnmup}))
can be rewritten in terms of the $kth$ column of $P$.
\begin{equation}
P_k^{(i)}  =  P_k^{(i-1)} - K^{(i)}H_{i}P_k^{(i-1)}, ~~k = 1,...,n
\label{eqnmupcol}
\end{equation}
Since $H_i$ is a vector, $H_{i}P$ and $PH_{i}^{T}$ are vectors, and since
$P$ is symmetric,
\begin{equation}
(H_{i}P)^T = PH_{i}^T.
\label{eqnmupequal}
\end{equation}
Since a vector stored in computer memory is identical to its
transpose, Equation (\ref{eqnmupequal}) implies that we can compute
$H_{i}P$ once and then use the resulting vector wherever $H_{i}P$ and
$PH_{i}^T$ appear in Equations (\ref{eqnmup}) and (\ref{eqngaink}).
It is easy to see that 
\begin{equation}
H_{i}P = \left[ H_{i}P_{1} ~ H_{i}P_{2} ~\cdots~ H_{i}P_{n} \right] 
\label{eqnhp}
\end{equation}
Equations (\ref{eqnmupcol}), (\ref{eqnmupequal}), and (\ref{eqnhp}) suggest
the parallel algorithm for computing the $ith$ scalar measurement
update of $P$.  Before the $ith$ scalar measurement update of $P$, the
$kth$ {\em p-cell} contains the $kth$ column of $P$:
\begin{center}
\begin{tabular}{rccccccccc}
cell:&1&2&3&4&5&6&7&8&9\\ 
p:&$P_1$&$P_2$&$P_3$&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$ \\
\end{tabular}
\end{center}
The elements of $H_i$ are sent by the {\em k-cell} to the array of {\em p-cells}.
\begin{displaymath}
\cdots ~H_{i2} ~H_{i1} \rightarrow \framebox{1} \rightarrow \framebox{2} 
\rightarrow \cdots \rightarrow \framebox{9}
\end{displaymath}
As $H_i$ flows past the $kth$ {\em p-cell}, the cell accumulates
the $kth$ scalar element of $H_i P$. As scalar elements of $H_i
P$ are computed, they are passed out of the array and back to the {\em k-cell}.
\begin{displaymath}
H_{i}P_1 ~H_{i}P_2 ~\cdots \leftarrow \framebox{1} \leftarrow
\framebox{2}
\leftarrow \cdots \leftarrow \framebox{9}
\end{displaymath}
When the elements of $H_i P$ have been computed, the array of {\em p-cells}
is in the following state.
\begin{center}
\begin{tabular}{rccccccccc}
cell:&1&2&3&4&5&6&7&8&9\\ 
p:&$P_1$&$P_2$&$P_3$&$P_4$&$P_5$&$P_6$&$P_7$&$P_8$&$P_9$ \\
a:&$a_1$&$a_2$&$a_3$&$a_4$&$a_5$&$a_6$&$a_7$&$a_8$&$a_9$ \\
\end{tabular}
\end{center}
where $a_k = H_i P_k$. Next, the {\em k-cell} uses $H_i P$ to
compute the gain vector $K^{(i)}$ (Equation (\ref{eqngaink})) As each
element of $K^{(i)}$ is computed it is sent to the array of {\em
p-cells}.
\begin{displaymath}
\cdots ~K_2^{(i)} ~K_1^{(i)} \rightarrow \framebox{1} \rightarrow \framebox{2} 
\rightarrow \cdots \rightarrow \framebox{9}
\end{displaymath}
As $K^{(i)}$ flows through the array, the $kth$ {\em p-cell} uses
$K^{(i)}$ and $a_k = H_{i} P_k$ to perform the scalar measurement
update of $P_k$ according to Equation (\ref{eqnmupcol}). 

The $ith$ scalar measurement update requires $2n$ scalar multiplications
on each {\em p-cell}. 
\subsection{K-cell Algorithm}
The {\em k-cell} cooperates with the array of {\em p-cells} to compute
the $ith$ Kalman gain vector.

\paragraph{Jacobian Transformation:}
While the {\em p-cells} are computing the covariance time update,
the {\em k-cell} receives the three elements of the state vector
corresponding to target location from the {\em x-cell}.
\begin{displaymath}
z ~y ~x \rightarrow \framebox{\em k-cell}
\end{displaymath}
The Jacobian (Equation (\ref{eqnjac})) is then computed:
\begin{displaymath}
H = 
\left[
\begin{array}{ccccccccc}
\frac{x}{r} &\frac{y}{r} &\frac{z}{r} &0  &0 &0  &0  & 0 &0 \\
\frac{-y}{d^2} &\frac{x}{d^2} &0 &0  &0 &0  &0  & 0 &0 \\
\frac{-xz}{r^{2}d} &\frac{-yz}{r^{2}d} &\frac{d}{r^2} &0  &0 &0  &0  & 0 &0 \\
\end{array}
\right]
\label{eqnjacstruct}
\end{displaymath}
where $d=(x^2 + y^2)^{1/2}$ and $r=(x^2 + y^2 + z^2)^{1/2}$\cite{baheti86}.
The Jacobian transformation requires $O(m)$ multiplications, the exact
number depending on the specific filter.
\paragraph{Gain Computation:}
To compute the $ith$ gain vector $K^{(i)}$ (Equation (\ref{eqngaink})),
the {\em k-cell} sends the elements of $H_i$ to the array of {\em p-cells}.
\begin{displaymath}
\framebox{\em k-cell} \rightarrow ~\cdots ~H_{i2} ~H_{i1} 
\end{displaymath}
After the last element of $H_i$ is sent, the {\em k-cell}
begins to receive elements of $H_i P$ from the {\em p-cells}.
\begin{displaymath}
\framebox{\em k-cell} \leftarrow ~H_{i}P_1 ~H_{i}P_2 ~\cdots 
\end{displaymath}
As the elements of $H_i P$ are received from the array of {\em p-cells}, the
{\em k-cell} computes $K^{(i)}$ according to Equation (\ref{eqngaink}).
The elements of $K^{(i)}$ are then distributed to the {\em p-cells}
and the {\em x-cell}.
\begin{displaymath}
K_1^{(i)} ~K_2^{(i)} ~\cdots \leftarrow \framebox{\em k-cell} \rightarrow
~\cdots ~K_2^{(i)} ~K_1^{(i)} 
\end{displaymath} 
The $ith$ Kalman gain vector requires
$2n$ scalar multiplications on the {\em k-cell}.
\subsection{X-cell Algorithm}
The {\em x-cell} contains the state vector ${\bf \hat{x}}$ and performs
the time and measurement updates of ${\bf \hat{x}}$.

\paragraph{State Time Update:}
While the {\em p-cells} are computing the covariance time update
and the {\em k-cell} is computing the Jacobian matrix, the {\em
x-cell} computes the time update of the state vector and the 
coordinate transformation. The {\em x-cell} first sends the
elements of the state vector corresponding to target location
to the {\em k-cell}.
\begin{displaymath}
\framebox{\em x-cell}\rightarrow ~z ~y ~x
\end{displaymath}
The {\em x-cell} then computes the time update of the state vector
according to Equation (\ref{eqntux}).
\begin{equation}
x_i =
\left\{
\begin{array}{ll}
x_i + x_{i+3}\delta + x_{i+6}\sigma & i=1,2,3 \\
x_i + x_{i+3}\delta & i=4,5,6 \\
x_{i}\alpha & i=7,8,9 \\
\end{array}
\right.
\end{equation}
The time update of the state vector requires $s-u$ multiplications on
the {\em x-cell}.

\paragraph{Coordinate Transform:}
The {\em x-cell} computes the transformation from the Cartesian 
coordinates to the polar coordinates defined by
\begin{eqnarray}
h_1 &= & r \nonumber \\
h_2 &=& \tan ^ {-1} (y/x) \nonumber \\
h_3 & = & \sin ^{-1}  (z/r) \nonumber
\end{eqnarray}
where $r=(x ^ 2 + y ^ 2 + z ^ 2)^{1/2}$\cite{baheti86}.  The
coordinate transform requires $O(m)$ multiplications, the exact number
depending on the implementation of the transcendental functions.
\paragraph{State Scalar Measurement Update:}
For the $ith$ scalar measurement update, the {\em x-cell} receives a
scalar measurement from the host, the elements of the gain vector
$K^{(i)}$ from the {\em k-cell}, and updates ${\bf \hat{x}}$ according to
Equation (\ref{eqnmuxk}).
\begin{displaymath}
y_i ~\rightarrow \framebox{\em x-cell} \leftarrow ~K_1^{(i)} ~K_2^{(i)} ~\cdots 
\end{displaymath} 
The $ith$ scalar measurement update requires $n$ multiplications on
the {\em x-cell}.
\paragraph{Output of State Vector:}
After the last scalar measurement update, the updated state vector
is sent back to the host.
\begin{displaymath}
\hat{x}(t|t) ~\leftarrow \framebox{\em x-cell}
\end{displaymath} 

\section{Mapping on Warp} 
The Warp computer \cite{annaratone87e} (or simply Warp) is a linear
array of 10 or more identical and programmable cells connected to a
general-purpose host.  Warp was designed by H. T. Kung and his
associates at Carnegie Mellon and is manufactured and sold by the
General Electric Company.  Figure \ref{warp} shows the architecture of
the current 10-cell Warp.  A Sun-3 workstation called the {\em master}
is connected via a VME repeater to a pair of {\em clusters}. Each
cluster consists of an MC68020 processor(P) with an MC68881 floating
point coprocessor, 3 Mbytes of data memory(M), 1 Mbyte of program
memory, and a switch(S), all connected by a local VSB bus. The
clusters are known collectively as the external host, so called
because they are external to the Master.

\begin{figure}
\setlength{\unitlength}{0.15in}
\begin{center}
\begin{picture}(30,20)(3,2)
\small
%host
%\multiput(4,25)(6,0){4}{\framebox(4,2){Sun-3}}
\put(13,19){\framebox(4,2){\shortstack{Sun-3\\Master}}}
%\put(6,23){\line(1,0){18}}
%\multiput(6,23)(6,0){4}{\line(0,1){2}}
%\put(15,21){\line(0,1){2}}
\put(15,16){\line(0,1){3}}
%external host
\put(9,16){\line(1,0){12}}
\put(9,13){\framebox(1,2){P}}
\put(11,13){\framebox(1,2){M}}
\put(13,11){\framebox(1,4){S}}
\put(20,13){\framebox(1,2){P}}
\put(18,13){\framebox(1,2){M}}
\put(16,11){\framebox(1,4){S}}
\multiput(9.5,15)(2,0){3}{\line(0,1){1}}
\multiput(16.5,15)(2,0){3}{\line(0,1){1}}
\multiput(9.5,12)(2,0){2}{\line(0,1){1}}
\multiput(18.5,12)(2,0){2}{\line(0,1){1}}
\put(9,12){\line(1,0){4}}
\put(17,12){\line(1,0){4}}
\put(13.5,9){\line(0,1){2}}
\put(16.5,9){\vector(0,1){2}}

%iu and array
%\put(12,8){\framebox(6,1){IU}}
\put(10,4){\framebox(2,2){\shortstack{\em Cell\\0}}}
\put(13,4){\framebox(2,2){\shortstack{\em Cell\\1}}}
\put(15.5,5){$\ldots$}
\put(18,4){\framebox(2,2){\shortstack{\em Cell\\9}}}
%from IU to array
\put(9,9){\line(1,0){4.5}}
\put(21,9){\line(-1,0){4.5}}
\put(9,5.25){\line(0,1){3.75}}
\put(21,5.25){\line(0,1){3.75}}
%X channel X
\put(9,5.25){\vector(1,0){1}}
\put(12,5.25){\vector(1,0){1}}
\scriptsize
\put(12.25,5.5){X}
\small
\put(17,5.25){\vector(1,0){1}}
\put(20,5.25){\line(1,0){1}}
%Y channel Y
\put(12,4.75){\vector(1,0){1}}
\put(13,4.75){\vector(-1,0){1}}
\scriptsize
\put(12.25,4.0){Y}
\small
\put(17,4.75){\vector(1,0){1}}
\put(18,4.75){\vector(-1,0){1}}

%dashed boxes labeling clusters and array
\put(9.5,3.5){\dashbox{.1}(11,3)}
\put(8.5,10.5){\dashbox{.1}(6,6)}
\put(15.5,10.5){\dashbox{.1}(6,6)}
\put(11.5,17){\makebox(0,0){Input Cluster}}
\put(18.5,17){\makebox(0,0){Output Cluster}}
\put(15.3,2.5){\makebox(0,0){Warp Array}}
\end{picture}
\end{center}
\caption{The Warp Computer}
\label{warp}
\end{figure}

The Warp array is a linear array of 10 programmable cells, each
containing a pipelined floating-point adder and multiplier, 8K
instruction words, and 32K data words. The clock cycle of each cell is
200ns. Each functional unit can emit one result per cycle, for a
maximum performance of 5 MFLOPS per functional unit, 10 MFLOPS per
cell, and 100 MFLOPS for the 10-cell Warp array. Each cell is
connected to its nearest neighbor by two 32-bit buffered data channels
(X and Y).  Data flows from left to right along the X channel; the Y
channel can be statically reconfigured to pass data in either
direction. Each cell can transfer up to 20 million 32-bit data words
to its neighboring cells each second.

Warp is programmed in W2, a high-level Algol-like language with {\em
send} and {\em receive} statements for cell/cell and host/array data
transfers.  The W2 compiler inputs a W2 source file, generates
microcode for the Warp array, and generates C code for the
clusters.  Warp is accessed with a programming environment, called
WPE, which provides a programmable shell for interactively running and
debugging W2 programs from any Sun connected via Ethernet to the
Master, as well as a set of lower-level routines that allow
user-written C and Lisp programs to access Warp.

The mapping of the 9-state tracking filter on Warp is 500 lines of W2
code. As shown in Figure \ref{mapping}, each {\em p-cell}, {\em
k-cell}, and {\em x-cell} is assigned a Warp cell. Cell 0 gets {\em
p-cell} 9, Cell 1 gets {\em p-cell} 8, and so forth. Since the current
Warp has only 10 cells, we combine the {\em k-cell} and the {\em
x-cell} on Cell 9.

Data flows on the X channel from left to right; data flows on the Y
channel from right to left. In Figure \ref{arch} the {\em x-cell}
handles all data transfers between the host and the array. Since data
may enter the Warp array only from the left hand side and may exit the
array only from the right hand side, the mapping uses Cell
0 (corresponding to {\em p-cell}~9) to input the measurement vector
${\bf y}(t)$ after the time update of $P$ and to stream it along the X
channel through the array to Cell 9 (corresponding to the {\em
x-cell} and the {\em k-cell}). The updated state vector ${\bf \hat{x}}$
exits the array along the X channel.

\begin{figure}
\setlength{\unitlength}{0.175in}
\begin{center}
\begin{picture}(20,5)(1,0)
\scriptsize
\put(5,1){\framebox(2,2){\shortstack{\em Cell\\0}}}
\put(6,4.25){\makebox(0,0){$\overbrace{}^{\shortstack{{\em p-cell}\\9}}$}}
\put(8,1){\makebox(2,2){$\cdots$}}
\put(11,1){\framebox(2,2){\shortstack{\em Cell\\8}}}
\put(12,4.25){\makebox(0,0){$\overbrace{}^{\shortstack{{\em p-cell}\\1}}$}}
\put(14,1){\framebox(2,2){\shortstack{\em Cell\\9}}}
\put(15,4.25){\makebox(0,0){$\overbrace{}^{\shortstack{{\em k-cell}\\{\em x-cell}}}$}}
\put(3,2.5){\vector(1,0){2}}
\put(4,3.1){\makebox(0,0){$y(t)$}}
\put(16,2.5){\vector(1,0){2}}
\put(17,3.1){\makebox(0,0){$\hat{x}(t|t)$}}
\multiput(7,2.5)(3,0){3}{\vector(1,0){1}}
\multiput(8,1.5)(3,0){3}{\vector(-1,0){1}}
\put(7.5,2.8){X}
\put(7.5,0.9){Y}
\end{picture}
\end{center}
\caption{Mapping on Warp}
\label{mapping}
\end{figure}

The key performance measure for the Kalman filter is {\em sample
time}, that is, the real time required to process one measurement
sample.  To compare the sample time of the Warp filter with a
conventional sequential machine, we used the first 300 samples from a
benchmark trajectory.  We ran a C version of the same Kalman filter,
with compiler optimization enabled on a Sun-3/60 with 68881
floating-point coprocessor. 

The C program does not access disk; all data are loaded with the
program and are available when the program begins execution.  The
sample time for the Sun was obtained using the {\em time} command of
the Unix shell. Total user CPU seconds was divided by a factor of 300
(the number of samples) to arrive at the sample time.  The sample time
for Warp was obtained using a built-in timing facility on the external
host. We noted the time that the input cluster started executing
before processing the first sample and we noted the time that the
output cluster finished executing after processing the last sample.
We divided the difference by 300 to arrive at the sample time. The
Warp sample time includes the time required to transfer data between
the external host and the Warp array, as well as startup times for the
external host and Warp array.

The sample times for Warp and the Sun are shown in Figure \ref{performance}.
There are a number of reasons for the two orders of magnitude speedup
of the Warp filter over the Sun filter. First, each of the Warp cells
is a fast scalar processor. Second, the Warp filter partitions the
covariance matrix updates over $n$ cells and performs these updates
in parallel. Third, the updates of columns of the columns of the
covariance matrix are performed in-place, and except during the first
phase of the covariance time update, elements of the covariance matrix
are not transferred between cells. Finally, by performing all of the
Kalman filter computations on the Warp array, interaction between
the host and the array is minimized.

\begin{figure}
\begin{center}
\begin{tabular}{|c|r|}
\hline
Machine & Sample Time\\
\hline
\hline
Sun-3/60 & 80000 $\mu$s \\
Warp & 282 $\mu$s\\
\hline
\end{tabular}
\end{center}
\caption{Kalman Filter Performance}
\label{performance}
\end{figure}

\section{Conclusions}
We have developed a parallel algorithm for solving an $n$-state Kalman
filter on $n+2$ linearly connected computational cells and we have
implemented the algorithm on the Warp computer.  The algorithm
distributes the expensive covariance matrix updates across $n$ cells
and minimizes data flow through the array and interaction between the
external host and the array.  The algorithm is appropriate for any
Kalman filter where the state transition matrix is constant, upper
triangular, and sparse, and where the process noise and measurement
noise covariance matrices are constant.  The Warp implementation
achieves a measured sample time of 282 $\mu$s, which represents a
speedup of almost 300 over the same Kalman filter running on a Sun
3/60 workstation.

Although the Warp sample times are extremely encouraging, there are
some important issues that still need to be addressed.  First, the
algorithm does not exploit the symmetric nature of the error
covariance matrix.  By ignoring the symmetry of the covariance matrix,
we gain regularity of computation and data flow, but at the expense of
duplicating some computations.  Second, the algorithm is not
appropriate for Kalman filters where the state transition matrix is variable
or nontriangular, nor is it appropriate for filters where the process
noise or measurement noise covariance matrices are variable.
Generalizing the algorithm to include these cases would allow us to
implement a larger collection of filters.  Third, the algorithm is not
applicable to the class of so-called {\em square-root} filters
\cite{bierman77}, where a factorized form of the covariance matrix is
updated to provide better numerical accuracy.

\section{Acknowledgements}
This work was supported in part by Office of Naval Research Contract
N00014-86-K-0607 under CMU subcontract 30511-51189. Thanks to H.T.
Kung for graciously allowing us unlimited use of the Warp computer at
Carnegie Mellon, and to Chuck Fiduccia for a discussion that led to
the algorithm for the covariance time update.

\bibliography{/usr/sol/ohallaro/bib/Defs,/usr/sol/ohallaro/bib/refs,/usr/sol/ohallaro/bib/me}
\end{document}
