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.
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.
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 though the factorized covariance is used for computation, the unfactorized
initial covariance matrix is read from the configuration file.
Therefore, this initial covariance matrix needs to be factorized 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.
Even though the factorized covariance is used for computation, the unfactorized initial covariance matrix is read from the configuration file. Therefore, this initial covariance matrix needs to be factorized 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.
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.
With compiler optimization turned on, this implementation takes about twice as long as 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 supports single-dimensional updates, each component of
the measurement has to be computed sequentially. This implies
that the residuals must be computed after every component update. Since
the measurement model is non-linear, the h function is reevaluated 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 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
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 supports single-dimensional updates, each component of the measurement has to be computed sequentially. This implies that the residuals must be computed after every component update. Since the measurement model is non-linear, the h function is reevaluated 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 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.
