Thornton-Bierman-factorization.md

+# Thornton-Bierman factorization
+
+## Theoretical background
+
+A problem of the standard Kalman filter algorithm is that it involves
+matrix inversions, which are known to be numerically unstable. A
+solution to this problem is to use the Thornton-Bierman algorithm which
+uses more stable operations. The idea is to use a factorized version of the
+covariance matrix P: `P = U·D·U^T` where U is an upper
+triangular matrix where all diagonal coefficients are 1, and D is a
+diagonal matrix. A disadvantage of this algorithm is that it only
+supports single-dimension measurements. This means that the
+multi-dimensional measurements from the sensors have to be processed
+sequentially.
+
+## Implementation
+
+The filtering algorithm can be chosen at run-time by changing a value in
+the `filter.ini` configuration file. The U matrix is stored directly
+in the P matrix, and the D diagonal matrix is stored as a vector
+containing the diagonal coefficients.
+
+### Initial decomposition
+
+Even that we always work with the factorized covariance, we still read
+the unfactorized initial covariance matrix from the configuration file.
+Therefore, we need to factorize this initial covariance matrix before
+beginning to filter. This is done by the `EKF::UDDecomp()` method. This
+factorization is a variant of the Cholewsky decomposition, and the
+implementation is largely inspired from the source of Eigen's `LDLT`
+decomposition which is very similar. Importantly, this implementation
+uses block operations that can be more easily optimized by the compiler.
+
+### Thornton propagation
+
+The algorithm for the propagation was proposed by Thornton. It is
+implemented in the `EKF::propagate`
+method. The main difference compared to the Matlab implementation
+is that all inner loops have been
+transformed into block matrix operations, since those are more easily
+optimizable by the compiler on modern processor architectures.
+
+With compiler optimization turned on, this implementation takes about
+twice longer than the standard algorithm. However, the embedded computer
+of the TOPOplane2 can still easily run it in real-time at 100 Hz.
+
+### Bierman update
+
+The algorithm for the update was proposed by Bierman. The C++
+implementation is very close to the Matlab
+implementation, since it is not possible to use many block operations in
+this case. It is implemented in the `EKF::updateState` method.
+
+Since the algorithm only support single-dimensional updates, we have to
+compute each component of the measurement sequentially. This implies
+that we must recompute the residuals after every component update. Since
+the measurement model is non-linear, we recompute the h function after
+every iteration, and the results differ significantly from the result of
+the standard Kalman filter algorithm. The difference can be greatly
+reduced by using a linearized version of the of the measurement model.
+In the absence of ground truth, it is not possible to tell which results
+are closest to reality, and this would require further research.
\ No newline at end of file