acts-project / acts-project/acts

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

未關閉
#5,479 0 則留言 0 個 reaction 已指派 0 人 在 GitHub 檢視
Component - Detray
主要語言
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 還沒有評估資料。

把新 issue 寄到你的電子郵件信箱

精選適合新手參與的 GitHub issue 摘要。