Line data Source code
1 : #ifndef DD2B2494_7486_42BA_84E2_32308E26DBBC
2 : #define DD2B2494_7486_42BA_84E2_32308E26DBBC
3 :
4 : #include "base/first_include.h" // IWYU pragma: keep
5 : #include "math/linalg/contracts/covariance_matrix_intf.h" // IWYU pragma: keep
6 : #include "math/linalg/square_matrix.h"
7 : #include <initializer_list> // IWYU pragma: keep
8 :
9 : namespace tracking
10 : {
11 : namespace math
12 : {
13 :
14 : /// \brief Full covariance matrix representation for Kalman filtering
15 : ///
16 : /// A covariance matrix represents the uncertainty of a random vector in Kalman filtering.
17 : /// This class provides a standard full matrix representation where all elements are stored.
18 : /// Covariance matrixes are always symmetric and positive semi-definite.
19 : ///
20 : /// Key properties:
21 : /// - Symmetric: P[i,j] = P[j,i] for all i,j
22 : /// - Positive semi-definite: x^T*P*x >= 0 for all vectors x
23 : /// - Represents uncertainty in state estimation
24 : ///
25 : /// This implementation uses row-major storage for consistency with other matrix types.
26 : /// All constructors enforce symmetry through assertions.
27 : ///
28 : /// \tparam ValueType_ The atomic data type of internal elements
29 : /// \tparam Size_ Dimension of the square covariance matrix
30 : ///
31 : /// \see CovarianceMatrixFactored for UDU factored representation
32 : /// \see SquareMatrix for the base matrix functionality
33 : template <typename ValueType_, sint32 Size_>
34 : class CovarianceMatrixFull
35 : : public SquareMatrix<ValueType_, Size_, true>
36 : , public contract::CovarianceMatrixIntf<CovarianceMatrixFull<ValueType_, Size_>>
37 : {
38 : public:
39 : using BaseSquareMatrix = SquareMatrix<ValueType_, Size_, true>; ///< type of the parent class
40 :
41 : using value_type = ValueType_;
42 : using compose_type = CovarianceMatrixFull;
43 : static constexpr auto dim = Size_;
44 :
45 : // rule of 5 declarations
46 39 : CovarianceMatrixFull() = default;
47 37 : CovarianceMatrixFull(const CovarianceMatrixFull&) = default;
48 43 : CovarianceMatrixFull(CovarianceMatrixFull&&) noexcept = default;
49 : auto operator=(const CovarianceMatrixFull&) -> CovarianceMatrixFull& = default;
50 58 : auto operator=(CovarianceMatrixFull&&) noexcept -> CovarianceMatrixFull& = default;
51 457 : virtual ~CovarianceMatrixFull() = default;
52 :
53 : //////////////////////////////////////////////////
54 : // additional constructors --->
55 : /// \brief Construct a new Covariance Matrix Full<ValueType_, Size_> object
56 : /// \param[in] other A base class object
57 8 : explicit CovarianceMatrixFull(const BaseSquareMatrix& other)
58 8 : : BaseSquareMatrix{other}
59 : {
60 8 : assert(this->isSymmetric() && "Constructed covariance not symmetric");
61 8 : }
62 :
63 : /// \brief Move construct a new Covariance Matrix Full<ValueType_, Size_> object
64 : /// \param[in] other A base class object
65 258 : explicit CovarianceMatrixFull(BaseSquareMatrix&& other) noexcept
66 258 : : BaseSquareMatrix{std::move(other)}
67 : {
68 258 : assert(this->isSymmetric() && "Constructed covariance not symmetric");
69 258 : }
70 :
71 : /// \brief Construct a new Covariance Matrix Full<ValueType_, Size_> object from a transposed SquareMatrix
72 : /// \param[in] other A transposed base class object
73 0 : explicit CovarianceMatrixFull(const typename BaseSquareMatrix::transpose_type& other)
74 0 : : BaseSquareMatrix{other.transpose()}
75 : {
76 0 : assert(this->isSymmetric() && "Constructed covariance not symmetric");
77 0 : }
78 :
79 : /// \brief Move construct a new Covariance Matrix Full<ValueType_, Size_> object from a transposed SquareMatrix
80 : /// \param[in] other A transposed base class object
81 299 : explicit CovarianceMatrixFull(typename BaseSquareMatrix::transpose_type&& other) noexcept
82 299 : : BaseSquareMatrix{std::move(other).transpose_rvalue()}
83 : {
84 299 : assert(this->isSymmetric() && "Constructed covariance not symmetric");
85 299 : }
86 :
87 : /// \brief Construct an Identity matrix
88 : /// \return CovarianceMatrixFull Resulting identity matrix
89 51 : static auto Identity() -> CovarianceMatrixFull { return CovarianceMatrixFull{BaseSquareMatrix::Identity()}; }
90 :
91 : /// \brief Construct a diagonal covariance matrix
92 : /// \return CovarianceMatrixFactored
93 : static auto FromDiagonal(const DiagonalMatrix<ValueType_, Size_>& diag) -> CovarianceMatrixFull;
94 :
95 : /// \brief Creates a CovarianceMatrixFull from a nested initializer list
96 : ///
97 : /// This function constructs a full covariance matrix from a nested initializer list.
98 : ///
99 : /// \param[in] list Nested initializer list representing the covariance matrix
100 : /// \return CovarianceMatrixFull instance initialized with the provided values
101 54 : static auto FromList(const std::initializer_list<std::initializer_list<ValueType_>>& list) -> CovarianceMatrixFull
102 : {
103 54 : return CovarianceMatrixFull{BaseSquareMatrix::FromList(list)};
104 : }
105 : // <---
106 :
107 : /// \brief Set internal matrix to the Identity matrix
108 4 : void setIdentity() { BaseSquareMatrix::setIdentity(); }
109 :
110 : /// \brief Access operator to the covariance value at (row, col)
111 : using BaseSquareMatrix::operator();
112 :
113 : /// \brief Creates the "composed" covariance, although no composition is needed
114 : /// \return const CovarianceMatrixFull&
115 0 : auto operator()() const -> const CovarianceMatrixFull& { return *this; }
116 :
117 : /// \brief Calculate the determinant of the covariance matrix.
118 : ///
119 : /// \return ValueType_ The determinant of the matrix
120 : /// \note uses SquareMatrix::determinant
121 10 : auto determinant() const -> ValueType_ { return BaseSquareMatrix::determinant(); }
122 :
123 : /// \brief Calculates the matrix inverse using Cholesky decomposition
124 : ///
125 : /// Computes the inverse of the covariance matrix using Cholesky decomposition.
126 : /// This method is numerically stable for positive definite matrixes.
127 : ///
128 : /// \return tl::expected containing the inverse matrix on success, or an error code
129 : /// \return Errors::NotPositiveDefinite if the matrix is not positive definite
130 : ///
131 : /// \note Time complexity: O(n^3) where n = Size_
132 : /// \note The result satisfies: (*this) * result = Identity()
133 : auto inverse() const -> tl::expected<CovarianceMatrixFull, Errors>;
134 :
135 : /// \brief Compute inverse as full covariance matrix
136 : ///
137 : /// Alias for inverse() to match the contract definitions
138 : ///
139 : /// \return Full covariance matrix containing the inverse
140 0 : auto composed_inverse() const -> tl::expected<compose_type, Errors> { return this->inverse(); }
141 :
142 : /// \brief Compute A*P*A^T in-place (covariance propagation)
143 : ///
144 : /// Performs the matrix operation A*P*A^T where P is this covariance matrix.
145 : /// This operation propagates covariance through a linear transformation A.
146 : /// The result overwrites the current matrix.
147 : ///
148 : /// \tparam IsRowMajor_ Storage layout of the transformation matrix A
149 : /// \param[in] A Square transformation matrix
150 : ///
151 : /// \note This is a key operation in Kalman filter prediction
152 : /// \note Time complexity: O(n^3) where n = Size_
153 : // TODO(matthias): optimization - add in-place variants for common ops like apaT to avoid copies
154 : // improving cache efficiency for small data; beneficial for small n without complexity overhead
155 : template <bool IsRowMajor_>
156 : void apaT(const tracking::math::SquareMatrix<ValueType_, Size_, IsRowMajor_>& A);
157 :
158 : /// \brief Compute A*P*A^T (covariance propagation)
159 : ///
160 : /// Performs the matrix operation A*P*A^T where P is this covariance matrix.
161 : /// This operation propagates covariance through a linear transformation A.
162 : /// Returns a new covariance matrix without modifying this one.
163 : ///
164 : /// \tparam IsRowMajor_ Storage layout of the transformation matrix A
165 : /// \param[in] A Square transformation matrix
166 : /// \return New covariance matrix containing A*P*A^T
167 : ///
168 : /// \note This is a key operation in Kalman filter prediction
169 : /// \note Time complexity: O(n^3) where n = Size_
170 : template <bool IsRowMajor_>
171 : auto apaT(const tracking::math::SquareMatrix<ValueType_, Size_, IsRowMajor_>& A) const -> CovarianceMatrixFull;
172 :
173 : /// \brief Set diagonal variance element and clear correlations
174 : ///
175 : /// Sets the variance (diagonal element) at position (idx, idx) to the given value
176 : /// and clears all off-diagonal correlations involving this state variable.
177 : /// This effectively makes the idx-th state variable uncorrelated with others.
178 : ///
179 : /// \param[in] idx Zero-based index of the state variable (0 <= idx < Size_)
180 : /// \param[in] val New variance value (must be positive)
181 : ///
182 : /// \note This operation modifies both the diagonal and off-diagonal elements
183 : /// \note Commonly used to reinitialize uncertainty for specific state components
184 : void setVariance(const sint32 idx, const ValueType_ val);
185 : };
186 :
187 : } // namespace math
188 : } // namespace tracking
189 :
190 : #endif // DD2B2494_7486_42BA_84E2_32308E26DBBC
|
