\documentstyle[12pt,algorithm]{cmu-art}
\title{Factorized Kalman  Filter Mapping on\\ 
Linear Array Processor}  
\author{Radhakisan S. Baheti and Howard R. Itzkowitz \\
GE Research and Development Center, Schenectady, NY 12301}
\begin{document}

\bibliographystyle{ieeetr}

\maketitle
\begin{abstract} 
A mapping of factorized Kalman filter with  $n$-states and $m$-measurements 
on
($n$+$m$+1)-cell linear array is described.  
The approach is to combine the time and measurement updates of the
covariance matrix and use fast Givens rotations to triangularize an
appropriate matrix.
The fast Givens rotations do not require computations of
arithmatic square-roots and are preferred over Householder
transformations.
The rotations of the rows in this matrix are done in parallel to 
distribute 
the computations.  The cell-to-cell communication is minimized
by arranging the matrix in a near triangular form. 
An extended-Kalman filter, commonly used in target tracking
applications, is implemented to demonstrate the mapping on Warp linear
array.
With the availability of high-performance multi-computer chips in
the near future
, the results open new solutions to
on-board target tracking and other Kalman filtering applications.\\
\ \\
{\bf Keywords:} Kalman filters; target tracking; parallel processing
\end{abstract}

\section{\bf 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 for systolic processing architectures
\cite{andrews81,chen86,jover86,shaffer87b,sung87}. 
However, none of these algorithms have been implemented on systolic
arrays.  A close study of these parallel algorithms reveal that the
implementation requires special hardware, and hence is not cost effective.  
Further, the communication cost in the parallel implementation
has been neglected and that may significantly impact the throughput.

The focus of this paper is to develop parallel factorized Kalman
filters and demonstrate the performance on Warp for a target tracking
application.  In many applications, the factorized filter is preferred
over the standard Kalman algorithm because of round off and numerical
errors in the covariance matrix.  For example, in multi-satellite
orbit determination, it has been demonstrated that the standard Kalman
algorithm requires  double precision computations of the covariance
matrix to achieve the same accuracy obtained by the  factorized filter 
implemented with single precision arithmetic \cite{bierman77}.
 

In section 2, an algorithm for the square root Kalman filter based on
stable orthogonal transformations is described.
In section 3, algorithms using fast Givens rotations are derived
to triangularize a matrix.  A square root filtering algorithm using
fast Givens computations on a linear array processor is given in
section 4 .  
In section 5, an extended-Kalman filter for target tracking application
is described. The results of the tracking filter implementation on Warp 
are given in section 6.

\section{\bf Factorized Filter Algorithm}

In this section, the time and measurement updates for the factorized
filter based on stable orthogonal transformations are described.  A factorization
of the form $LDL^T$ rather than the Cholesky decomposition
$LL^T$ is used to avoid computations of the arithmetic
square roots.  The time and measurement updates are combined and the resulting structure of the matrix is discussed.



A discrete-time state space model is given by
\begin{eqnarray}
	{\bf x}_{t+1}  &=& \Phi _t {\bf x}_t + G_t{\bf w}_t \nonumber \\
	{\bf y}_t	&=& H_t{\bf x}_t + {\bf v}_t		\label{model-y-var}
\end{eqnarray}
where subscript $t$ is the discrete-time, ${\bf x}_t$ is an $(n \times 1)$ 
state vector, and
${\bf y}_t$ is an $(m \times 1)$ measurement vector.  The process noise 
${\bf w}_t$
and measurement noise ${\bf v}_t$ are zero mean Gaussian random sequences.
$\Phi _t , G_t,$ and $H_t $ are time varying matrices of dimension
$(n \times n)$ ,$ (n \times p)$, and $ (m \times n)$, respectively. 

It is assumed that
\begin{eqnarray}
E({\bf w}_t {\bf w}_ {\tau} ^ T ) & = & Q_t \delta _{t \tau} \nonumber \\
E({\bf v}_t {\bf v}_ {\tau} ^ T ) & = & R_t \delta _{t \tau} \nonumber \\
E({\bf w}_t {\bf v}_ {\tau} ^ T ) & = & 0
\end{eqnarray}

The predicted state estimate is given by the recursive relation
\cite{baheti87}
\begin{equation}
{{\bf \hat{x}}}_{t+1} = \Phi _t {\bf \hat{x}}_{t}+K_t[{\bf y}_t-H_t{\bf \hat{x}}_{t}] \label{comb-x}
\end{equation}
The covariance of the innovations $R_{e,t}$ and
the Kalman gain matrix $K_t$ %, and the covariance matrix $P_{t|t}$ are
are given by
\begin{eqnarray}
R_{e,t} &= & H_tP_{t}H_t ^ T + R_t \nonumber \\ 	
K_t & = & \Phi  P_t H_t^T  { R_{e,t}} ^ {-1} 	
\end{eqnarray}
The covariance matrix $P_t$ is given by the Riccati difference
equation
\begin{equation}
P_{t+1}  = \Phi _t P_{t} \Phi _t ^ T + G_tQ_tG_t^{T} - K_tR_{e,t}
K_t^T		\label{time-P}
\end{equation}

A square root algorithm is presented in the following,
where the  covariance update is  computed using matrix
factorization and triangularization \cite{kailath87}.

Consider the matrix
  \[
	S = \left [ \begin{array}{cc}
			R_{e,t}	&  HP_{t} \Phi ^T \\
			\Phi P_{t}H^{T}	& \Phi P_{t} \Phi ^T+GQG^T
		    \end{array}
	    \right ],
  \]
where some of the subscripts are not explicitly shown.
Define the following factorizations:
  \begin{eqnarray*}
	P_{t}	 &=& L \overline{D} L^{T}			\\
	P_{t+1}	 &=& L_{+}D_{+}L_{+}^{T}	\\
	R_{e,t} &=& L_{e}D_{e}L_{e}^{T}.
  \end{eqnarray*}
 The matrix $S$ can be factored as
  \begin{eqnarray*}
    S &=& ADA^{T} \\
      &=&
	\left [ \begin{array}{ccc}
		I & HL & 0		\\
		0 & \Phi L & G
		\end{array}
	\right ] \! \! \! \!
	\left [ \begin{array}{ccc}
		R & 0 & 0	\\
		0 & \overline{D} & 0	\\
		0 & 0 & Q
		\end{array}
	\right ] \! \! \! \!
	\left [ \begin{array}{cc}
		    I		&       0		\\
		L^{T}H^{T}	& L^{T} \Phi ^{T}	\\
		    0		&	G^{T}		
		\end{array}
	\right ],
  \end{eqnarray*}
where $A$ is an ($n$+$m$) x (2$n$+$m$) matrix
and $D$ is a (2$n$+$m$) x (2$n$+$m$) diagonal matrix.
\footnote{For this paper, matrices $R$ and $Q$ are assumed to be diagonal.
If these matrices are not diagonal, the factorization can be changed
so that $D$ remains diagonal.}
Similarly, it is easily verified that $S$ also can be factored as

  \begin{eqnarray*}
    S &=& A'D'(A')^{T} \\
      &=&
	\left [ \begin{array}{ccc}
		L_{e}		&   0	& 0	\\
		K_tL_{e}		& L_{+} & 0
		\end{array}
	\right ] \! \! \! \!
	\left [ \begin{array}{ccc}
		D_{e}	& 0 	& 0	\\
		0 	& D_{+} & 0	\\
		0 	& 0 	& D_{a}
		\end{array}
	\right ] \! \! \! \!
	\left [ \begin{array}{cc}
		L_{e}^{T}	& L_{e}^{T}K_t^{T}	\\
		     0		& 	L_{+}^{T}	\\
		     0		&	    0	
		\end{array}
	\right ]
  \end{eqnarray*}
where $A'$ is an ($n$+$m$) x (2$n$+$m$) unit lower triangular matrix
and $D'$ is a (2$n$+$m$) x (2$n$+$m$) diagonal matrix.
Because of the nature of this factorization, the matrix $D_{a}$ is
arbitrary.

From these two block factorizations of $S$, it is known that there exists 
an orthogonal transformation matrix $\Theta$ such that
  \begin{eqnarray}
  \lefteqn{
	\left [ \begin{array}{ccc}
		I & HL & 0		\\
		0 & \Phi L & G
		\end{array}
	\right ]
	\left [ \begin{array}{ccc}
		R & 0 & 0	\\
		0 & \overline{D} & 0	\\
		0 & 0 & Q
		\end{array}
	\right ]^{\frac{1}{2}} \Theta =
	  }					\nonumber \\
 & & \hspace*{0.5in}
	\left [ \begin{array}{ccc}
		L_{e}		&   0	& 0	\\
		K_tL_{e}		& L_{+} & 0
		\end{array}
	\right ]
	\left [ \begin{array}{ccc}
		D_{e}	& 0 	& 0	\\
		0 	& D_{+} & 0	\\
		0 	& 0 	& D_{a}
		\end{array}
	\right ]^{\frac{1}{2}}
     \label{pre-ADThetaA'D'}
  \end{eqnarray}
or, for a suitably defined $\Psi$,
  \begin{eqnarray}
	\left [ \begin{array}{ccc}
		I & HL & 0		\\
		0 & \Phi L & G
		\end{array}
	\right ]  \Psi &=& 
	\left [ \begin{array}{ccc}
		L_{e}		&   0	& 0	\\
		K_tL_{e}		& L_{+} & 0
		\end{array}
	\right ].
     \label{pre-APsiA'}
  \end{eqnarray}
It is seen that the right side of Equation~\ref{pre-APsiA'}
has a triangular structure.  
Using fast Givens rotations \cite{gentleman73}, we can find $\Psi$
as a product of elementary matrices, none of which contain any scalar
square roots.  The fast Givens technique is discussed in the next
section.

Since matrix $A'$ contains $L_{+}$ and matrix $D'$ contains $D_{+}$, the 
matrix $P_{t+1}$ can be explicitly computed as soon as the triangularization
procedure completes.  However, since the recursive algorithm requires the 
factorized version of $P_{t}$, explicit calculation of the
error covariance matrices is unnecessary.

Since matrix $A'$ contains both $L_{e}$ and
$K_t L_{e}$, the gain matrix can be computed by 
  \[ K_t = [K_t L_{e}][L_{e}]^{-1}. \]
Note that this involves computing the inverse of  
unit lower triangular $m$ x $m$ matrix $L_e$.

\section{Fast Givens Rotations}

In this section we show how to use fast Givens rotations to find
the appropriate matrix $\Psi$ in Equation~\ref{pre-APsiA'}.
The Givens rotation performed on a 1 x 2 vector $A = [a_{1} \;\; a_{2}]$ consists
of post-multiplying vector $A$ by an orthogonal matrix $\Theta$
of the form
  \begin{equation}
	\Theta = \left [
		  \begin{array}{cr}
			\cos \alpha  &  - \sin \alpha \\
			\sin \alpha  &    \cos \alpha
		  \end{array}
		 \right ]
     \label{Theta}
  \end{equation}
to give a new 1 x 2 vector $A^{'} = [a^{'}_{1} \;\; a^{'}_{2}]$.  
 The matrix $\Theta$ can be chosen such that the resulting
vector $A^{'}$ lies along the first coordinate axis (i.e. $a^{'}_2$ = 0).
Thus, a Givens rotation can be used to zero out one particular 
element of the vector.
It is seen that
  \begin{equation}
    [a_{1} \;\; a_{2}] \left [
		   \begin{array}{cc}
			\frac{a_{1}}{\sqrt{a_{1}^{2} + a_{2}^{2}}} &
			\frac{- a_{2}}{\sqrt{a_{1}^{2} + a_{2}^{2}}} \\
			\frac{a_{2}}{\sqrt{a_{1}^{2} + a_{2}^{2}}} &
			\frac{a_{1}}{\sqrt{a_{1}^{2} + a_{2}^{2}}} 
		   \end{array}
		  \right ]
	=
    \left [ \sqrt{a_{1}^{2} + a_{2}^{2}} \;\; 0 \right ],
   \label{appropTheta}
  \end{equation}
therefore an appropriate choice for $\Theta$ can be made.  
As seen in Equation~\ref{appropTheta}, the Givens rotation requires a
square root calculation.  Computation of square roots can be
time consuming, so it is beneficial to minimize the number of square root
calculations.
A triangularization algorithm which eliminates the necessity for computing
square roots is called fast Givens rotations \cite{gentleman73}.
Consider a matrix factorization
  \begin{equation}
	  S = ADA^{T},		\label{M-ADA}
  \end{equation}
where $D$ is a diagonal matrix.  The goal is to transform the factorization in
Equation~\ref{M-ADA} to the factorization
  \begin{equation}
	  S = A^{'}D^{'}(A^{'})^{T},		\label{M-A'D'A'}
  \end{equation}
where $D^{'}$ is a diagonal matrix and $A^{'}$ is unit lower triangular.

\subsection{\bf 1 x 2 Vector}

For the example of $A$ being a 1 x 2 vector, 
Equations~\ref{M-ADA}~and~\ref{M-A'D'A'} can be combined to form the
equation
  \begin{equation}
    [a_{1} \;\; a_{2}] \left [  \begin{array}{cc}
				d_{1}  &    0     \\
				  0    &  d_{2}
			   \end{array}
		  \right ]
		  \left [  \begin{array}{c}
				a_{1} \\
				a_{2}
			   \end{array}
		  \right ]
	=
    [1 \;\; 0] \left [  \begin{array}{cc}
				d^{'}_{1}  &      0     \\
				    0      &  d^{'}_{2}
		   \end{array}
	  \right ]
	  \left [  \begin{array}{c}
				1 \\
				0
		   \end{array}
	  \right ].
     \label{ADA-A'D'A'}
  \end{equation}
In most problems, including square root filtering, the matrices $A$ and $D$
will be given, leaving only matrix $D^{'}$ to be determined.
Performing the matrix multiplication on both sides of Equation~\ref{ADA-A'D'A'}
gives
  \begin{equation}
	d^{'}_{1} = a_{1}^{2}d_{1} + a_{2}^{2}d_{2}.	\label{d'1}
  \end{equation}
Notice that Equation~\ref{d'1} holds for an arbitrary choice of $d^{'}_{2}$.
This fact can be exploited when an appropriate transformation matrix
$\Theta$ is developed.

In order to achieve the factorization specified in Equation~\ref{M-A'D'A'},
we need to find an orthogonal matrix $\Theta$ such that
  \[
    [a_{1} \;\; a_{2}] \left [  \begin{array}{cc}
				d_{1}  &    0     \\
				  0    &  d_{2}
			   \end{array}
		  \right ] ^{\frac{1}{2}}
		  \Theta
	=
    [1 \;\; 0] \left [  \begin{array}{cc}
				d^{'}_{1}  &      0     \\
				    0      &  d^{'}_{2}
		   \end{array}
	  \right ] ^{\frac{1}{2}}.
 \]
This can be stated equivalently as 
  \[  [a_{1} \;\; a_{2}] \Psi = [1 \;\; 0], \]
where
  \begin{equation}
    \Psi =        \left [  \begin{array}{cc}
				d_{1}  &    0     \\
				  0    &  d_{2}
			   \end{array}
		  \right ] ^{\frac{1}{2}}
 	  \Theta
          \left [  \begin{array}{cc}
				d^{'}_{1}  &      0     \\
				    0      &  d^{'}_{2}
		   \end{array}
	  \right ] ^{- \frac{1}{2}}.
     \label{Psi}
  \end{equation}
Using Equation~\ref{d'1}, it is to verify that
  \[
    [a_{1} \;\; a_{2}] \left [ \begin{array}{cr}
				a_{1}d_{1}/d^{'}_{1}  &  - a_{2}  \\
				a_{2}d_{2}/d^{'}_{1}  &    a_{1} 
			  \end{array}
		  \right ]
	= [1 \;\; 0],
  \]
and thus a candidate for $\Psi$ is
  \begin{equation}
	\Psi =
		  \left [ \begin{array}{cr}
				a_{1}d_{1}/d^{'}_{1}  &  - a_{2}  \\
				a_{2}d_{2}/d^{'}_{1}  &    a_{1} 
			  \end{array}
		  \right ].
     \label{Psicandidate}
  \end{equation}

It is necessary to ensure that the matrix $\Theta$ is orthogonal.
Rearranging Equation~\ref{Psi},
  \begin{eqnarray}
    \Theta &=&
                  \left [  \begin{array}{cc}
				d_{1}  &    0     \\
				  0    &  d_{2}
			   \end{array}
		  \right ] ^{- \frac{1}{2}}
		  \left [ \begin{array}{cr}
				a_{1}d_{1}/d^{'}_{1}  &  - a_{2}  \\
				a_{2}d_{2}/d^{'}_{1}  &    a_{1} 
			  \end{array}
		  \right ]
          \left [  \begin{array}{cc}
				d^{'}_{1}  &      0     \\
				    0      &  d^{'}_{2}
		   \end{array}
	  \right ] ^{\frac{1}{2}}
			\nonumber \\
      &=& \left [ \begin{array}{cc}
 	     a_{1} \sqrt{d_{1}/d^{'}_{1}} & - a_{2} \sqrt{d^{'}_{2}/d_{1}} \\
 	     a_{2} \sqrt{d_{2}/d^{'}_{1}} &  a_{1} \sqrt{d^{'}_{2}/d_{2}}
		  \end{array}
	  \right ].
     \label{ThetaDPsiD'}
  \end{eqnarray}
To make $\Theta$ orthogonal, we take advantage of the flexibility in
choosing $d^{'}_{2}$ to let
  \begin{equation}
    d^{'}_{2} = \frac{d_{1}d_{2}}{d^{'}_{1}}.		\label{d'2}
  \end{equation}
From Equation~\ref{ThetaDPsiD'}, it is seen that 
$\Theta$ is orthogonal.
Note that $\Theta$ need not be explicitly determined in order to
find the proper choice for $d^{'}_2$.

The matrix $D^{'}$ is determined by Equations~\ref{d'1}~and~\ref{d'2}.
Since the vector $A^{'} = [1 \;\; 0]$, the factorization is complete.
The fast Givens rotation algorithm is summarized in
Algorithm~\ref{givens-1x2}.  Although the explicit calculation 
of $\Psi$ is unnecessary
for solution of this problem, it is necessary when Givens rotations
are performed on matrices.  

\begin{algorithm}
\begin{tabbing}
Calculate matrix $D^{'}$ xxxxxxxxxx \= \kill
Calculate matrix $D^{'}$ \> [Equations \ref{d'1} and \ref{d'2}] \\
Calculate matrix $\Psi$  \> [Equation \ref{Psicandidate}]
\end{tabbing}
\caption{Fast Givens Rotations: 1 x 2 vector}
\label{givens-1x2}
\end{algorithm}


\subsection{\bf  1 x n Vector}

In the previous section, the fast Givens algorithm is used to zero out
the second element of a 1 x 2 vector.  A natural extension of this
procedure is to use the fast Givens algorithm to rotate
a 1 x $n$ vector $A = [a_{1},a_2, \ldots ,a_{n}]$, resulting in a 1 x $n$
vector $A^{'} = [1 \;\; 0 \; \ldots \; 0]$.
This can be accomplished by zeroing out elements
2 through $n$ of $A$ one at a time.  The matrix $D=$diag.$[d_1,d_2,...,d_n]$.

Consider the $n$~x~$n$ matrix $\Psi _{i,j}$ as
follows:
  \begin{equation}
     \Psi _{i,j} =
	\left [ \begin{array}{ccccrcc}
		1 & \cdots & 0 & \cdots & & \cdots & 0 \\
		\vdots & \ddots &&&&& \vdots \\
		0 &  & a_{i}d_{i}/d^{'}_{i} & & - a_{j} & &  \\
		\vdots &&& \ddots &&& \vdots \\
		&& a_{j}d_{j}/d^{'}_{i} & & a_{i} && \\
		\vdots &&&&& \ddots & \vdots \\
		0 & \cdots && \cdots && \cdots & 1
		\end{array}
	\right ],
     \label{Psiij}
  \end{equation}
where
  \begin{equation}
	d^{'}_{i} = a_{i}^{2}d_{i} + a_{j}^{2}d_{j}.
     \label{d'i}
  \end{equation}
Matrix $\Psi _{i,j}$ differs from the identity matrix in four 
places --- the intersections of rows $i$ and $j$ with
columns $i$ and $j$.
It is seen that
  \begin{equation} 
	[a_{1} \ldots a_{i} \ldots a_{j} \ldots a_{n}] \Psi _{i,j}  =
	[a_{1} \ldots 1 \ldots 0 \ldots a_{n}],
     \label{APsiA}
  \end{equation}
where the $j$th element becomes zero,
the $i$th element becomes unity, and all other elements of $A$ remain
unchanged.  Let
  \begin{equation}
    d^{'}_{j} = \frac{d_{i}d_{j}}{d^{'}_{i}}		\label{d'j}
  \end{equation}
and 
  \begin{equation}
	d^{'}_{k} = d_{k}				\label{d'k}
  \end{equation}
for integers $1 \leq k \leq n$ such that $k \neq i$ and $k \neq j$.
It can be verified that 
 \[ \Theta _{i,j} = D^{- \frac{1}{2}} \Psi _{i,j} (D^{'})^{\frac{1}{2}} \]
results in an orthogonal matrix.  Therefore, by computing the matrix 
$D^{'}$ as specified above, the $j$th element of row vector $A$
is set to zero.


A procedure to zero out elements of a row vector is summarized in
Algorithm~\ref{givens-1xn}.  Vector $A$ is first rotated to zero
out the last element, then the result is rotated to zero out
the next to last element, and so on until $A^{'}$ is achieved by zeroing
out all elements of $A$ (except the first element).
As in the 1 x 2 vector case, the explicit calculation of $\Psi$ 
is unnecessary for solution of this problem.  


\begin{algorithm}
\begin{tabbing}
For \= each element $j$ of $A$ ($j=n,\ldots,2$) \\
\> Calculate $A_{'}$, the rotation of $A$ xxxxxxxx \= \kill
\> Calculate $d^{'}_{1}$ \> [Equation \ref{d'i}] \\
\> Calculate $d^{'}_{j}$ \> [Equation \ref{d'j}] \\
\> Calculate matrix $\Psi _{1,j}$  \> [Equation \ref{Psiij}] \\
\> Calculate $A^{'}$, the rotation of $A$ \> [Equation \ref{APsiA}] \\
\> \> \\
\> Reset $D$ to $D^{'}$ \> \\
\> Reset $A$ to $A^{'}$ \> \\
EndFor
\end{tabbing}
\caption{Fast Givens Rotations: 1 x $n$ vector}
\label{givens-1xn}
\end{algorithm}

\subsection{m x n Matrix}

The procedure developed in the previous section to zero out 
a sequence of elements in a 1 x $n$ vector can be extended to triangularize 
an arbitrary $m$ x $n$ matrix with $m \leq n$.
\footnote{For matrices with $m > n$, the triangularization procedure is
the same except that column vectors must be zeroed out (rather than row
vectors).}
The strategy is to zero out the rows of $A$ one at a time to obtain
a unit lower triangular matrix.

Let $A_{i}$ and $A'_{i}$ represent the $i$th row of the $m$ x $n$ matrices 
$A$ and $A'$, respectively.
The first step is to zero out entries in $A_{1}$, 
which can be done by employing \mbox{Algorithm~\ref{givens-1xn}}.  
Each row of matrix $A$ is multiplied by $\Psi _{1,j}$
  \[
	\left [ \begin{array}{c}
		A_{1} \\
		A_{2} \\
		\vdots  \\
		A_{m} 
		\end{array}
	\right ]
	\Psi _{1,j} =
	\left [ \begin{array}{c}
		A^{'}_{1} \\
		A^{'}_{2} \\
		\vdots  \\
		A^{'}_{m} 
		\end{array}
	\right ].
  \]
The equation $A_{1} \Psi _{1,j} = A^{'}_{1}$ is Equation~\ref{APsiA}
with $i$=1.
The remaining row transformations can be represented by
  \begin{equation}
	[a_{k1} \ldots a_{kj} \ldots a_{kn}] \Psi _{1,j} =
	[a^{'}_{k1} \ldots a^{'}_{kj} \ldots a_{kn}],
     \label{AkPsiAk}
  \end{equation}
where $a_{kj}$ and $a'_{kj}$ represent the $j$th elements of $A_{k}$
and $A'_{k}$, respectively.  Only elements
1 and $j$ of $A_{k}$ change, given by
  \begin{equation}
	a^{'}_{k1} = \frac{a_{11}d_{1}}{d^{'}_{1}}a_{k1} +
			\frac{a_{1j}d_{j}}{d^{'}_{1}}a_{kj}
     \label{pre-a'k1}
  \end{equation}
  \begin{equation}
	a^{'}_{kj} = - a_{1j}a_{k1} + a_{11}a_{kj}.
     \label{pre-a'kj}
  \end{equation}

For $A_{i}$, we need to compute $\Psi _{i,j}$ for all $j > i$ 
in order to complete the matrix triangularization.  
When zeroing $A_{i}$, Equations~\ref{pre-a'k1}~and~\ref{pre-a'kj}
become
  \begin{equation}
	a^{'}_{ki} = \frac{a_{ii}d_{i}}{d^{'}_{i}}a_{ki} +
			\frac{a_{ij}d_{j}}{d^{'}_{i}}a_{kj}
     \label{a'ki}
  \end{equation}
  \begin{equation}
	a^{'}_{kj} = - a_{ij}a_{ki} + a_{ii}a_{kj}.
     \label{a'kj}
  \end{equation}
Note that all rotations of $A_{i}$ affect all other rows.  
However, each row above $A_{i}$
contains zeroes as $i$th and $j$th elements, and as evidenced by
Equations~\ref{a'ki}~and~\ref{a'kj}, these elements will remain as zeroes
for any rotation applied to them.  

A procedure to triangularize the $m$ x $n$ matrix $A$ using fast
Givens rotations is summarized in
Algorithm~\ref{givens-mxn}.  Row $A_{1}$ is first zeroed out, then $A_{2}$,
and so on until a unit lower triangular matrix is achieved by zeroing
out all rows of $A$.
The order in which the elements of $A$ are zeroed out can affect the 
efficiency of the algorithm.

\begin{algorithm}
\begin{tabbing}
For \= For \= For \= each $A_{k}$ below $A_{i}$ ($i < k \leq m$) \= \kill
For each row $A_{i}$ of $A$ ($i=1,\ldots,m$) \\
\> For each element $j$ of $A_{i}$ ($j=n,\ldots,i+1$) \\
\> \> Calculate $d^{'}_{i}$ \> \> [Equation \ref{d'i}] \\
\> \> Calculate $d^{'}_{j}$ \> \> [Equation \ref{d'j}] \\
\> \> Calculate matrix $\Psi _{i,j}$  \> \> [Equation \ref{Psiij}] \\
\> \> Calculate $A^{'}_{i}$, the rotation of $A_{i}$
					 \> \> [Equation \ref{APsiA}] \\
\> \> \\
\> \> For each $A_{k}$ below $A_{i}$ ($k=i+1,\ldots,m$) \\
\> \> \> Calculate $a^{'}_{ki}$ \> [Equation \ref{a'ki}] \\
\> \> \> Calculate $a^{'}_{kj}$ \> [Equation \ref{a'kj}] \\
\> \> \> Reset $A_{k}$ to $A^{'}_{k}$ \> \\
\> \> EndFor \\
\> \> \\
\> \> Reset $A_{i}$ to $A^{'}_{i}$ \> \\
\> \> Reset $D$ to $D^{'}$ \> \\
\> EndFor \\
EndFor 
\end{tabbing}
\caption{Fast Givens Rotations: $m$ x $n$ matrix}
\label{givens-mxn}
\end{algorithm}

\subsection{\bf  Efficiency Improvement to 
		 Algorithm~\protect\ref{givens-mxn}  } 
%\subsection{Efficiency Improvement to Algorithm}

The computation of $a^{'}_{ki}$ and $a^{'}_{kj}$ using 
Equations~\ref{a'ki}~and~\ref{a'kj} requires four multiplications and
two additions for each row $A_{k}$ being rotated.  However, after the
first rotation zeroing an element of row $A_{i}$, notice that 
$a_{ii} = 1$.  Hence, for all subsequent rotations zeroing elements in
$A_{i}$, Equation~\ref{a'kj} can be simplied as
  \begin{equation}
	a^{'}_{kj} = - a_{ij}a_{ki} + a_{kj}.
     \label{a'kj-opt}
  \end{equation}
Using $a_{ii} = 1$ and the results of Equation~\ref{a'kj-opt}, 
Equation~\ref{a'ki} can be written as follows \cite{gentleman73}:
  \begin{eqnarray}
	a^{'}_{ki} &=& \frac{d_{i}}{d^{'}_{i}}a_{ki} +
			\frac{a_{ij}d_{j}}{d^{'}_{i}}a_{kj} \nonumber \\
%		   &=& \frac{d_{i}}{d^{'}_{i}}a_{ki} +
%			\frac{a_{ij}d_{j}}{d^{'}_{i}}(a^{'}_{kj}+a_{ij}a_{ki})
%							 \nonumber \\
		   &=& \frac{d_{i}+a_{ij}^{2}d_{j}}{d^{'}_{i}}a_{ki} +
			  a_{ij} \frac{d_{j}}{d^{'}_{i}} a^{'}_{kj}
							 \nonumber \\
		   &=& a_{ki} + (a_{ij} \frac{d_{j}}{d^{'}_{i}}) a^{'}_{kj}.
     \label{a'ki-opt}
  \end{eqnarray}
Since $a_{ij}d_{j}/d^{'}_{i}$ is an element of the
transformation matrix $\Psi _{i,j}$, this quantity need not be
calculated in Equation~\ref{a'ki-opt}.  Hence, 
Equations~\ref{a'ki-opt}~and~\ref{a'kj-opt} compute the values of
$a^{'}_{ki}$ and $a^{'}_{kj}$, respectively, 
using only two multiplications and two additions.

A triangularization procedure which employs these more efficient 
equations is presented in \mbox{Algorithm~\ref{givens-mxn-opt}}.
The 
\begin{algorithm}
\begin{tabbing}
For \= For \= For \= each $A_{k}$ below $A_{i}$ ($i < k \leq m$) xx \= \kill
For each row $A_{i}$ of $A$ ($i=1,\ldots,m$) \\
\> Calculate $d^{'}_{i}$ \> \> \> [Equation \ref{d'i}] \\
\> Calculate $d^{'}_{n}$ \> \> \> [Equation \ref{d'j}] \\
\> Calculate matrix $\Psi _{i,n}$ \> \> \> [Equation \ref{Psiij}] \\
\> Calculate $A^{'}_{i}$, the rotation of $A_{i}$
					 \> \> \> [Equation \ref{APsiA}] \\
\> \\
\> For each $A_{k}$ below $A_{i}$ ($k=i+1,\ldots,m$) \\
\> \> Calculate $a^{'}_{ki}$ \> \> [Equation \ref{a'ki}] \\
\> \> Calculate $a^{'}_{kn}$ \> \> [Equation \ref{a'kj}] \\
\> \> Reset $A_{k}$ to $A^{'}_{k}$ \> \\
\> EndFor \\
\> \\
\> Reset $A_{i}$ to $A^{'}_{i}$ \> \\
\> Reset $D$ to $D^{'}$ \> \\
\> \\
\> For each element $j$ of $A_{i}$ ($j=n-1,\ldots,i+1$) \\
\> \> Calculate $d^{'}_{i}$ \> \> [Equation \ref{d'i}] \\
\> \> Calculate $d^{'}_{j}$ \> \> [Equation \ref{d'j}] \\
\> \> Calculate matrix $\Psi _{i,j}$  \> \> [Equation \ref{Psiij}] \\
\> \> Calculate $A^{'}_{i}$, the rotation of $A_{i}$
					 \> \> [Equation \ref{APsiA}] \\
\> \> \\
\> \> For each $A_{k}$ below $A_{i}$ ($k=i+1,\ldots,m$) \\
\> \> \> Calculate $a^{'}_{kj}$ \> [Equation \ref{a'kj-opt}] \\
\> \> \> Calculate $a^{'}_{ki}$ \> [Equation \ref{a'ki-opt}] \\
\> \> \> Reset $A_{k}$ to $A^{'}_{k}$ \> \\
\> \> EndFor \\
\> \> \\
\> \> Reset $A_{i}$ to $A^{'}_{i}$ \> \\
\> \> Reset $D$ to $D^{'}$ \> \\
\> EndFor \\
EndFor 
\end{tabbing}
\caption{Improved Fast Givens Rotations 
		\protect\cite{gentleman73} : $m$~x~$n$ vector}
\label{givens-mxn-opt}
\end{algorithm}
improved algorithm is more complicated than \mbox{Algorithm~\ref{givens-mxn}}
due to the separation of the first zeroing rotation of a row from
the rest of the rotations, but the improvement in performance is
very significant.  In fact, Algorithm~\ref{givens-mxn-opt}
triangularizes an $n$ x $n$ matrix ($m$=$n$) in order $\frac{2}{3} n^{3}$
operations \cite{golub83} (compared with order $\frac{4}{3} n^{3}$
operations required by Algorithm~\ref{givens-mxn}).

\section{Square Root Filter on Linear Array}

The problem of interest is systematic design of parallel square root     
Kalman filtering algorithms for a linear array of computational cells.  
For our purposes, a linear array is a collection of separately 
programmable cells, each of which communicates with at most two other
cells.  Linear
arrays have traditionally been applied to regular and uniform computations.
For example, efficient parallel algorithms are known for vector-matrix
multiplications and matrix-matrix multiplications.
The problem of interest here is the mapping of an irregular and non-uniform
computation with specific structure onto a linear array.  

A high-level diagram of a linear array for the filtering algorithm is given
in Figure~\ref{sqr-array}.  The boxes represent computational cells and
the arrows represent unidirectional buffered communication cells.
The measurement vector ${\bf y}_{t}$ arrives from an
unspecified host along the upper channel, and the updated state vector
${\bf \hat{x}}_{t+1}$ is sent back to the host along the lower channel.  The
algorithm uses $n$+$m$+1 computational cells, where $n$+$m$ {\em a-cells}
perform the combined covariance update and a single {\em x-cell} performs
the combined state update.  The details of the mapping are discussed in
reference \cite{baheti89}.  
%  The update algorithms are described in the sections below.

\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){\shortstack{{\em a-cell}\\{\em 1}}}}
\put(11,1){\framebox(2,2){\shortstack{{\em a-cell}\\{\em 2}}}}
\put(14,1){\makebox(2,2){$\cdots$}}
\put(17,1){\framebox(2,2){\shortstack{{\em a-cell}\\{\em n+m}}}}
\put(3,2.5){\vector(1,0){2}}
\put(5,1.5){\vector(-1,0){2}}
\put(4,3.1){\makebox(0,0){${\bf y}_{t}$}}
\put(4,1.1){\makebox(0,0){${\bf \hat{x}}_{t+1}$}}
\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 Linear Array}
\label{sqr-array}
\end{figure}


\subsection{Combined Covariance Update}

The combined update of the covariance matrix $P_{t}$ is performed using
fast Givens rotations of the matrices
  \[ A = 
	\left [ \begin{array}{ccc}
		I & HL & 0		\\
		0 & \Phi L & G
		\end{array}
	\right ]
  \]
  \[ D = 
	\left [ \begin{array}{ccc}
		R & 0 & 0	\\
		0 & \overline{D} & 0	\\
		0 & 0 & Q
		\end{array}
	\right ],
  \]
where $A$ is an ($n$+$m$)x(2$n$+$m$) matrix and $D$ is a
(2$n$+$m$)x(2$n$+$m$) diagonal matrix.
For notational convenience, define $A$ as an $M$ x $N$ matrix.
In order to facilitate the development of the mapping for
fast Givens rotations, it is useful to represent 
Algorithm~\ref{givens-mxn-opt} 
in a condensed manner, shown in Algorithm~\ref{givens-short}.
Below is a textual description of what each procedure listed in 
Algorithm~\ref{givens-short} represents:
\begin{description}
\item[$W_{i,N}$:]  
	Rotate $A_{i}$ to zero out the $N$th element
	and compute resulting $A'_{i}$ and $D'$.
\item[$X_{i,N,k}$:]  
	Rotate row $A_{k}$ of matrix $A$,
	changing only elements $i$ and $N$.
\item[$Y_{i,j}$:]  
	Rotate $A_{i}$ to zero out the $j$th element (when $a_{ii}=1$)
	and compute resulting $A'_{i}$ and $D'$.
\item[$Z_{i,j,k}$:]  
	Rotate row $A_{k}$ of matrix $A$ (when $a_{ii}=1$),
	changing only elements $i$ and $j$.
\end{description}


\begin{algorithm}
\begin{tabbing}
For \= For \= For \= each $A_{k}$ below $A_{i}$ xx \= \kill
For each row $A_{i}$ of $A$ ($i=1,\ldots,M$) \\
\> Procedure $W_{i,N}$ \> \> \> 
	[Equations \ref{d'i},\ref{d'j},\ref{Psiij},\ref{APsiA}] \\
\> \\
\> For each $A_{k}$ below $A_{i}$ ($k=i+1,\ldots,M$) \\
\> \> Procedure $X_{i,N,k}$ \> \>
	[Equations \ref{a'ki},\ref{a'kj}] \\
\> EndFor \\
\> \\
\> For each element $j$ of $A_{i}$ ($j=N-1,\ldots,i+1$) \\
\> \> Procedure $Y_{i,j}$ \> \>
	[Equations \ref{d'i},\ref{d'j},\ref{Psiij},\ref{APsiA}] \\
\> \> \\
\> \> For each $A_{k}$ below $A_{i}$ ($k=i+1,\ldots,M$) \\
\> \> \> Procedure $Z_{i,j,k}$ \>
	[Equations \ref{a'kj-opt},\ref{a'ki-opt}] \\
\> \> EndFor \\
\> \> \\
\> EndFor \\
EndFor 
\end{tabbing}
\caption{Condensed Description of Fast Givens Rotations Algorithm}
\label{givens-short}
\end{algorithm}

A summary of the
data used in the computation and the data changed by each procedure
is presented in Table~\ref{data-used}.
The primes have been dropped from the notation because the
primed variables are moved back into the unprimed variables as soon as all
the computations are completed.

\begin{table}
\begin{center}
\begin{tabular}{||c|c|c||}    \hline
Procedure & Data Used & Data Changed \\  \hline
$W_{i,N}$   & $A_{i}$, \, $D$ &  $A_{i}$, \, $D$, \, $\Psi _{i,N}$	\\
$X_{i,N,k}$ & $A_{k}$, \, $\Psi _{i,N}$	&       $A_{k}$		\\
$Y_{i,j}$     & $A_{i}$, \, $D$	&  $A_{i}$, \, $D$, \, $\Psi _{i,j}$	\\
$Z_{i,j,k}$   & $A_{k}$, \, $\Psi _{i,j}$	&       $A_{k}$		\\
	\hline
\end{tabular}
\end{center}
\caption{Data Used and Changed by Procedures in 
			Algorithm~\protect\ref{givens-short} }
\label{data-used}
\end{table}

From the table, it is apparent that procedure $W_{i,N}$ can be
executed by a cell which stores only $D$ and row $A_{i}$ of matrix $A$.
This cell computes $\Psi _{i,N}$ and communicates it to other cells.

It is also apparent that procedure $X_{i,N,k}$ can be executed by a cell
which can access the rotation matrix $\Psi _{i,N}$ and which stores
only row $A_{k}$ of matrix $A$.  Hence, the procedures $X_{i,N,k}$ 
($i < k \leq M$) can be executed by separate cells in parallel.

The data details for procedures $Y_{i,j}$ and $Z_{i,j,k}$ are similar to
the details for procedures $W_{i,N}$ and $X_{i,N,k}$, respectively.
The only difference is notational, where the rotation matrix
is represented by $\Psi _{i,j}$ rather than $\Psi _{i,N}$.

The above analysis leads to a natural mapping of the algorithm onto a
linear array.  Consider the problem of zeroing row $A_{i}$ of matrix $A$.
What is desired is an array where rows $A_{i}$ through $A_{M}$ are 
stored on a separate cells.  
\footnote{Recall that, in the triangularization algorithm, zeroing
row $A_{i}$ does not affect the rows above it in matrix $A$.  Therefore,
this storage is not considered here.}
Cell $i$ contains
$A_{i}$ and $D$ and performs procedure $W_{i,N}$, computing
$\Psi _{i,N}$ in the process.  Then $\Psi _{i,N}$ is passed to
cells $i$+1 through $M$ so that cell $k$ can appropriately rotate $A_{k}$
using procedure $X_{i,N,k}$.
Subsequently, for each remaining element of $A_{i}$ to be zeroed, 
\begin{enumerate}
\item cell $i$ performs procedure $Y_{i,j}$, 
\item cell $i$ sends $\Psi _{i,j}$ to all cells $k$ ($k=i+1,\ldots,M$), and
\item each cell $k$ performs procedure $Z_{i,j,k}$.
\end{enumerate}

The next step is to extend the above reasoning from the case of zeroing
one row to the case of zeroing all rows (Table~\ref{zero-matrix}).  
The first step of triangularizing
a matrix is zeroing the first row, so the above reasoning applies directly
($i$=1).  Hence, what is required is an array of
$n$+$m$ {\em a-cells}, where {\em a-cell}
$k$ contains row $A_{k}$ of matrix $A$.
Initially, the matrix $D$ is stored on {\em a-cell} 1.  
As described in the previous paragraph, {\em a-cell} 1
performs procedures $W$ and $Y$; {\em a-cells} 2 through $n$+$m$ perform
procedures $X$ and $Z$.  
The subscripts have been dropped for convenience.

\begin{table}
%\scriptsize
%\small
\begin{center}
\begin{tabular}{c|ccccc}
	\hline
Row   & {\em a-cell} 1 & {\em a-cell} 2 & {\em a-cell} 3 & $\cdots$ & 
	 {\em a-cell} $M$ \\ \hline
 1 & $A_{1}$,$D$ & $A_{2}$ & $A_{3}$ & $\cdots$ & 
		 $A_{M}$ \\
      & $W$,$Y$     & $X$,$Z$ & $X$,$Z$ & $\cdots$ & $X$,$Z$ \\
	&&&&& \\
 2 & $A_{1}$ & $A_{2}$,$D$ & $A_{3}$ & $\cdots$ 
		 & $A_{M}$ \\
      &             & $W$,$Y$ & $X$,$Z$ & $\cdots$ &  $X$,$Z$ \\
	&&&&& \\
 3 & $A_{1}$ & $A_{2}$ & $A_{3}$,$D$ & $\cdots$ & 
		 $A_{M}$ \\
      &             &         & $W$,$Y$ & $\cdots$ & $X$,$Z$ \\
  \vdots&&&&& \\
  \vdots&&&&& \\
 $M$-1 & $A_{1}$ & $A_{2}$ & $A_{3}$ & $\cdots$ & 
		 $A_{M}$ \\
      &             &         &  &                 &      $X$,$Z$ \\
        &&&&& \\
 $M$ & $A_{1}$ & $A_{2}$ & $A_{3}$ & $\cdots$ & 
		 $A_{M}$,$D$ \\
      &             &         &  &                 &          $W$,$Y$ \\
		\hline
\end{tabular}
\end{center}
\caption{Data and execution pattern of linear array throughout the
	 fast Givens algorithm.  The first column lists the row currently
	 being zeroed, and the remaining columns show the data stored
	 (first row) and the procedures executed (second row) 
	 on the {\em a-cells} of the array.}
\label{zero-matrix}
\end{table}

Once $A_{1}$ is zeroed, the next step is to zero $A_{2}$.  As shown in 
Table~\ref{zero-matrix}, this requires that {\em a-cell} 2 store matrix $D$.
Therefore, $D$ must be passed to {\em a-cell} 2 by {\em a-cell} 1 
before row $A_{2}$
can be zeroed.  Once this passage is complete, the zeroing process
follows the single row reasoning outlined above ($i$=2).  Hence, 
{\em a-cell} 2 now
performs procedures $W$ and $Y$, and {\em a-cells} 3 through $n$+$m$ perform
procedures $X$ and $Z$.  Since row $A_{1}$
will not be changed by subsequent rotations in the algorithm, {\em a-cell} 1
will be dormant.

This process continues for the zeroing of all rows, as shown in
Table~\ref{zero-matrix}.  Notice that zeroing $A_{n+m}$ does not require
the execution of procedures $X$ and $Z$ (there are no rows in $A$ below
row $A_{n+m}$).  When the triangularization process is complete, the
resulting matrix $A'$ (stored in $A$) is partitioned just as the original
matrix: row $k$ resides on {\em a-cell} $k$.  The new diagonal matrix $D'$ 
(stored in $D$) resides on {\em a-cell} $n$+$m$, 
whereas the initial diagonal matrix resided on {\em a-cell} 1.  
Thus, an algorithm is defined on an $n$+$m$-cell
linear array to compute a unit lower triangular $A'$ and a diagonal $D'$ 
such that Equation~\ref{M-A'D'A'} (repeated here for convenience)
  \[
	  S = A'D'(A')^{T}
  \]
is satisfied.

To set the $A$ matrix for Kalman filtering,
the $i$th {\em a-cell} always contains the following 
matrices:
\begin{center}
\begin{tabular}{rccccccc}
a-cell:	& 1 & 2 & $\cdots$ & $m$ & $m$+1 & $\cdots$ & $m$+$n$		\\
contents:& $Q,R$&&$\cdots$& & $\Phi _{1},G_{1}$ &$\cdots$& $\Phi _{n},G_{n}$,\\
\end{tabular}
\end{center}
where $\Phi _{k}$ and $G_{k}$ represent the $k$th rows of $\Phi$ and $G$,
respectively.  The first {\em a-cell} initializes $D$, so it must contain 
$Q$ and $R$.  Initially, the cells contain the rows of
matrix $L$:
\begin{center}
\begin{tabular}{rccccccc}
a-cell:	& 1 & 2 & $\cdots$ & $m$ & $m$+1 & $\cdots$ & $m$+$n$		\\
contents: & & & $\cdots$ & & $L_{1}$ & $\cdots$ & $L_{n}$,		\\
\end{tabular}
\end{center}
where $L_{k}$ represents the $k$th row of $L$.

To begin the covariance update computation, matrix $H$ is received from
the {\em x-cell} one row at a time such that $H_{k}$, the $k$th row of $H$,
is stored on the $k$th {\em a-cell}.
\begin{displaymath}
\cdots ~H_{2} ~H_{1} \rightarrow \framebox{1} \rightarrow \framebox{2} 
\rightarrow \cdots \rightarrow \framebox{m}
\end{displaymath}
Then the rows of matrix $L$ are distributed to all of the {\em a-cells} so
that the contents of the cells are:
\begin{center}
\begin{tabular}{rccccccc}
a-cell:	& 1 & 2 & $\cdots$ & $m$ & $m$+1 & $\cdots$ & $m$+$n$		\\
contents: & $H_{1},L$ & $H_{2},L$ & $\cdots$ & 
				$H_{m},L$ & $L$ & $\cdots$ & $L$	\\
\end{tabular}
\end{center}
At this point, each cell has all the information it needs to compute its
row of matrix $A$.

Once matrix $A$ is formed, we can triangularize the matrix by performing 
the appropriate fast Givens rotations, and the covariance update is 
complete.  However, it is useful to examine the triangularization process
in two stages:  zeroing the first $m$ rows of $A$ and zeroing the last
$n$ rows of $A$.  After completion of the first stage, the first $m$ rows
and columns of matrix $A$ will not be affected by subsequent rotations.
Since the matrices $L_{e}$ and $K_{t}L_{e}$ 
reside within the first $m$ columns of
the triangularized matrix $A'$, they are completely determined upon completion
of the first stage of the fast Givens process.  Hence, these matrices
may be sent to the {\em x-cell} before the second stage of the 
triangularization is performed.
\begin{displaymath}
L_{e} ~K_{t}L_{e} \leftarrow \framebox{1} \leftarrow
\framebox{2}
\leftarrow \cdots \leftarrow \framebox{n+m}
\end{displaymath}

While the {\em a-cells} compute the combined covariance update, the
{\em x-cell} computes the state update.  This update calculation
is performed in a straight-forward manner represented in 
Equation~\ref{comb-x}.

\section{\bf Target Tracking Kalman Filter}

In this section we describe a Kalman filter for target tracking applications.
Let $n$ denote the number of elements in the state vector and $m$ denote 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.  
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 
time, and $ {\bf x} (t) $ is a nine-dimensional state vector with position,
velocity, and acceleration in each of the Cartesian coordinates axes $x,y,$
and $z$.
The first-order Markov parameters of discretized stochastic acceleration models in
$ x,y,$ and $ z $ axes are denoted by $ ( 3 \times 3 ) $ matrix
\mbox{ $ {\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}
    \Phi _x & 0 & 0  \\
    0 &  \Phi _y       & 0 \\
    0 &  0          &  {\Phi _z }
\end{array}
\right ]
\label{eqnphi}
\end{equation}
where $ \Phi _x $ is $ (3 \times 3) $ matrix given by
\begin{displaymath}
\Phi _x = \left [
\begin{array} {c c c}
1  & \Delta t  & 1/2 (\Delta t) ^ 2 \\
0  &  1        & \Delta t \\
0  &  0        & \lambda _x
\end{array}
\right ]
\end{displaymath}
The symbols $ \Phi _x,\Phi _y, \Phi _z $ denote the state transition matrices
for $x,y$ and $z$ axes, respectively.

The state vector
$ {\bf x}(t) $ is represented by

\begin{displaymath}
{\bf x}(t)= [ x, \dot x,\ddot x,y,\dot y,\ddot y ,z,\dot z,
\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.

\section{\bf 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. 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 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 implementation with a
conventional sequential machine, we used the first 300 samples from a
maneuvering target 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 sample times for Sun and Warp with 1-cell (sequential processing) and
Warp with 10-cells (parallel processing)
are shown in Figure \ref{performance}.
The speedup
is the ratio of 1-cell sample time to 10-cell sample time. 
It is seen that the factorized Kalman filter can be efficiently 
mapped on Warp parallel computer to achieve a speedup of 2.


A mapping of the standard Kalman filter algorithm (using matrix
Riccati difference equation) for target tracking
applications is described in reference \cite{baheti88e}. 
\begin{figure}
\begin{center}
\begin{tabular}{|l|r|}
\hline\hline
  & Filter Sample Time\\
\hline\hline
Sun-3/60 & 80 millisec. \\
\hline
Warp (1 cell) & 1.6 millisec.\\
\hline
Warp (10 cells) & 0.8 millisec. \\
\hline
speedup & 2 \\
\hline\hline
\end{tabular}
\end{center}
\caption{Tracking Filter Performance}
\label{performance}
\end{figure}

\section{\bf Conclusions}
We have developed a parallel algorithm for solving an $n$-state, 
$m$-measurement factorized Kalman
filter on $n+m+1$ linearly connected computational cells. The
factorized filter for a target tracking application has been
implemented on the Warp parallel computer.  
The algorithm distributes the row rotations necessary to compute the
covariance update across $n+m$ cells.  The algorithm also minimizes data 
flow through the array and interaction between the
external host and the array.
The Warp implementation has achieved a speedup of two orders 
of magnitude over the same  filter running on a Sun
3/60 workstation.  The parallel factorized filter algorithm on 10-cell
Warp ran two times faster than the sequential filter algorithm on 1-cell
Warp.


In the current Warp design, each cell is a board. For on-board
applications, iWarp has been designed by Carnegie Mellon
University and Intel Corporation, where each cell computation
and communication function is integrated into a single VLSI
chip \cite{borkar88}.	
Since the algorithms developed here are applicable to the iWarp
architecture, they open new high-speed computing solutions to on-board target tracking
applications.

\section{Acknowledgements}
Thanks to H. T.
Kung for graciously allowing us unlimited use of the Warp computer at
Carnegie Mellon, and to Dave O'Hallaron from GE R \& D center
for many discussions and help that led to
the algorithm and the implementation on Warp.

\bibliography{/usr/droh/bib/defs,/usr/droh/bib/refs,/usr/droh/bib/me}
\end{document}

