Information Formulation of the UDU Kalman Filter

Abstract

A new information formulation of the Kalman filter is presented where the information matrix is parameterized as the product of an upper triangular matrix, a diagonal matrix, and the transpose of the triangular matrix (UDU factorization). The UDU factorization of the Kalman filter is known for its numerical stability, this work extends the technique to the information filter. A distinct characteristic of the new algorithm is that measurements can be processed as vectors, while the classic UDU factorization requires scalar measurement processing, i.e. a diagonal measurement noise covariance matrix.

I. Introduction

The UDU formulation of the Kalman Filter has been used in aerospace engineering applications for several decades. Thornton [1], Bierman and Thornton [2] and Bierman [3] introduced an elegant formulation where the covariance matrix P is replaced by two factors: a diagonal matrix D and an upper triangular matrix U with ones on the main diagonal, such that P = UDU^T. Whereas the UDU factorization improves the computational stability and efficiency of large navigation filters, it was originally used in a batch formulation [4]. However, this formulation lent itself to sequential implementations, well-suited for platforms where both computational stability and numerical efficiency are at a premium. It serves as the backbone of the Orion Navigation System [5].

Factorization of the covariance matrix in a Kalman filter [6] is almost as old as the filter itself. In 1963 James Potter developed a square-root formulation of the Kalman filter to implement on the Apollo onboard computer [7]. The main driver at the time was numerical precision, as computer words were only 8 bits long. Replacing the covariance by a square root matrix S, such as P = SS^T, reduces the spread of the elements of P bringing them closer to 1, doubling the numerical precision of the stored variable. Potter’s algorithm requires the computation of scalar square roots (one per measurement). At the time, the Apollo Kalman filter was designed without any process noise, because computations required for inclusion of the process noise required too many computations [8]. A very desirable by-product of this factorization is that the symmetry and semi-positive definiteness of the covariance are insured by construction, and does not need to be checked or enforced to correct for numerical and round-off errors. It should be noted that this Apollo factorization was not a triangular square root matrix.

An alternative square root covariance factorization is the Cholesky factorization [9], [10]. The Cholesky method is very similar to Potter’s but computes the square root of the covariance matrix with a Cholesky decomposition (S is a triangular matrix) [11]. Another relevant covariance factorization work is that proposed by Oshman and Bar-Itzhack [12], which utilizes the spectral decomposition of the covariance matrix.

The UDU factorization is not a square root filter; the numerical precision of the stored variable does not increase due to the factorization. For example, if P is diagonal, U = I and D = P; therefore the full range of values in P are preserved in this factorization. However, the UDU formulation of the Kalman filter has great numerical stability properties [3]; it insures symmetry of the covariance by construction, and it requires a trivial check and correction to ensure semi-positive definiteness (it suffices to enforce that the diagonal elements of D remain non-negative). The UDU formulation is free from square root operations, making it computationally cheaper than the Cholesky approach. For these reasons the UDU has endured as one of the preferred practical implementation of Kalman filters in aerospace applications.

While the UDU factorization is well known, it has never been applied to the information formulation of the Kalman filter [13], [14], [15]. In this formulation the inverse of the covariance matrix, known as the information matrix, is carried in the recursive algorithm rather than the covariance matrix itself. The information formulation is a popular approach in several situations. In particular, the Square Root Information Filter (SRIF) [16], [17], [18], [3], [19], [20] is a go-to Kalman filter factorization method used in orbit determination packages such as Monte because of its great stability and accuracy. In this work we introduce the UDU Information Filter, a never developed before algorithm with two key properties that make it a very desirable implementation of a recursive estimator: i. unlike the regular UDU filter, measurements do not need to be processed as scalars, i.e. the measurement noise covariance matrix R does not need to be diagonal or diagonalized, and ii. unlike the regular information formulation the state estimation error covariance matrix does not actually need to be inverted.

II. Background

The well-known Kalman filter measurement update equations are given by

$${\widehat{\mathbf{x}}}_k={\stackrel{‒}{\mathbf{x}}}_k+K_k({\mathbf{y}}_k-H_k{\stackrel{‒}{\mathbf{x}}}_k)$$ (1)

$$\begin{gathered} P_k={\stackrel{‒}{P}}_k-{\stackrel{‒}{P}}_k{H}_k^T{(H_k{\stackrel{‒}{P}}_k{H}_k^T+R_k)}^{-1}{H}_k{\stackrel{‒}{P}}_k \\ =(I-K_k{H}_k){\stackrel{‒}{P}}_k \end{gathered}$$ (2)

$$K_k={\stackrel{‒}{P}}_k{H}_k^T{(H_k{\stackrel{‒}{P}}_k{H}_k^T+R_k)}^{-1}=P_k{H}_k^T{R}_k^{-1}$$ (3)

where the bar represents the a priori value, K_k is the n × m Kalman gain, $\mathbf{x}\in {\Re }^n$ is the state vector, P_k is the n × n estimation error covariance matrix, $\mathbf{y}\in {\Re }^m$ is the measurement vector defined as

$${\mathbf{y}}_k=H_k{\mathbf{x}}_k+{\eta }_k$$ (4)

where η_k is a zero mean, white sequence with covariance matrix R_k. The propagation equations are

$${\stackrel{‒}{\mathbf{x}}}_{k+1}=\Phi (t_{k+1},t_k){\widehat{\mathbf{x}}}_k$$ (5)

$$\begin{gathered} {\stackrel{‒}{P}}_{k+1}=\Phi (t_{k+1},t_k)P_k\Phi {(t_{k+1},t_k)}^T+G_k{Q}_k{G}_k^T \\ ={\Phi }_k{P}_k{\Phi }_k^T+G_k{Q}_k{G}_k^T \end{gathered}$$ (6)

where Φ(t_k+1, t_k) (which we will denote as Φ_k;) is the state n × n transition matrix from t_k to t_k+1, Q_k is the p × p process noise covariance matrix, and G_k is the n × p process noise shaping matrix.

The UDU factorization implements the above equations by replacing the covariance matrix P_k with an upper triangular matrix with ones on the diagonal (U_k) and a diagonal matrix D_k, such that

$$P_k=U_k{D}_k{U}_k^T$$ (7)

The UDU approach to propagate U_k and D_k forward in time makes use of the Modified Weighted Gram-Schmidt (MWGS) orthogonalization algorithm that avoids loss of orthogonality due to round-off errors [21]. Measurements are processed one at the time as scalars by noting that when R_k is diagonal the update in Eq. (2) is obtained by recursively processing one element of y_k at a time, using the corresponding row of H_k and diagonal element of R_k. The measurement residual covariance matrix $W_k=H_k{\stackrel{‒}{P}}_k{H}_k^T+R_k$ thus becomes a scalar, and the quantity ${\stackrel{‒}{P}}_k{H}_k^T={\mathbf{w}}_k$ becomes a vector; thus each of the scalar updates takes the form

$$P_k={\stackrel{‒}{P}}_k-\frac{1}{W_k}{\mathbf{w}}_k{\mathbf{w}}_k^T$$ (8)

since matrix P̄_k is updated with a rank one matrix ($\frac{1}{W_k}{\mathbf{w}}_k{\mathbf{w}}_k^T$), we call this a rank one update, Agee and Turner [22] detailed how to directly update the U_k and D_k factors due to a rank one update. The subtraction in Eq. (8) could cause some numerical instabilities in Agee-Turner’s algorithm. Carlson [23] introduced an alternative rank-one update algorithm that, while less generic, is more stable for the measurement update. Carlson’s rank-one update is not valid for generic values of w_k and W_k, but only when the update is done with the optimal Kalman gain.

An alternative formulation of the Kalman filter is the information formulation, where the covariance matrix P is replaced by its inverse. The covariance update and Kalman gain are calculated as [13]

$$P_k^{-1}={\stackrel{‒}{P}}_k^{-1}+H_k^T{R}_k^{-1}{H}_k$$ (9)

$$K_k=P_k{H}_k^T{R}_k^{-1}$$ (10)

The information formulation is particularly useful when there is no prior information, i.e. P₀ = ∞, in this case the covariance formulation of the KF is not defined, while the information formulation is, and starts from $P_0^{-1}=O$. In the covariance formulation, the m × m measurement residual covariance matrix $W_k=H_k{\stackrel{‒}{P}}_k{H}_k^T+R_k$ is inverted to process the measurement, while in the information formulation the n × n covariance matrix is inverted in the time propagation step. In situations when m > n, therefore, the information formulation could be computationally cheaper, although measurements are often processed one at the time as scalars. Processing measurements as scalars is only possible when R_k is diagonal, otherwise the additional steps of a change of variables to diagonalize R_k is required.

III. The UDU Information Filter

A. The Measurement Update

Begin with factorizing the covariance P into an LDL form; that is, rather than using an upper triangular matrix we will use a lower triangular matrix. We denote the diagonal matrix with Δ

$$P_k=L_k{\Delta }_k{L}_k^T\text{and}{\overline{P}}_k={\overline{L}}_k{\overline{\Delta }}_k{\overline{L}}_k^T$$ (11)

and we define U and D as the inverses of L^T and Δ, respectively

$$P_k^{-1}=L_k^{-T}{\Delta }_k^{-1}{L}_k^{-1}=U_k{D}_k{U}_k^T$$ (12)

$${\overline{P}}_k^{-1}={\overline{L}}_k^{-T}{\overline{\Delta }}_k^{-1}{\overline{L}}_k^{-1}={\overline{U}}_k{\overline{D}}_k{\overline{U}}_k^T$$ (13)

so that the measurement update (Eq. (9)) becomes

$$U_k{D}_k{U}_k^T={\overline{U}}_k{\overline{D}}_k{\overline{U}}_k^T+H_k^T{R}_k^{-1}{H}_k$$ (14)

We now factorize the m × m matrix R_k into LDL form as

$$R_k=L_{R_k}{\Delta }_{R_k}{L}_{R_k}^T$$ (15)

and

$$R_k^{-1}=L_{R_k}^{-T}{\Delta }_{R_k}^{-1}{L}_{R_k}^{-1}=U_{R_k}{D}_{R_k}{U}_{R_k}^T$$ (16)

so that Eq. (14) becomes

$$U_k{D}_k{U}_k^T={\overline{U}}_k{\overline{D}}_k{\overline{U}}_k^T+H_k^T{U}_{R_k}{D}_{R_k}{U}_{R_k}^T{H}_k$$ (17)

We now work on the term U_Rk H where we note that it is of dimension m × n so that it can be expressed as

$$U_{R_k}^T{H}_R=\left[\begin{array}{c}\hfill {\mathbf{v}}_1^T\hfill \\ \hfill {\mathbf{v}}_2^T\hfill \\ \hfill ⋮\hfill \\ \hfill {\mathbf{v}}_m^T\hfill \end{array}\right]$$ (18)

where each v_i is an n × 1 vector.

The factor $H_k^T{R}_k^{-1}{H}_k$ can be expressed as

$$\begin{gathered} H_k^T{R}_k^{-1}{H}_k=H_k^T{U}_{R_k}{D}_{R_k}{U}_{R_k}^T{H}_k \\ ={\left[\begin{array}{c}\hfill {\mathbf{v}}_1^T\hfill \\ \hfill {\mathbf{v}}_2^T\hfill \\ \hfill ⋮\hfill \\ \hfill {\mathbf{v}}_m^T\hfill \end{array}\right]}^T\left[\begin{array}{cccc}\hfill 1∕d_{1_R}\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 1∕d_{2_R}\hfill & \hfill \cdots \hfill & \hfill 0\hfill \\ \hfill ⋮\hfill & \hfill ⋮\hfill & \hfill \ddots \hfill & \hfill ⋮\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 1∕d_{m_R}\hfill \end{array}\right]\left[\begin{array}{c}\hfill {\mathbf{v}}_1^T\hfill \\ \hfill {\mathbf{v}}_2^T\hfill \\ \hfill ⋮\hfill \\ \hfill {\mathbf{v}}_m^T\hfill \end{array}\right] \\ =\sum _{i=1}^m\frac{1}{d_{i_R}}{\mathbf{v}}_i{\mathbf{v}}_i^T \end{gathered}$$ (19)

so that the measurement update equation is now

$$U_k{D}_k{U}_k^T={\overline{U}}_k{\overline{D}}_k{\overline{U}}_k^T+\sum _{i=1}^m\frac{1}{d_{i_R}}{\mathbf{v}}_i{\mathbf{v}}_i^T$$ (20)

Thus it reduces to a series of m rank-one updates.

Notice that Eq. (20) is the update equation due to a vector measurement so that the need to process scalar measurements, as in the covariance UDU formulation, is avoided in the proposed information UDU formulation. Rather than performing an eigenvalue decomposition of R_k and a corresponding change of variables for y_k, we simply perform the UDU factorization of $R_k^{-1}$.

As stated earlier, one of the benefits of using an information formulation is that if P₀ is singular, this allows for an estimate to be obtained [14]. When P₀ is singular, x₀ is not completely defined. To this end, ẑ_k and z̄_k, which are directly related to x̂_k and x̄_k, are defined as

$${\widehat{\mathbf{z}}}_k\triangleq P_k^{-1}{\widehat{\mathbf{x}}}_k,{\stackrel{‒}{\mathbf{z}}}_k\triangleq {\stackrel{‒}{P}}_k^{-1}{\stackrel{‒}{\mathbf{x}}}_k$$ (21)

Premultiplying Eq. (1) by $P_k^{-1}$, we get

$${\widehat{\mathbf{z}}}_k=P_k^{-1}(I-K_k{H}_k){\stackrel{‒}{\mathbf{x}}}_k+P_k^{-1}{K}_k{\mathbf{y}}_k$$ (22)

$$={\stackrel{‒}{\mathbf{z}}}_k+H_k^T{R}_k^{-1}{\mathbf{y}}_k$$ (23)

B. The Time Update

Prior to propagation, the standard information formulation of the Kalman filter inverts the information matrix to obtain the covariance, it then propagates the covariance with Eq. (6), and finally inverted the propagated covariance matrix to prepare for the measurement update phase. We propose an algorithm that propagates the factors of the information matrix directly. Starting from the covariance propagation Eq. (6), we factorize Q_k via a UDU parameterization so that

$$Q_k=U_{Q_k}{\Delta }_{Q_k}{U}_{Q_k}^T$$ (24)

where Δ_Qk is a diagonal p × p matrix and define G_Qk as

$$G_{Q_k}\triangleq G_k{U}_{Q_k}$$ (25)

so that P̅_k becomes

$${\overline{P}}_{k+1}={\Phi }_k{P}_k{\Phi }_k^T+G_{Q_k}{\Delta }_{Q_k}{G}_{Q_k}^T$$ (26)

Invoking the matrix inversion lemma

$${(Z+\mathit{XAY})}^{-1}=Z^{-1}-Z^{-1}X{(A^{-1}+Y{Z}^{-1}X)}^{-1}Y{Z}^{-1}$$ (27)

and letting

$$Z={\Phi }_k{P}_k{\Phi }_k^T;A={\Delta }_{Q_k};X=G_{Q_k};Y=G_{Q_k}^T$$ (28)

and

$$Z^{-1}=M_k\triangleq {\Phi }_k^{-T}{P}_k^{-1}{\Phi }_k^{-1}$$ (29)

The inverse of the propagated covariance is

$${\overline{P}}_{k+1}^{-1}=M_k-M_k{G}_{Q_k}{\left[G_k^T{M}_k{G}_{Q_k}+{\Delta }_{Q_k}^{-1}\right]}^{-1}{G}_{Q_k}^T{M}_k$$ (30)

Defining

$${\overline{G}}_k\triangleq {\Phi }_k^{-1}{G}_{Q_k}{D}_{Q_k}={\Delta }_{Q_k}^{-1}$$ (31)

$P_{k+1}^{-1}$ becomes

$${\overline{P}}_{k+1}^{-1}={\Phi }_k^{-T}\left\{P_k^{-1}-P_k^{-1}{\overline{G}}_k{\left[{\overline{G}}_k^T{P}_k^{-1}{\overline{G}}_k+D_{Q_k}\right]}^{-1}{\overline{G}}_k^T{P}_k^{-1}\right\}{\Phi }_k^{-1}$$ (32)

and defining ${\mathcal{K}}_k$ as

$${\mathcal{K}}_k\triangleq P_k^{-1}{\overline{G}}_k{\left[{\overline{G}}_k^T{P}_k^{-1}{\overline{G}}_k+D_{Q_k}\right]}^{-1}$$ (33)

Defining the quantity inside the brackets in Eq. (32) as ${\mathcal{P}}_k^{-1}$

$$\begin{gathered} {\mathcal{P}}_k^{-1}\triangleq P_k^{-1}-P_k^{-1}{\overline{G}}_k{\left[{\overline{G}}_k^T{P}_k^{-1}{\overline{G}}_k+D_{Q_k}\right]}^{-1}{\overline{G}}_k^T{P}_k^{-1} \\ =\left[I-{\mathcal{K}}_k{\overline{G}}_k^T\right]P_k^{-1} \end{gathered}$$ (34)

We notice Eq. (34) is an analog to Eq. (2) with

$$\begin{array}{c}\hfill {\mathcal{P}}_k^{-1}\to P_k\hfill \\ \hfill P_k^{-1}\to {\stackrel{‒}{P}}_k\hfill \end{array}\begin{array}{c}\hfill {\overline{G}}_k\to H_k^T\hfill \\ \hfill {\mathcal{K}}_k\to K_k\hfill \end{array}\begin{array}{c}\hfill D_{Q_k}\to R_k\hfill \\ \hfill \hfill \end{array}$$

and since D_Qk is a diagonal p × p matrix, we can solve for the UDU factorization of ${\mathcal{P}}_k^{-1}$ directly by using a Carlson Rank-One Update [23] performed p times on

$$\begin{gathered} {\mathcal{U}}_k{\mathcal{D}}_k{\mathcal{U}}_k^T=U_k{D}_k{U}_k^T- \\ {U}_k{D}_k{U}_k^T{\overline{G}}_k{\left[{\overline{G}}_k^T{U}_k{D}_k{U}_k^T{\overline{G}}_k+{\Delta }_{Q_k}^{-1}\right]}^{-1}{\overline{G}}_k^T{U}_k{D}_k{U}_k \end{gathered}$$ (35)

so that we can find the time-propagated UDU factors of ${\overline{P}}_k^{-1}$ as

$${\overline{P}}_{k+1}^{-1}={\overline{U}}_{k+1}{\overline{D}}_{k+1}{\overline{U}}_{k+1}^T={\Phi }_k^{-T}{\mathcal{U}}_k{\mathcal{D}}_k{\mathcal{U}}_k^T{\Phi }_k^{-1}$$ (36)

Since this equation is equivalent to a covariance propagation without process noise, the MWGS orthogonalization algorithm can be used to obtain the factors U̅_k+1 and D̅_k+1.

Notice that ${\Phi }_k^{-1}$ does not necessarily need to be computed by a direct matrix inversion. Usually Φ_k is computed via integration of a matrix differential equation or by series approximation. Similarly, ${\Phi }_k^{-1}$ can be obtained directly by backwards integration or by series approximation.

Beginning with Eq. (5) the time update for z̄_k+1 is obtained as follows

$${\stackrel{‒}{P}}_{k+1}^{-1}{\stackrel{‒}{\mathbf{x}}}_{k+1}={\stackrel{‒}{P}}_{k+1}^{-1}{\Phi }_k{P}_k{P}_k^{-1}{\widehat{\mathbf{x}}}_k$$ (37)

which becomes

$${\stackrel{‒}{\mathbf{z}}}_{k+1}={\stackrel{‒}{P}}_{k+1}^{-1}{\Phi }_k{P}_k{\widehat{\mathbf{z}}}_k$$ (38)

substituting from Eqs. (32) and (34)

$${\stackrel{‒}{\mathbf{z}}}_{k+1}={\Phi }_k^{-T}\left[I-{\mathcal{K}}_k{\overline{G}}_k^T\right]{\widehat{\mathbf{z}}}_k$$ (39)

or

$${\stackrel{‒}{\mathbf{z}}}_{k+1}={\Phi }^{-T}(t_{k+1},t_k)\left[I-{\mathcal{K}}_k{\overline{G}}_k^T\right]{\widehat{\mathbf{z}}}_k$$ (40)

The following table summarizes the UDU Information Filter. While Table I contains a compact notation for the covariance time propagation and measurement update; in the actual algorithm the covariance factors U_k and D_k are individually propagated and updated using the Rank-1 Update and the Modified Weighted Gram-Schmidt orthogonalization algorithms.

TABLE I.

Summary of UDU Information Filter Equations

Initialization
State	z0 = P−1(t0)x(t0)
Covariance	$U_0{D}_0{U}_0^T=P^{-1}(t_0)$
Time Propagation
Truth	xk+1 = Φkxt + Gkνk, νk ∼ n(0, Qk)
Process Noise	$U_{Q_k}{\Delta }_{Q_k}{U}_{Q_k}^T=Q_k,{\overline{G}}_k={\Phi }_k^{-1}{G}_k{U}_{Q_k}$
Gain	${\mathcal{K}}_k=U_k{D}_k{U}_k^T{\overline{G}}_k{\left[{\overline{G}}_k^T{U}_k{D}_k{U}_k^T{\overline{G}}_k+{\Delta }_{Q_k}\right]}^{-1}$
Covariance	${\mathcal{U}}_k{\mathcal{D}}_k{\mathcal{U}}_k^T=\lfloor I-{\mathcal{K}}_k{\overline{G}}_k^T\rfloor U_k{D}_k{U}_k^T$ (Rank-1 Update) ${\overline{U}}_{k+1}{\overline{D}}_{k+1}{\overline{U}}_{k+1}^T={\Phi }_k^{-T}{\mathcal{U}}_k{\mathcal{D}}_k{\mathcal{U}}_k^T{\Phi }_k^{-1}$ (MWGS)
State	${\overline{\mathbf{z}}}_{k+1}={\Phi }_k^{-T}\mid I-{\mathcal{K}}_k{\overline{G}}_k^T\mid {\widehat{\mathbf{z}}}_k$
Measurement Update
Truth	yk+1 = Hk+1xk + ηk+1, ηk+1 ∼ n(0, Rk+1
Meas. Noise	$$\begin{gathered} U_{R_{k+1}}{D}_{R_{k+1}}{U}_{R_{k+1}}^T=R_{k+1}^{-1} \\ {\sum }_{i=1}^m\frac{1}{d_{i_R}}{\mathbf{v}}_i{\mathbf{v}}_i^T=H_{k+1}^T{U}_{R_{k+1}}{D}_{R_{k+1}}{U}_{R_{k+1}}^T{H}_{k+1} \end{gathered}$$
Covariance	$$\begin{gathered} U_{k+1}{D}_{k+1}{U}_{k+1}^T={\overline{U}}_{k+1}{\overline{D}}_{k+1}{\overline{U}}_{k+1}^T+ \\ +{\sum }_{i=1}^m(1∕d_{i_R}){\mathbf{v}}_i{\mathbf{v}}_i^T \end{gathered}$$ (Rank-1 Update)
State	${\widehat{\mathbf{z}}}_{k+1}={\overline{\mathbf{z}}}_{k+1}+H_{k+1}{R}_{k+1}^{-1}{\mathbf{y}}_{k+1}$

C. An Efficient Algorithm to compute U⁻¹

The algorithm proposed does not necessitate to invert the covariance matrix nor its U or L factor. However, in case the initial covariance was provided, it might be convenient to factorize it first, and to efficiently invert its factors rather than inverting the full covariance. In this section we compute the inverse in an efficient manner, taking advantage of the ‘1’s’ and ‘0’s’. It is as follows: Given an n × n upper triangular ‘unit’ matrix U expressed as

$$U=\left[\begin{array}{cccccc}\hfill 1\hfill & \hfill U_{1,2}\hfill & \hfill U_{1,3}\hfill & \hfill \cdots \hfill & \hfill U_{1,n-1}\hfill & \hfill U_{1,n}\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill U_{2,3}\hfill & \hfill \cdots \hfill & \hfill U_{2,n-1}\hfill & \hfill U_{2,n}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill \cdots \hfill & \hfill U_{3,n-1}\hfill & \hfill U_{3,n}\hfill \\ \hfill ⋮\hfill & \hfill ⋮\hfill & \hfill ⋮\hfill & \hfill \ddots \hfill & \hfill ⋮\hfill & \hfill ⋮\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 1\hfill & \hfill U_{n-1,n}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right]$$ (41)

the inverse is also an n × n upper triangular ‘unit’ matrix V (so that det (V) = 1) which is

$$V=U^{-1}=\left[\begin{array}{cccccc}\hfill 1\hfill & \hfill V_{1,2}\hfill & \hfill V_{1,3}\hfill & \hfill \cdots \hfill & \hfill V_{1,n-1}\hfill & \hfill V_{1,n}\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill V_{2,3}\hfill & \hfill \cdots \hfill & \hfill V_{2,n-1}\hfill & \hfill V_{2,n}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 1\hfill & \hfill \cdots \hfill & \hfill V_{3,n-1}\hfill & \hfill V_{3,n}\hfill \\ \hfill ⋮\hfill & \hfill ⋮\hfill & \hfill ⋮\hfill & \hfill \ddots \hfill & \hfill ⋮\hfill & \hfill ⋮\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 1\hfill & \hfill V_{n-1,n}\hfill \\ \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill \cdots \hfill & \hfill 0\hfill & \hfill 1\hfill \end{array}\right]$$ (42)

since

$$\mathit{UV}=I$$ (43)

and the ij-th element of UV is given by

$$\begin{gathered} \sum _{k=i}^j{U}_{i,k}{V}_{k,j}=U_{i,i}{V}_{i,j}+U_{i,j}{V}_{j,j}+\sum _{k=i+1}^{j-1}{U}_{i,k}{V}_{k,j}= \\ =V_{i,j}+U_{i,j}+\sum _{k=i+1}^{j-1}{U}_{i,k}{V}_{k,j} \end{gathered}$$ (44)

we can solve for the elements of V as

$$\begin{gathered} j=n,\cdots ,2,i=j-1,\cdots ,1 \\ {V}_{i,j}=-\left[U_{i,j}+\sum _{k=i+1}^{j-1}{U}_{i,k}{V}_{k,j}\right] \end{gathered}$$ (45)

IV. A Numerical Example

In this section we show the performance of the algorithm in linear, time-varying example with correlated measurement noise covariance matrix R. The system is given by

$${\mathbf{x}}_{k+1}={\Phi }_k{\mathbf{x}}_k+{\nu }_k$$ (46)

$${\mathbf{y}}_k=H_k{\mathbf{x}}_k+{\eta }_k$$ (47)

$${\Phi }_k=\left[\begin{array}{cc}\hfill I\hfill & \hfill A_k\hfill \\ \hfill B_k\hfill & \hfill I\hfill \end{array}\right]$$ (48)

$$A_k=\left[\begin{array}{cc}\hfill t_k-t_{k-1}\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill t_k-t_{k-1}\hfill \end{array}\right]$$ (49)

$$B_k=0.1\left[\begin{array}{cc}\hfill \sin (t_k)-\sin (t_{k-1})\hfill & \hfill -(\cos (t_k)-\cos (t_{k-1}))\hfill \\ \hfill 0\hfill & \hfill \sin (t_k)-\sin (t_{k-1})\hfill \end{array}\right]$$ (50)

$$H_k=\left[\begin{array}{cccc}\hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill & \hfill 0\hfill \\ \hfill 0\hfill & \hfill 1\hfill & \hfill 0\hfill & \hfill 0\hfill \end{array}\right]$$ (51)

where I is the identity matrix t_k − t_k-1 = 1 second, ν_k is a zero mean, Gaussian white sequence with covariance matrix Q_k = 0.01 I, and η_k is a zero mean, Gaussian white sequence with covariance matrix R_k

$$R_k=\left[\begin{array}{cc}\hfill 2.96\hfill & \hfill 2.8\hfill \\ \hfill 2.8\hfill & \hfill 2.96\hfill \end{array}\right]$$ (52)

The initial estimate is unbiased, and the initial estimation error is Gaussian with covariance P₀ = I. Fig. 1 shows the result of a single run and the 3σ predicted standard deviations from a straight formulation of a Kalman filter. In order to show the equivalence between the Kalman filter (KF) and the UDU information approach (UDUI), Fig. 2 shows the norm of the difference between the two state estimates

$${ϵ}_{\mathbf{x}}=\Vert {\widehat{\mathbf{x}}}_{\mathit{KF}}-{\widehat{\mathbf{x}}}_{\mathit{UDUI}}\Vert$$

while Fig. 3 compares the Kalman filter covariance P_KF with the UDU factorization of the Information matrix $P_{UDUI}^{-1}=UD{U}^T$ by plotting the following quantity:

$${ϵ}_P=\Vert P_{\mathit{KF}}{P}_{\mathit{UDUI}}^{-1}-I\Vert$$

Fig. 1.

Estimation Error and 3σ predicted standard deviations

Fig. 2.

Norm of the State Error (||x̂_KF − x̂_UDUI||)

Fig. 3.

Norm of the Covariance error ($\Vert P_{KF}{P}_{UDUI}^{-1}-I\Vert$)

The figures show that the proposed algorithm results closely match the Kalman filter hence validating the proposed algorithm as its UDU information formulation. The growth of the error in Figs. 2 and 3 is due to the accumulation of round-off errors in the algorithms. It is known [3] that numerical errors accumulate faster in the full covariance formulation of the Kalman filter than in the UDU’s.

V. Conclusions

A new algorithmic mechanization of the classic Kalman filter is presented, the new algorithm combines the information formulation with the UDU factorization. While the covariance formulation of the Kalman filter is usually employed, the information formulation has distinct advantages in some applications, for example when no initial condition is available. The UDU factorization is a widely adopted technique to produce a numerically stable and accurate algorithm to keep the covariance matrix symmetric and positive definite. A numerical example confirms the equivalency between the Kalman filter and the proposed algorithm.

Biography

Christopher D’Souza is a Navigation Engineer in the Aerosciences and Flight Mechanics Division at the Johnson Space Center, Houston, TX. He has developed navigation filters and guidance laws for the US Air Force and NASA. He has worked at the Jet Propulsion Laboratory, the Air Force Research Laboratory, Draper Laboratory and the NASA Johnson Space Center. His areas of research include onboard navigation algorithms, covariance analysis, guidance laws, and trajectory optimization. He received his Ph.D. in aerospace engineering from the University of Texas in 1991.

Renato Zanetti received his undergraduate degree from Politecnico di Milano, Italy in 2003 and his Ph.D. from the University of Texas at Austin in 2007, both in aerospace engineering. From 2007 to 2013 he was a senior member of the technical staff at the Charles Stark Draper Laboratory, in the Houston, Texas office. From 2013 to 2017 he was an Engineer at the NASA Johnson Space Center in Houston, TX. In 2017 he joined the faculty of the Aerospace Engineering and Engineering Mechanics department at the University of Texas at Austin. His projects include autonomous navigation for various NASA and commercial space vehicles. His research work focuses on nonlinear/non-Gaussian estimation, attitude dynamics and control, and autonomous vehicles.