INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/Math/VanLoan.hpp
Date:	2025-06-02 15:19:59

	Exec	Total	Coverage
Lines:	11	11	100.0%
Functions:	2	4	50.0%
Branches:	23	46	50.0%

  
      Line
      Branch
      Exec
      Source
    
      // This file is part of INSTINCT, the INS Toolkit for Integrated
    
      // Navigation Concepts and Training by the Institute of Navigation of
    
      // the University of Stuttgart, Germany.
    
      //
    
      // This Source Code Form is subject to the terms of the Mozilla Public
    
      // License, v. 2.0. If a copy of the MPL was not distributed with this
    
      // file, You can obtain one at https://mozilla.org/MPL/2.0/.
    
      /// @file VanLoan.hpp
    
      /// @brief Van Loan Method \cite Loan1978
    
      /// @author T. Topp (topp@ins.uni-stuttgart.de)
    
      /// @date 2022-01-11
    
      ///
    
      /// #### Algorithm description
    
      /// 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$)
    
      /// \anchor eq-Loan-A \f{equation}{ \label{eq:eq-Loan-A}
    
      ///   \mathbf{A} = \begin{bmatrix} -\mathbf{F} & \mathbf{G} \mathbf{W} \mathbf{G} \\
    
      ///                                 \mathbf{0} &            \mathbf{F}^T          \end{bmatrix} \Delta t
    
      /// \f}
    
      ///
    
      /// 2. Calculate the exponential of \f$ \mathbf{A} \f$
    
      /// \anchor eq-Loan-B \f{equation}{ \label{eq:eq-Loan-B}
    
      ///   \mathbf{B} = \text{expm}(\mathbf{A}) = \left[ \begin{array}{c;{2pt/2pt}c}
    
      ///                                             \dots          & \mathbf{\Phi}^{-1} \mathbf{Q} \\[2mm]
    
      ///                                             \hdashline[2pt/2pt] &                                  \\[-2mm]
    
      ///                                             \mathbf{0}          & \mathbf{\Phi}^T                     \end{array} \right]
    
      ///                                        = \begin{bmatrix} \mathbf{B}_{11} & \mathbf{B}_{12} \\
    
      ///                                                          \mathbf{B}_{21} & \mathbf{B}_{22} \end{bmatrix}
    
      /// \f}
    
      ///
    
      /// 3. Calculate the state transition matrix \f$ \mathbf{\Phi} \f$ as
    
      /// \anchor eq-Loan-Phi \f{equation}{ \label{eq:eq-Loan-Phi}
    
      ///   \mathbf{\Phi} = \mathbf{B}_{22}^T
    
      /// \f}
    
      ///
    
      /// 4. Calculate the process noise covariance matrix \f$ \mathbf{Q} \f$ as
    
      /// \anchor eq-Loan-Q \f{equation}{ \label{eq:eq-Loan-Q}
    
      ///   \mathbf{Q} = \mathbf{\Phi} \mathbf{B}_{12}
    
      /// \f}
    
      ///
    
      /// @note See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential \cite Loan1978
    
      #pragma once
    
      #include <utility>
    
      #include <Eigen/Core>
    
      #include <unsupported/Eigen/MatrixFunctions>
    
      #include "Navigation/Math/Math.hpp"
    
      namespace NAV
    
      {
    
      /// @brief Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐
    
      /// @tparam DerivedF Matrix type of the F matrix
    
      /// @tparam DerivedG Matrix type of the G matrix
    
      /// @tparam DerivedW Matrix type of the W matrix
    
      /// @param[in] F System model matrix
    
      /// @param[in] G Noise model matrix
    
      /// @param[in] W Noise scale factors
    
      /// @param[in] dt Time step in [s]
    
      /// @return A pair with the matrices {𝚽, 𝐐}
    
      template<typename DerivedF, typename DerivedG, typename DerivedW>
    
      [[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject>
    
      74222
          calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F,
    
                                       const Eigen::MatrixBase<DerivedG>& G,
    
                                       const Eigen::MatrixBase<DerivedW>& W,
    
                                       double dt)
    
      {
    
          //     ┌            ┐
    
          //     │ -F  ┊ GWG^T│
    
          // A = │------------│ * dT
    
          //     │  0  ┊  F^T │
    
          //     └            ┘
    
          // W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote)
    
          // W = Identity, as noise scale factor is included within G matrix
    
        2/4✓ Branch 3 taken 37111 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 37111 times.
✗ Branch 7 not taken.

      74222
          Eigen::MatrixX<typename DerivedF::Scalar> A = Eigen::MatrixX<typename DerivedF::Scalar>::Zero(2 * F.rows(), 2 * F.cols());
    
        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;
    
        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();
    
        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();
    
        1/2✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.

      74222
          A *= typename DerivedF::Scalar(dt);
    
          // Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp)
    
        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();
    
          //               ┌                ┐
    
          //               │ ... ┊ Phi^-1 Q │
    
          // B = expm(A) = │----------------│
    
          //               │  0  ┊   Phi^T  │
    
          //               └                ┘
    
        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();
    
        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());
    
        1/2✓ Branch 1 taken 37111 times.
✗ Branch 2 not taken.

      148444
          return { Phi, Q };
    
      74222
      }
    
      /// @brief Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐
    
      /// @tparam DerivedF Matrix type of the F matrix
    
      /// @tparam DerivedG Matrix type of the G matrix
    
      /// @tparam DerivedW Matrix type of the W matrix
    
      /// @param[in] F System model matrix
    
      /// @param[in] G Noise model matrix
    
      /// @param[in] W Noise scale factors
    
      /// @param[in] dt Time step in [s]
    
      /// @param[in] expmOrder Order to use to calculate the exponential matrix
    
      /// @return A pair with the matrices {𝚽, 𝐐}
    
      template<typename DerivedF, typename DerivedG, typename DerivedW>
    
      [[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject>
    
          calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F,
    
                                       const Eigen::MatrixBase<DerivedG>& G,
    
                                       const Eigen::MatrixBase<DerivedW>& W,
    
                                       double dt,
    
                                       size_t expmOrder)
    
      {
    
          //     ┌            ┐
    
          //     │ -F  ┊ GWG^T│
    
          // A = │------------│ * dT
    
          //     │  0  ┊  F^T │
    
          //     └            ┘
    
          // W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote)
    
          // W = Identity, as noise scale factor is included within G matrix
    
          Eigen::MatrixX<typename DerivedF::Scalar> A = Eigen::MatrixX<typename DerivedF::Scalar>::Zero(2 * F.rows(), 2 * F.cols());
    
          A.topLeftCorner(F.rows(), F.cols()) = -F;
    
          A.topRightCorner(F.rows(), F.cols()) = G * W * G.transpose();
    
          A.bottomRightCorner(F.rows(), F.cols()) = F.transpose();
    
          A *= typename DerivedF::Scalar(dt);
    
          // Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp)
    
          Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic> B = math::expm(A, expmOrder);
    
          //               ┌                ┐
    
          //               │ ... ┊ Phi^-1 Q │
    
          // B = expm(A) = │----------------│
    
          //               │  0  ┊   Phi^T  │
    
          //               └                ┘
    
          typename DerivedF::PlainObject Phi = B.bottomRightCorner(F.rows(), F.cols()).transpose();
    
          typename DerivedF::PlainObject Q = Phi * B.topRightCorner(F.rows(), F.cols());
    
          return { Phi, Q };
    
      }
    
      } // namespace NAV

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			/// #### Algorithm description
15			/// 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$)
16			/// \anchor eq-Loan-A \f{equation}{ \label{eq:eq-Loan-A}
17			/// \mathbf{A} = \begin{bmatrix} -\mathbf{F} & \mathbf{G} \mathbf{W} \mathbf{G} \\
18			/// \mathbf{0} & \mathbf{F}^T \end{bmatrix} \Delta t
19			/// \f}
20			///
21			/// 2. Calculate the exponential of \f$ \mathbf{A} \f$
22			/// \anchor eq-Loan-B \f{equation}{ \label{eq:eq-Loan-B}
23			/// \mathbf{B} = \text{expm}(\mathbf{A}) = \left[ \begin{array}{c;{2pt/2pt}c}
24			/// \dots & \mathbf{\Phi}^{-1} \mathbf{Q} \\[2mm]
25			/// \hdashline[2pt/2pt] & \\[-2mm]
26			/// \mathbf{0} & \mathbf{\Phi}^T \end{array} \right]
27			/// = \begin{bmatrix} \mathbf{B}_{11} & \mathbf{B}_{12} \\
28			/// \mathbf{B}_{21} & \mathbf{B}_{22} \end{bmatrix}
29			/// \f}
30			///
31			/// 3. Calculate the state transition matrix \f$ \mathbf{\Phi} \f$ as
32			/// \anchor eq-Loan-Phi \f{equation}{ \label{eq:eq-Loan-Phi}
33			/// \mathbf{\Phi} = \mathbf{B}_{22}^T
34			/// \f}
35			///
36			/// 4. Calculate the process noise covariance matrix \f$ \mathbf{Q} \f$ as
37			/// \anchor eq-Loan-Q \f{equation}{ \label{eq:eq-Loan-Q}
38			/// \mathbf{Q} = \mathbf{\Phi} \mathbf{B}_{12}
39			/// \f}
40			///
41			/// @note See C.F. van Loan (1978) - Computing Integrals Involving the Matrix Exponential \cite Loan1978
42
43			#pragma once
44
45			#include <utility>
46			#include <Eigen/Core>
47			#include <unsupported/Eigen/MatrixFunctions>
48
49			#include "Navigation/Math/Math.hpp"
50
51			namespace NAV
52			{
53
54			/// @brief Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐
55			/// @tparam DerivedF Matrix type of the F matrix
56			/// @tparam DerivedG Matrix type of the G matrix
57			/// @tparam DerivedW Matrix type of the W matrix
58			/// @param[in] F System model matrix
59			/// @param[in] G Noise model matrix
60			/// @param[in] W Noise scale factors
61			/// @param[in] dt Time step in [s]
62			/// @return A pair with the matrices {𝚽, 𝐐}
63			template<typename DerivedF, typename DerivedG, typename DerivedW>
64			[[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject>
65		74222	calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F,
66			const Eigen::MatrixBase<DerivedG>& G,
67			const Eigen::MatrixBase<DerivedW>& W,
68			double dt)
69			{
70			// ┌ ┐
71			// │ -F ┊ GWG^T│
72			// A = │------------│ * dT
73			// │ 0 ┊ F^T │
74			// └ ┘
75			// W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote)
76			// W = Identity, as noise scale factor is included within G matrix
77	2/4 ✓ Branch 3 taken 37111 times. ✗ Branch 4 not taken. ✓ Branch 6 taken 37111 times. ✗ Branch 7 not taken.	74222	Eigen::MatrixX<typename DerivedF::Scalar> A = Eigen::MatrixX<typename DerivedF::Scalar>::Zero(2 * F.rows(), 2 * F.cols());
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.topLeftCorner(F.rows(), F.cols()) = -F;
79	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();
80	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();
81	1/2 ✓ Branch 1 taken 37111 times. ✗ Branch 2 not taken.	74222	A *= typename DerivedF::Scalar(dt);
82
83			// Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp)
84	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();
85
86			// ┌ ┐
87			// │ ... ┊ Phi^-1 Q │
88			// B = expm(A) = │----------------│
89			// │ 0 ┊ Phi^T │
90			// └ ┘
91	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();
92	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());
93
94	1/2 ✓ Branch 1 taken 37111 times. ✗ Branch 2 not taken.	148444	return { Phi, Q };
95		74222	}
96
97			/// @brief Numerical Method to calculate the State transition matrix 𝚽 and System/Process noise covariance matrix 𝐐
98			/// @tparam DerivedF Matrix type of the F matrix
99			/// @tparam DerivedG Matrix type of the G matrix
100			/// @tparam DerivedW Matrix type of the W matrix
101			/// @param[in] F System model matrix
102			/// @param[in] G Noise model matrix
103			/// @param[in] W Noise scale factors
104			/// @param[in] dt Time step in [s]
105			/// @param[in] expmOrder Order to use to calculate the exponential matrix
106			/// @return A pair with the matrices {𝚽, 𝐐}
107			template<typename DerivedF, typename DerivedG, typename DerivedW>
108			[[nodiscard]] std::pair<typename DerivedF::PlainObject, typename DerivedF::PlainObject>
109			calcPhiAndQWithVanLoanMethod(const Eigen::MatrixBase<DerivedF>& F,
110			const Eigen::MatrixBase<DerivedG>& G,
111			const Eigen::MatrixBase<DerivedW>& W,
112			double dt,
113			size_t expmOrder)
114			{
115			// ┌ ┐
116			// │ -F ┊ GWG^T│
117			// A = │------------│ * dT
118			// │ 0 ┊ F^T │
119			// └ ┘
120			// W = Power Spectral Density of u (See Brown & Hwang (2012) chapter 3.9, p. 126 - footnote)
121			// W = Identity, as noise scale factor is included within G matrix
122			Eigen::MatrixX<typename DerivedF::Scalar> A = Eigen::MatrixX<typename DerivedF::Scalar>::Zero(2 * F.rows(), 2 * F.cols());
123			A.topLeftCorner(F.rows(), F.cols()) = -F;
124			A.topRightCorner(F.rows(), F.cols()) = G * W * G.transpose();
125			A.bottomRightCorner(F.rows(), F.cols()) = F.transpose();
126			A *= typename DerivedF::Scalar(dt);
127
128			// Exponential Matrix of A (https://eigen.tuxfamily.org/dox/unsupported/group__MatrixFunctions__Module.html#matrixbase_exp)
129			Eigen::Matrix<typename DerivedF::Scalar, Eigen::Dynamic, Eigen::Dynamic> B = math::expm(A, expmOrder);
130
131			// ┌ ┐
132			// │ ... ┊ Phi^-1 Q │
133			// B = expm(A) = │----------------│
134			// │ 0 ┊ Phi^T │
135			// └ ┘
136			typename DerivedF::PlainObject Phi = B.bottomRightCorner(F.rows(), F.cols()).transpose();
137			typename DerivedF::PlainObject Q = Phi * B.topRightCorner(F.rows(), F.cols());
138
139			return { Phi, Q };
140			}
141
142			} // namespace NAV
143