Kalman filter building blocks: gain computation and covariance update. More...
#include "config.h"#include "utilities/expected.h"#include "matrix_traits.h"#include "fused/fused_matrix.h"#include "algorithms/operations/inverse.h"
Go to the source code of this file.
Namespaces | |
| namespace | matrix_algorithms |
Functions | |
| template<typename T , my_size_t N, my_size_t M> | |
| Expected< FusedMatrix< T, N, M >, MatrixStatus > | matrix_algorithms::kalman_gain (const FusedMatrix< T, N, N > &P, const FusedMatrix< T, M, N > &H, const FusedMatrix< T, M, M > &R) |
| Compute the Kalman gain K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹. | |
| template<typename T , my_size_t N, my_size_t M> | |
| FusedMatrix< T, N, N > | matrix_algorithms::joseph_update (const FusedMatrix< T, N, M > &K, const FusedMatrix< T, M, N > &H, const FusedMatrix< T, N, N > &P, const FusedMatrix< T, M, M > &R) |
| Joseph form covariance update: P' = (I-K·H)·P·(I-K·H)ᵀ + K·R·Kᵀ. | |
Detailed Description
Kalman filter building blocks: gain computation and covariance update.
Provides the core update-step operations for a discrete-time Kalman filter. The prediction step uses symmetric_rank_k_update (see rank_update.h).
Template parameters:
- N: state dimension
- M: measurement dimension
KALMAN GAIN (2b)
S = H·P·Hᵀ + R (innovation covariance, M×M) K = P·Hᵀ · S⁻¹ (Kalman gain, N×M)
Uses inverse(S) rather than solve to get the full gain matrix. S is typically small (M = 1–3 for most sensor fusion), so the inverse is cheap and straightforward.
JOSEPH FORM UPDATE (2c)
P' = (I - K·H) · P · (I - K·H)ᵀ + K·R·Kᵀ
Numerically more stable than the standard P' = P - K·S·Kᵀ form. Guarantees symmetry and positive semi-definiteness of the result, even with floating-point rounding.
FAILURE MODES
- MatrixStatus::Singular — innovation covariance S is not invertible (degenerate measurement noise or numerical issues)
