Implement square-root-decomposed Kálmán updating

[stephenswat]

Similar to the $UDU$-decomposed filter in #987, the square-root-decomposed Kálmán filter is an alternate form of storing the covariance matrix where instead of saving $C$ directly, we save the square root matrix $S = \sqrt{C}$ such that:

$$C = SS^\intercal$$

Like the $UDU$ factorization, this guarantees that the symmetry of the covariance matrix is definitionally conserved (because $AA^\intercal$ is symmetric for all matrices $A$). Unlike the $UDU$ decomposition, it also increases precision but not necessarily computational performance.

Computing the matrix $S$ is trivial using either a Cholesky factorization or using the $UDU$ decompositon algorithm, because:

$$C = UDU^\intercal = U\sqrt{D}\sqrt{D}U^\intercal = U\sqrt{D}\sqrt{D}^\intercal U^\intercal = SS^\intercal$$

...where...

$$S = U\sqrt{D}$$

Note

Taking the square root of a positive definite diagonal matrix $D$ is trivial.

Computation of the innovation matrix remains the same; it remains a multiplication with the Jacobian $C_k = JC_{k | k-1}J^\intercal$. However, we instead construct the following $12\times 6$ block matrix:

$$A = \begin{bmatrix} JS \\ S \end{bmatrix}$$

Notice that:

$$AA^\intercal = \begin{bmatrix} JSS^\intercal J^\intercal & JC_{k | k-1} \\ C_{k | k-1}J^\intercal & C_{k | k-1} \end{bmatrix} = \begin{bmatrix} C_k & JC_{k | k-1} \\ C_{k | k-1}J^\intercal & C_{k | k-1} \end{bmatrix}$$

We then perform a QR decomposition on $A$ to find $B = \mathrm{QR}(A)$ which has the $12\times 12$ structure (see source for why this is true):

$$B = \begin{bmatrix} \sqrt{JC_{k | k-1}J^\intercal} & 0\\ C_{k|k-1}J^\intercal (JC_{k | k-1}J^\intercal)^{-1}\sqrt{JC_{k | k-1}J^\intercal} & \sqrt{C_k} \end{bmatrix}$$

Notice that the desired updated covariance $S_k = \sqrt{C_k}$ is a block of $B$, so we can simply extract it. That concludes the application of the Jacobian matrix. The Kálmán gain comes for free from matrix $B$, but note that this will require us to transfer (half of, $12 \times 6$ elements) the matrix $B$ from the propagator to traccc! How do we most efficiently do that?

Note

Unlike the $UDU$-decomposed Kálmán filter, the square-root decomposed filter can be easily applied to smoothing.

Useful sources:

• https://ipnpr.jpl.nasa.gov/progress_report/42-233/42-233A.pdf