Joseph form

One of the biggest challenges in implementing Kalman filters is that numerical errors can cause the covariances to lose symmetry and positive definiteness, causing the filter to diverge and no longer provide an estimation result. This happens especially in system models with large degrees of freedom. The solution is to use a numerically more stable equation. For the update equation this is the so-called joseph form, which has a higher computation time, but is less sensitive to numerical errors.

\[ \mathbf{P}(k|k) = \bigl(\mathbf{I} - \mathbf{K}(k) \mathbf{C}(k) \bigl) \mathbf{P}(k) \bigl(\mathbf{I} - \mathbf{K}(k) \mathbf{C}(k) \bigl)^T + \mathbf{K}(k) \mathbf{R}(k) \mathbf{K}(k)^T \]