| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // This file is part of INSTINCT, the INS Toolkit for Integrated | ||
| 2 | // Navigation Concepts and Training by the Institute of Navigation of | ||
| 3 | // the University of Stuttgart, Germany. | ||
| 4 | // | ||
| 5 | // This Source Code Form is subject to the terms of the Mozilla Public | ||
| 6 | // License, v. 2.0. If a copy of the MPL was not distributed with this | ||
| 7 | // file, You can obtain one at https://mozilla.org/MPL/2.0/. | ||
| 8 | |||
| 9 | /// @file KalmanFilter.hpp | ||
| 10 | /// @brief Generalized Kalman Filter class | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2020-11-25 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <Eigen/Core> | ||
| 17 | #include <Eigen/Dense> | ||
| 18 | |||
| 19 | #include "Navigation/Math/Math.hpp" | ||
| 20 | |||
| 21 | namespace NAV | ||
| 22 | { | ||
| 23 | /// @brief Generalized Kalman Filter class | ||
| 24 | class KalmanFilter | ||
| 25 | { | ||
| 26 | public: | ||
| 27 | /// @brief Constructor | ||
| 28 | /// @param[in] n Number of States | ||
| 29 | /// @param[in] m Number of Measurements | ||
| 30 | 232 | KalmanFilter(int n, int m) | |
| 31 | 232 | { | |
| 32 | // xฬ State vector | ||
| 33 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | x = Eigen::MatrixXd::Zero(n, 1); |
| 34 | |||
| 35 | // ๐ Error covariance matrix | ||
| 36 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | P = Eigen::MatrixXd::Zero(n, n); |
| 37 | |||
| 38 | // ๐ฝ State transition matrix | ||
| 39 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | Phi = Eigen::MatrixXd::Zero(n, n); |
| 40 | |||
| 41 | // ๐ System/Process noise covariance matrix | ||
| 42 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | Q = Eigen::MatrixXd::Zero(n, n); |
| 43 | |||
| 44 | // ๐ณ Measurement vector | ||
| 45 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | z = Eigen::MatrixXd::Zero(m, 1); |
| 46 | |||
| 47 | // ๐ Measurement sensitivity Matrix | ||
| 48 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | H = Eigen::MatrixXd::Zero(m, n); |
| 49 | |||
| 50 | // ๐ = ๐ธ{๐ฐโ๐ฐโแต} Measurement noise covariance matrix | ||
| 51 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | R = Eigen::MatrixXd::Zero(m, m); |
| 52 | |||
| 53 | // ๐ฆ Measurement prediction covariance matrix | ||
| 54 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | S = Eigen::MatrixXd::Zero(m, m); |
| 55 | |||
| 56 | // ๐ Kalman gain matrix | ||
| 57 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | K = Eigen::MatrixXd::Zero(n, m); |
| 58 | |||
| 59 | // ๐ฐ Identity Matrix | ||
| 60 |
2/4✓ Branch 1 taken 232 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 232 times.
✗ Branch 5 not taken.
|
232 | I = Eigen::MatrixXd::Identity(n, n); |
| 61 | 232 | } | |
| 62 | |||
| 63 | /// @brief Default constructor | ||
| 64 | KalmanFilter() = delete; | ||
| 65 | |||
| 66 | /// @brief Sets all Vectors and matrices to 0 | ||
| 67 | 6 | void setZero() | |
| 68 | { | ||
| 69 | // xฬ State vector | ||
| 70 | 6 | x.setZero(); | |
| 71 | |||
| 72 | // ๐ Error covariance matrix | ||
| 73 | 6 | P.setZero(); | |
| 74 | |||
| 75 | // ๐ฝ State transition matrix | ||
| 76 | 6 | Phi.setZero(); | |
| 77 | |||
| 78 | // ๐ System/Process noise covariance matrix | ||
| 79 | 6 | Q.setZero(); | |
| 80 | |||
| 81 | // ๐ณ Measurement vector | ||
| 82 | 6 | z.setZero(); | |
| 83 | |||
| 84 | // ๐ Measurement sensitivity Matrix | ||
| 85 | 6 | H.setZero(); | |
| 86 | |||
| 87 | // ๐ = ๐ธ{๐ฐโ๐ฐโแต} Measurement noise covariance matrix | ||
| 88 | 6 | R.setZero(); | |
| 89 | |||
| 90 | // ๐ฆ Measurement prediction covariance matrix | ||
| 91 | 6 | S.setZero(); | |
| 92 | |||
| 93 | // ๐ Kalman gain matrix | ||
| 94 | 6 | K.setZero(); | |
| 95 | 6 | } | |
| 96 | |||
| 97 | /// @brief Do a Time Update | ||
| 98 | /// @attention Update the State transition matrix (๐ฝ) and the Process noise covariance matrix (๐) before calling this | ||
| 99 | /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2) | ||
| 100 | 17386 | void predict() | |
| 101 | { | ||
| 102 | // Math: \mathbf{\hat{x}}_k^- = \mathbf{\Phi}_{k-1}\mathbf{\hat{x}}_{k-1}^+ \qquad \text{P. Groves}\,(3.14) | ||
| 103 |
2/4✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
|
17386 | x = Phi * x; |
| 104 | |||
| 105 | // Math: \mathbf{P}_k^- = \mathbf{\Phi}_{k-1} P_{k-1}^+ \mathbf{\Phi}_{k-1}^T + \mathbf{Q}_{k-1} \qquad \text{P. Groves}\,(3.15) | ||
| 106 |
5/10✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17386 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17386 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17386 times.
✗ Branch 14 not taken.
|
17386 | P = Phi * P * Phi.transpose() + Q; |
| 107 | 17386 | } | |
| 108 | |||
| 109 | /// @brief Do a Measurement Update with a Measurement ๐ณ | ||
| 110 | /// @attention Update the Measurement sensitivity Matrix (๐), the Measurement noise covariance matrix (๐) | ||
| 111 | /// and the Measurement vector (๐ณ) before calling this | ||
| 112 | /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2) | ||
| 113 | 17386 | void correct() | |
| 114 | { | ||
| 115 |
5/10✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17386 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17386 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17386 times.
✗ Branch 14 not taken.
|
17386 | S = H * P * H.transpose() + R; |
| 116 | |||
| 117 | // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21) | ||
| 118 |
5/10✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17386 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17386 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17386 times.
✗ Branch 14 not taken.
|
17386 | K = P * H.transpose() * S.inverse(); |
| 119 | |||
| 120 | // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{\hat{x}}_k^-) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24) | ||
| 121 |
5/10✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17386 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17386 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 17386 times.
✗ Branch 14 not taken.
|
17386 | x = x + K * (z - H * x); |
| 122 | |||
| 123 | // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25) | ||
| 124 |
4/8✓ Branch 1 taken 17386 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 17386 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 17386 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 17386 times.
✗ Branch 11 not taken.
|
17386 | P = (I - K * H) * P; |
| 125 | 17386 | } | |
| 126 | |||
| 127 | /// @brief Do a Measurement Update with a Measurement Innovation ๐น๐ณ | ||
| 128 | /// @attention Update the Measurement sensitivity Matrix (๐), the Measurement noise covariance matrix (๐) | ||
| 129 | /// and the Measurement vector (๐ณ) before calling this | ||
| 130 | /// @note See P. Groves (2013) - Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems (ch. 3.2.2) | ||
| 131 | /// @note See Brown & Hwang (2012) - Introduction to Random Signals and Applied Kalman Filtering (ch. 5.5 - figure 5.5) | ||
| 132 | ✗ | void correctWithMeasurementInnovation() | |
| 133 | { | ||
| 134 | // Math: \mathbf{K}_k = \mathbf{P}_k^- \mathbf{H}_k^T (\mathbf{H}_k \mathbf{P}_k^- \mathbf{H}_k^T + R_k)^{-1} \qquad \text{P. Groves}\,(3.21) | ||
| 135 | ✗ | K = P * H.transpose() * (H * P * H.transpose() + R).inverse(); | |
| 136 | |||
| 137 | // Math: \begin{align*} \mathbf{\hat{x}}_k^+ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k (\mathbf{z}_k - \mathbf{H}_k \mathbf{\hat{x}}_k^-) \\ &= \mathbf{\hat{x}}_k^- + \mathbf{K}_k \mathbf{\delta z}_k^{-} \end{align*} \qquad \text{P. Groves}\,(3.24) | ||
| 138 | ✗ | x = x + K * z; | |
| 139 | |||
| 140 | // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- \qquad \text{P. Groves}\,(3.25) | ||
| 141 | // P = (I - K * H) * P; | ||
| 142 | |||
| 143 | // Math: \mathbf{P}_k^+ = (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k) \mathbf{P}_k^- (\mathbf{I} - \mathbf{K}_k \mathbf{H}_k)^T + \mathbf{K}_k \mathbf{R}_k \mathbf{K}_k^T \qquad \text{Brown & Hwang}\,(p. 145, eq. 4.2.11) | ||
| 144 | ✗ | P = (I - K * H) * P * (I - K * H).transpose() + K * R * K.transpose(); | |
| 145 | ✗ | } | |
| 146 | |||
| 147 | /// @brief Updates the state transition matrix ๐ฝ limited to first order in ๐ ๐โ | ||
| 148 | /// @param[in] F System Matrix | ||
| 149 | /// @param[in] tau_s time interval in [s] | ||
| 150 | /// @note See Groves (2013) chapter 14.2.4, equation (14.72) | ||
| 151 | static Eigen::MatrixXd transitionMatrix(const Eigen::MatrixXd& F, double tau_s) | ||
| 152 | { | ||
| 153 | // Transition matrix ๐ฝ | ||
| 154 | return Eigen::MatrixXd::Identity(F.rows(), F.cols()) + F * tau_s; | ||
| 155 | } | ||
| 156 | |||
| 157 | /// xฬ State vector | ||
| 158 | Eigen::MatrixXd x; | ||
| 159 | |||
| 160 | /// ๐ Error covariance matrix | ||
| 161 | Eigen::MatrixXd P; | ||
| 162 | |||
| 163 | /// ๐ฝ State transition matrix | ||
| 164 | Eigen::MatrixXd Phi; | ||
| 165 | |||
| 166 | /// ๐ System/Process noise covariance matrix | ||
| 167 | Eigen::MatrixXd Q; | ||
| 168 | |||
| 169 | /// ๐ณ Measurement vector | ||
| 170 | Eigen::MatrixXd z; | ||
| 171 | |||
| 172 | /// ๐ Measurement sensitivity Matrix | ||
| 173 | Eigen::MatrixXd H; | ||
| 174 | |||
| 175 | /// ๐ = ๐ธ{๐ฐโ๐ฐโแต} Measurement noise covariance matrix | ||
| 176 | Eigen::MatrixXd R; | ||
| 177 | |||
| 178 | /// ๐ฆ Measurement prediction covariance matrix | ||
| 179 | Eigen::MatrixXd S; | ||
| 180 | |||
| 181 | /// ๐ Kalman gain matrix | ||
| 182 | Eigen::MatrixXd K; | ||
| 183 | |||
| 184 | private: | ||
| 185 | /// ๐ฐ Identity Matrix (n x n) | ||
| 186 | Eigen::MatrixXd I; | ||
| 187 | }; | ||
| 188 | |||
| 189 | /// @brief Calculates the state transition matrix ๐ฝ limited to specified order in ๐ ๐โ | ||
| 190 | /// @param[in] F System Matrix | ||
| 191 | /// @param[in] tau_s time interval in [s] | ||
| 192 | /// @param[in] order The order of the Taylor polynom to calculate | ||
| 193 | /// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.34, p. 98 | ||
| 194 | template<typename Derived> | ||
| 195 | ✗ | typename Derived::PlainObject transitionMatrix_Phi_Taylor(const Eigen::MatrixBase<Derived>& F, double tau_s, size_t order) | |
| 196 | { | ||
| 197 | // Transition matrix ๐ฝ | ||
| 198 | ✗ | typename Derived::PlainObject Phi; | |
| 199 | |||
| 200 | if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) | ||
| 201 | { | ||
| 202 | ✗ | Phi = Eigen::MatrixBase<Derived>::Identity(F.rows(), F.cols()); | |
| 203 | } | ||
| 204 | else | ||
| 205 | { | ||
| 206 | ✗ | Phi = Eigen::MatrixBase<Derived>::Identity(); | |
| 207 | } | ||
| 208 | // std::cout << "Phi = I"; | ||
| 209 | |||
| 210 | ✗ | for (size_t i = 1; i <= order; i++) | |
| 211 | { | ||
| 212 | ✗ | typename Derived::PlainObject Fpower = F; | |
| 213 | // std::cout << " + (F"; | ||
| 214 | |||
| 215 | ✗ | for (size_t j = 1; j < i; j++) // F^j | |
| 216 | { | ||
| 217 | // std::cout << "*F"; | ||
| 218 | ✗ | Fpower *= F; | |
| 219 | } | ||
| 220 | // std::cout << "*tau_s^" << i << ")"; | ||
| 221 | // std::cout << "/" << math::factorial(i); | ||
| 222 | ✗ | Phi += (Fpower * std::pow(tau_s, i)) / math::factorial(i); | |
| 223 | } | ||
| 224 | // std::cout << "\n"; | ||
| 225 | |||
| 226 | ✗ | return Phi; | |
| 227 | ✗ | } | |
| 228 | |||
| 229 | /// @brief Calculates the state transition matrix ๐ฝ using the exponential matrix | ||
| 230 | /// @param[in] F System Matrix | ||
| 231 | /// @param[in] tau_s time interval in [s] | ||
| 232 | /// @note See \cite Groves2013 Groves, ch. 3.2.3, eq. 3.33, p. 97 | ||
| 233 | /// @attention The cost of the computation is approximately 20n^3 for matrices of size n. The number 20 depends weakly on the norm of the matrix. | ||
| 234 | template<typename Derived> | ||
| 235 | typename Derived::PlainObject transitionMatrix_Phi_exp(const Eigen::MatrixBase<Derived>& F, typename Derived::Scalar tau_s) | ||
| 236 | { | ||
| 237 | // Transition matrix ๐ฝ | ||
| 238 | return (F * tau_s).exp(); | ||
| 239 | } | ||
| 240 | |||
| 241 | } // namespace NAV | ||
| 242 |