acts-project / acts-project/acts
Implement square-root-decomposed Kálmán updating
- 主要語言
- C++
- 星號
- 131
- 分支
- 276
- 平均合併
- 3 天 13 小時
- 30 天內合併 PR
- 112
描述
Similar to the $UDU$-decomposed filter in acts-project/acts#5478, 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
貢獻指南
評估
這個 Issue 還沒有評估資料。