Line data Source code
1 : #ifndef F9044FD7_A3A8_43F4_BDD6_F43011384722
2 : #define F9044FD7_A3A8_43F4_BDD6_F43011384722
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/contracts/covariance_matrix_intf.h" // IWYU pragma: keep
6 : #include <initializer_list>
7 : #include <iostream> // IWYU pragma: keep
8 :
9 : namespace tracking
10 : {
11 : namespace math
12 : {
13 :
14 : template <typename ValueType_, sint32 Rows_, sint32 Cols_, bool IsRowMajor_>
15 : class Matrix;
16 :
17 : template <typename ValueType_, sint32 Size_>
18 : class DiagonalMatrix;
19 :
20 : template <typename ValueType_, sint32 Size_, bool IsRowMajor_>
21 : class SquareMatrix;
22 :
23 : template <typename ValueType_, sint32 Size_, bool IsLower_, bool IsRowMajor_>
24 : class TriangularMatrix;
25 :
26 : template <typename ValueType_, sint32 Size_>
27 : class Vector;
28 :
29 : template <typename ValueType_, sint32 Size_>
30 : class CovarianceMatrixFull;
31 :
32 : /// \brief UDU factored covariance matrix for numerical stability
33 : ///
34 : /// This class implements the UDU^T factorization of covariance matrixes, where
35 : /// P = U*D*U^T. Here U is a unit upper triangular matrix (diagonal elements = 1)
36 : /// and D is a diagonal matrix containing the diagonal elements of the factorization.
37 : ///
38 : /// UDU factorization provides superior numerical stability compared to full matrix
39 : /// representations, especially for ill-conditioned covariance matrixes. This is
40 : /// particularly important in Kalman filtering where covariance matrixes can become
41 : /// near-singular over time.
42 : ///
43 : /// Key algorithms implemented:
44 : /// - Thornton update: P = Φ*P*Φ^T + G*Q*G^T (prediction step)
45 : /// - Agee-Turner rank-1 update: P = P ± x*x^T (measurement updates)
46 : ///
47 : /// The factorization maintains the symmetry and positive semi-definiteness of
48 : /// the covariance matrix while avoiding matrix inversion operations.
49 : ///
50 : /// \tparam ValueType_ The atomic data type of internal elements
51 : /// \tparam Size_ Dimension of the square covariance matrix
52 : ///
53 : /// \see CovarianceMatrixFull for standard matrix representation
54 : /// \see Thornton, C. L. "Triangular Covariance Factorizations for Kalman Filtering"
55 : /// \see Bierman, G. J. "Factorization Methods for Discrete Sequential Estimation"
56 : ///
57 : /// \note Based on academic publications by Thornton, Bierman, and D'Souza
58 : template <typename ValueType_, sint32 Size_>
59 : class CovarianceMatrixFactored: public contract::CovarianceMatrixIntf<CovarianceMatrixFactored<ValueType_, Size_>>
60 : {
61 : public:
62 : using value_type = ValueType_;
63 : using compose_type = CovarianceMatrixFull<ValueType_, Size_>;
64 : static constexpr auto dim = Size_;
65 :
66 : // rule of 5 declarations
67 : CovarianceMatrixFactored() = default;
68 46 : CovarianceMatrixFactored(const CovarianceMatrixFactored&) = default;
69 19 : CovarianceMatrixFactored(CovarianceMatrixFactored&&) noexcept = default;
70 : auto operator=(const CovarianceMatrixFactored&) -> CovarianceMatrixFactored& = default;
71 20 : auto operator=(CovarianceMatrixFactored&&) noexcept -> CovarianceMatrixFactored& = default;
72 171 : virtual ~CovarianceMatrixFactored() = default;
73 :
74 : //////////////////////////////////////////////////
75 : // additional constructors --->
76 : /// \brief Construct a new Covariance Matrix Factored object
77 : /// \param[in] u Unit upper triangular matrix
78 : /// \param[in] d Diagonal matrix
79 : explicit CovarianceMatrixFactored(const TriangularMatrix<ValueType_, Size_, false, true>& u,
80 : const DiagonalMatrix<ValueType_, Size_>& d);
81 :
82 : /// \brief Construct an Identity matrix
83 : /// \return CovarianceMatrixFactored
84 : static auto Identity() -> CovarianceMatrixFactored;
85 :
86 : /// \brief Construct a diagonal covariance matrix
87 : /// \param[in] diag Diagonal matrix
88 : /// \return CovarianceMatrixFactored
89 : static auto FromDiagonal(const DiagonalMatrix<ValueType_, Size_>& diag) -> CovarianceMatrixFactored;
90 :
91 : /// \brief Creates a CovarianceMatrixFactored from separate U and D initializer lists
92 : ///
93 : /// This function constructs a factored covariance matrix from separate initializer lists
94 : /// for the upper triangular U matrix and diagonal D matrix components.
95 : ///
96 : /// \param[in] u Nested initializer list for the upper triangular U matrix
97 : /// \param[in] d Flat initializer list for the diagonal D matrix
98 : /// \return CovarianceMatrixFactored instance with the specified U and D components
99 : static auto FromList(const std::initializer_list<std::initializer_list<ValueType_>>& u,
100 : const std::initializer_list<ValueType_>& d) -> CovarianceMatrixFactored;
101 : // <---
102 :
103 : /// \brief Set Identity covariance
104 : void setIdentity();
105 :
106 : /// \brief Access operator to the covariance value at (row, col)
107 : /// \param[in,out] row The specified row
108 : /// \param[in,out] col The specified column
109 : /// \return tl::expected<value_type, Errors> either the value at (row,col) or an Error descriptor
110 : auto operator()(sint32 row, sint32 col) const -> tl::expected<value_type, Errors>;
111 :
112 : /// \brief Creates the composed covariance
113 : /// \return compose_type
114 : auto operator()() const -> compose_type;
115 :
116 : /// \brief Access operator to the triangular matrix U
117 : /// \return const reference to the internal triangular matrix U
118 0 : [[nodiscard]] auto U() const -> const TriangularMatrix<ValueType_, Size_, false, true>& { return _u; }
119 :
120 : /// \brief Access operator to the diagonal matrix D
121 : /// \return const reference to the internal diagonal matrix D
122 40 : [[nodiscard]] auto D() const -> const DiagonalMatrix<ValueType_, Size_>& { return _d; }
123 :
124 : /// \brief Calculate the trace of the square matrix.
125 : ///
126 : /// Computes the sum of all diagonal elements of the matrix.
127 : /// The trace is defined as the sum of elements A_ii for i = 1 to n.
128 : ///
129 : /// \return ValueType_ The trace of the matrix(sum of diagonal elements)
130 : [[nodiscard]] auto trace() const -> ValueType_;
131 :
132 : /// \brief Calculate the determinant of the covariance matrix.
133 : ///
134 : /// Computes the determinant as the product of the diagonal elements.
135 : /// det(cov) = det(U)*det(D)*det(U') = det(D)
136 : ///
137 : /// \return ValueType_ The determinant of the matrix
138 : /// \note Time complexity: O(n) where n is the matrix dimension
139 : /// \note For singular matrixes, the determinant will be zero or very close to zero
140 10 : [[nodiscard]] auto determinant() const -> ValueType_ { return _d.determinant(); }
141 :
142 : /// \brief Compute inverse in factored form
143 : ///
144 : /// Calculates the inverse of the covariance matrix while maintaining
145 : /// the UDU factorization. This is more numerically stable than
146 : /// inverting the composed full matrix.
147 : ///
148 : /// \return tl::expected containing the inverse in factored form on success
149 : /// \return Errors::NotPositiveDefinite if the matrix is singular or near-singular
150 : ///
151 : /// \note Time complexity: O(n^3) where n = Size_
152 : /// \note More stable than full matrix inversion for ill-conditioned covariances
153 : auto inverse() const -> tl::expected<CovarianceMatrixFactored, Errors>;
154 :
155 : /// \brief Compute inverse as full covariance matrix
156 : ///
157 : /// Calculates the inverse and returns it as a full covariance matrix.
158 : /// This composes the inverse of UDU factorization into a full matrix.
159 : ///
160 : /// \return Full covariance matrix containing the inverse
161 : ///
162 : /// \note Time complexity: O(n^3) where n = Size_
163 : auto composed_inverse() const -> tl::expected<compose_type, Errors>;
164 :
165 : /// \brief Compute A*P*A^T in-place (factored covariance propagation)
166 : ///
167 : /// Performs the matrix operation A*P*A^T where P is this factored covariance matrix.
168 : /// The operation is performed in-place, overwriting the current matrix.
169 : /// This maintains the UDU factorization throughout the computation.
170 : ///
171 : /// \tparam IsRowMajor_ Storage layout of the transformation matrix A
172 : /// \param[in] A Square transformation matrix
173 : ///
174 : /// \note Used for coordinate transformations in factored Kalman filters
175 : /// \note Maintains numerical stability of the UDU representation
176 : template <bool IsRowMajor_>
177 : void apaT(const SquareMatrix<ValueType_, Size_, IsRowMajor_>& A);
178 :
179 : /// \brief Compute A*P*A^T (factored covariance propagation)
180 : ///
181 : /// Performs the matrix operation A*P*A^T where P is this factored covariance matrix.
182 : /// Returns a new factored covariance matrix without modifying this one.
183 : ///
184 : /// \tparam IsRowMajor_ Storage layout of the transformation matrix A
185 : /// \param[in] A Square transformation matrix
186 : /// \return New factored covariance matrix containing A*P*A^T
187 : ///
188 : /// \note Used for coordinate transformations in factored Kalman filters
189 : /// \note Maintains numerical stability of the UDU representation
190 : template <bool IsRowMajor_>
191 : auto apaT(const SquareMatrix<ValueType_, Size_, IsRowMajor_>& A) const -> CovarianceMatrixFactored;
192 :
193 : /// \brief Thornton update: Φ*P*Φ^T + G*Q*G^T (Kalman prediction)
194 : ///
195 : /// Implements Thornton's algorithm for updating the factored covariance matrix
196 : /// during the Kalman filter prediction step. This computes:
197 : /// P_{k+1} = Φ*P_k*Φ^T + G*Q*G^T
198 : ///
199 : /// Where:
200 : /// - Φ is the state transition matrix
201 : /// - P_k is the current covariance (this matrix)
202 : /// - G is the process noise coupling matrix
203 : /// - Q is the diagonal process noise covariance
204 : ///
205 : /// The algorithm maintains the UDU factorization throughout the computation,
206 : /// providing better numerical stability than full matrix operations.
207 : ///
208 : /// \tparam SizeQ_ Dimension of the process noise
209 : /// \param[in] Phi State transition matrix (Size_ x Size_)
210 : /// \param[in] G Process noise coupling matrix (Size_ x SizeQ_)
211 : /// \param[in] Q Diagonal process noise covariance (SizeQ_ x SizeQ_)
212 : ///
213 : /// \note This is the primary prediction update in factored Kalman filters
214 : /// \note Time complexity: O(n^2) where n = Size_
215 : /// \note Numerically stable for ill-conditioned matrixes
216 : ///
217 : /// \see Thornton, C. L. "Triangular Covariance Factorizations for Kalman Filtering"
218 : template <sint32 SizeQ_>
219 : void thornton(const SquareMatrix<ValueType_, Size_, true>& Phi,
220 : const Matrix<ValueType_, Size_, SizeQ_, true>& G,
221 : const DiagonalMatrix<ValueType_, SizeQ_>& Q);
222 :
223 : /// \brief Agee-Turner rank-1 update: P ± x*x^T
224 : ///
225 : /// Performs a rank-1 update of the covariance matrix: P = P + c*x*x^T
226 : /// where c is a scalar and x is a vector. This operation is fundamental
227 : /// to Kalman filter measurement updates.
228 : ///
229 : /// When c > 0: rank-1 update (increases uncertainty)
230 : /// When c < 0: rank-1 downdate (decreases uncertainty)
231 : ///
232 : /// The algorithm maintains the UDU factorization and is numerically stable.
233 : ///
234 : /// \param[in] c Scalar multiplier (positive for update, negative for downdate)
235 : /// \param[in] x Update vector defining the outer product x*x^T
236 : ///
237 : /// \note Used in Kalman measurement updates and Joseph stabilized updates
238 : /// \note Maintains positive semi-definiteness when c >= -1/λ_min where λ_min is the smallest eigenvalue
239 : /// \note Time complexity: O(n^2) where n = Size_
240 : ///
241 : /// \see Agee, W. H. and Turner, R. C. "Triangular Decomposition of a Positive Definite Matrix Plus a Symmetric Dyad"
242 : void rank1Update(const ValueType_ c, const Vector<ValueType_, Size_>& x);
243 :
244 : /// \brief Set the variance at (idx,idx) and clears any correlations
245 : /// \param[in] idx Index in diagonal matrix
246 : /// \param[in] val The value to be set
247 : void setVariance(const sint32 idx, const ValueType_ val);
248 :
249 : /// \brief Fill the covariance with first N=SrcCount rows and cols of the other covariance
250 : /// \tparam SrcSize Size of the other covariance
251 : /// \tparam SrcCount Count rows/cols to copy from other
252 : /// \param[in] other The other matrix to copy from
253 : template <sint32 SrcSize_, sint32 SrcCount_>
254 : void fill(const CovarianceMatrixFactored<ValueType_, SrcSize_>& other);
255 :
256 : /// \brief Set the Diagonal matrix element to given value
257 : /// \param[in] idx Index in diagonal matrix
258 : /// \param[in] val The value to be set
259 : void D(const sint32 idx, const ValueType_ val);
260 :
261 : /// \brief Check if the matrix is symmetric.
262 : ///
263 : /// The factored covariance is symmetric by design.
264 : ///
265 : /// \return always true
266 0 : [[nodiscard]] auto isSymmetric() const -> bool { return true; }
267 :
268 : /// \brief Check if the matrix matrix is positive definite.
269 : ///
270 : /// Tests whether all eigenvalues of the matrix are non-negative.
271 : /// A positive definite matrix satisfies x^T * A * x > 0 for all non-zero vectors x.
272 : ///
273 : /// \return true if the matrix is positive definite, false otherwise
274 0 : [[nodiscard]] auto isPositiveDefinite() const -> bool { return _d.isPositiveDefinite(); }
275 :
276 : /// \brief Checks if the matrix is positive semi-definite.
277 : ///
278 : /// Tests whether all eigenvalues of the matrix are non-negative.
279 : /// A positive semi-definite matrix satisfies x^T * A * x >= 0 for all non-zero vectors x.
280 : ///
281 : /// This is a key property for covariance matrixes in statistics and machine learning.
282 : ///
283 : /// \return true if the matrix is positive semi-definite, false otherwise
284 0 : [[nodiscard]] auto isPositiveSemiDefinite() const -> bool { return _d.isPositiveSemiDefinite(); }
285 :
286 : //////////////////////////////////////////////////
287 : // unsafe access operators --->
288 : /// \brief Unsafe element read-only access
289 : /// \param[in] row row of the element to read
290 : /// \param[in] col column of the element to read
291 : /// \return ValueType_ the value at (row,col)
292 : [[nodiscard]] auto at_unsafe(sint32 row, sint32 col) const -> ValueType_;
293 : /// <---
294 :
295 : // clang-format off
296 : TEST_REMOVE_PRIVATE:
297 : ; // workaround for correct indentation
298 : // clang-format on
299 :
300 : TriangularMatrix<ValueType_, Size_, false, true> _u{};
301 : DiagonalMatrix<ValueType_, Size_> _d{};
302 : };
303 :
304 : } // namespace math
305 : } // namespace tracking
306 :
307 : #endif // F9044FD7_A3A8_43F4_BDD6_F43011384722
|
