Implement UDU-decomposed Kálmán updating

[stephenswat]

I'm starting a series of issues to track some potential ideas to accelerate and also increase the numerical stability of our Kálmán filter.

The $UDU$-decomposed formulation of Kálmán filtering replaces the storage of the full covariance matrix $C$ with two matrices, $U$ which is upper-unitriangular and $D$ which is diagonal. The logic is that the covariance matrix can be obtained using the equation $C = UDU^\intercal$. This formulation provides increased numerical stability and performance, but not increased precision. The storage of the parameter vector is untouched.

From an initial covariance matrix $C$, we can compute the $UDU$ decomposition with the following Python algorithm:

def udu(P):
    U = numpy.zeros((6, 6))
    D = numpy.zeros((6, 6))
    D[5, 5] = P[5, 5]

    U[:, 5] = P[:, 5] / D[5, 5]

    for j in reversed(range(0, 5)):
        tmp = D[j, j]
        for k in range(j+1, 6):
            tmp += D[k, k] * U[j, k] * U[j, k]
        D[j, j] = P[j, j] - tmp
        for i in reversed(range(0, j + 1)):
            tmp = U[i, j]
            for k in range(j + 1, 6):
                tmp += D[k, k] * U[j, k] * U[i, j]
            U[i, j] = (P[i, j] - tmp) / D[j, j]
        U[j, j] = 1.0

    return U, D

The Kálmán prediction step becomes slightly more complicated than before. Instead of simply updating the covariance matrix with the Jacobian, i.e.:

$$C = JCJ^\intercal$$

We have to find updated $\overline{U}$ and $\overline{D}$ such that:

$$\overline{U}\overline{D}\overline{U}^\intercal = JUDU^\intercal J^\intercal$$

...that preserves the diagonality of $\overline{D}$ and $\overline{U}$. This can be achieved using a weighted modified Gram-Schmidt algorithm. For this, it would be nice to add an update_covariance method to a new $UDU$-decomposed track parameter class.

The Kálmán update step also changes, as the filtered covariance computation also changes. For this, we use the Agee-Turner algorithm.

Caution

I'm not currently sure if the $UDU$-decomposed Kálmán filter state also allows for more efficient smoothing.

Note

The diagonal matrix $D$ doesn't need to be stored in full, it can be stored as a vector of 6 floats. Similarly, the matrix $U$ can be stored as vector of 15 floats. This reduces the storage requirement from 72 floats to 21.

Useful documents:

• https://sites.utexas.edu/renato/files/2018/05/UDU_Information.pdf
• https://ntrs.nasa.gov/api/citations/20180003657/downloads/20180003657.pdf