| 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 VanLoan.hpp | ||
| 10 | /// @brief Van Loan Method \cite Loan1978 | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2022-01-11 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <utility> | ||
| 17 | #include <Eigen/Core> | ||
| 18 | #include <unsupported/Eigen/MatrixFunctions> | ||
| 19 | |||
| 20 | namespace NAV | ||
| 21 | { | ||
| 22 | |||
| 23 | /// @brief Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐 | ||
| 24 | /// @tparam DerivedF Matrix type of the F matrix | ||
| 25 | /// @tparam DerivedG Matrix type of the G matrix | ||
| 26 | /// @tparam DerivedW Matrix type of the W matrix | ||
| 27 | /// @param[in] F System model matrix | ||
| 28 | /// @param[in] G Noise model matrix | ||
| 29 | /// @param[in] W Noise scale factors | ||
| 30 | /// @param[in] dt Time step in [s] | ||
| 31 | /// @return A pair with the matrices {𝚽, 𝐐} | ||
| 32 | /// | ||
| 33 | /// 1. Form a \f$ 2n \times 2n \f$ matrix called \f$ \mathbf{A} \f$ (\f$ n \f$ is the dimension of \f$ \mathbf{x} \f$ and \f$ \mathbf{W} \f$ is the power spectral density of the noise \f$ W(t) \f$) | ||
| 34 | /// \anchor eq-Loan-A \f{equation}{ \label{eq:eq-Loan-A} | ||
| 35 | /// \mathbf{A} = \begin{bmatrix} -\mathbf{F} & \mathbf{G} \mathbf{W} \mathbf{G} \\ | ||
| 36 | /// \mathbf{0} & \mathbf{F}^T \end{bmatrix} \Delta t | ||
| 37 | /// \f} | ||
| 38 | /// | ||
| 39 | /// 2. Calculate the exponential of \f$ \mathbf{A} \f$ | ||
| 40 | /// \anchor eq-Loan-B \f{equation}{ \label{eq:eq-Loan-B} | ||
| 41 | /// \mathbf{B} = \text{expm}(\mathbf{A}) = \left[ \begin{array}{c;{2pt/2pt}c} | ||
| 42 | /// \dots & \mathbf{\Phi}^{-1} \mathbf{Q} \\[2mm] | ||
| 43 | /// \hdashline[2pt/2pt] & \\[-2mm] | ||
| 44 | /// \mathbf{0} & \mathbf{\Phi}^T \end{array} \right] | ||
| 45 | /// = \begin{bmatrix} \mathbf{B}_{11} & \mathbf{B}_{12} \\ | ||
| 46 | /// \mathbf{B}_{21} & \mathbf{B}_{22} \end{bmatrix} | ||
| 47 | /// \f} | ||
| 48 | /// | ||
| 49 | /// 3. Calculate the state transition matrix \f$ \mathbf{\Phi} \f$ as | ||
| 50 | /// \anchor eq-Loan-Phi \f{equation}{ \label{eq:eq-Loan-Phi} | ||
| 51 | /// \mathbf{\Phi} = \mathbf{B}_{22}^T | ||
| 52 | /// \f} | ||
| 53 | /// | ||
| 54 | /// 4. Calculate the process noise covariance matrix \f$ \mathbf{Q} \f$ as | ||
| 55 | /// \anchor eq-Loan-Q \f{equation}{ \label{eq:eq-Loan-Q} | ||
| 56 | /// \mathbf{Q} = \mathbf{\Phi} \mathbf{B}_{12} | ||
| 57 | /// \f} | ||
| 58 | /// | ||
| 59 | /// @note See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential \cite Loan1978 | ||
| 60 | template<typename DerivedF, typename DerivedG, typename DerivedW> | ||
| 61 | [[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject> | ||
| 62 | 74222 | calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F, | |
| 63 | const Eigen::MatrixBase<DerivedG>& G, | ||
| 64 | const Eigen::MatrixBase<DerivedW>& W, | ||
| 65 | typename DerivedF::Scalar dt) | ||
| 66 | { | ||
| 67 | // ┌ ┐ | ||
| 68 | // │ -F ┊ GWG^T│ | ||
| 69 | // A = │------------│ * dT | ||
| 70 | // │ 0 ┊ F^T │ | ||
| 71 | // └ ┘ | ||
| 72 | // W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote) | ||
| 73 | // W = Identity, as noise scale factor is included within G matrix | ||
| 74 | Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic> | ||
| 75 |
2/4✓ Branch 3 taken 37111 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.
|
74222 | A = Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic>::Zero(2 * F.rows(), 2 * F.cols()); |
| 76 |
3/6✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37111 times.
✗ Branch 10 not taken.
|
74222 | A.topLeftCorner(F.rows(), F.cols()) = -F; |
| 77 |
5/10✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37111 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37111 times.
✗ Branch 8 not taken.
✓ Branch 12 taken 37111 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 37111 times.
✗ Branch 16 not taken.
|
74222 | A.topRightCorner(F.rows(), F.cols()) = G * W * G.transpose(); |
| 78 |
3/6✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37111 times.
✗ Branch 10 not taken.
|
74222 | A.bottomRightCorner(F.rows(), F.cols()) = F.transpose(); |
| 79 |
1/2✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
|
74222 | A *= dt; |
| 80 | |||
| 81 | // Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp) | ||
| 82 |
2/4✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37111 times.
✗ Branch 5 not taken.
|
74222 | Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic> B = A.exp(); |
| 83 | |||
| 84 | // ┌ ┐ | ||
| 85 | // │ ... ┊ Phi^-1 Q │ | ||
| 86 | // B = expm(A) = │----------------│ | ||
| 87 | // │ 0 ┊ Phi^T │ | ||
| 88 | // └ ┘ | ||
| 89 |
3/6✓ Branch 3 taken 37111 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37111 times.
✗ Branch 10 not taken.
|
74222 | typename DerivedF::PlainObject Phi = B.bottomRightCorner(F.rows(), F.cols()).transpose(); |
| 90 |
3/6✓ Branch 3 taken 37111 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 37111 times.
✗ Branch 10 not taken.
|
74222 | typename DerivedF::PlainObject Q = Phi * B.topRightCorner(F.rows(), F.cols()); |
| 91 | |||
| 92 |
1/2✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.
|
148444 | return { Phi, Q }; |
| 93 | 74222 | } | |
| 94 | |||
| 95 | } // namespace NAV | ||
| 96 |