INSTINCT Code Coverage Report

Directory:	src/
File:	Navigation/INS/LocalNavFrame/ErrorEquations.cpp
Date:	2025-02-07 16:54:41

	Exec	Total	Coverage
Lines:	90	90	100.0%
Functions:	12	12	100.0%
Branches:	42	84	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/.
    
      #include "ErrorEquations.hpp"
    
      #include "Navigation/Constants.hpp"
    
      namespace NAV
    
      {
    
      37109
      Eigen::Matrix3d n_F_dpsi_dpsi(const Eigen::Vector3d& n_omega_in)
    
      {
    
          // Math: \mathbf{F}_{11}^n = -[\mathbf{\omega}_{in}^n \land]
    
      37109
          Eigen::Matrix3d skewMat;
    
          // clang-format off
    
        5/10✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37109 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37109 times.
✗ Branch 14 not taken.

      37109
          skewMat <<       0       ,  n_omega_in(2), -n_omega_in(1),
    
        5/10✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37109 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37109 times.
✗ Branch 14 not taken.

      37109
                     -n_omega_in(2),       0       ,  n_omega_in(0),
    
        5/10✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 37109 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 37109 times.
✗ Branch 14 not taken.

      37109
                      n_omega_in(1), -n_omega_in(0),       0       ;
    
          // clang-format on
    
      37109
          return skewMat;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dpsi_dv(double latitude, double height, double R_N, double R_E)
    
      {
    
          // Math: \mathbf{F}_{12}^n = \begin{bmatrix} 0 & -\frac{1}{R_E + h} & 0 \\ \frac{1}{R_N + h} & 0 & 0 \\ 0 & \frac{\tan{\phi}}{R_E + h} & 0 \end{bmatrix}
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_12 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          n_F_12(0, 1) = -1 / (R_E + height);
    
      37109
          n_F_12(1, 0) = 1 / (R_N + height);
    
      37109
          n_F_12(2, 1) = std::tan(latitude) / (R_E + height);
    
      37109
          return n_F_12;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dpsi_dr(double latitude, double height, const Eigen::Vector3d& n_velocity, double R_N, double R_E)
    
      {
    
      37109
          const double& v_N = n_velocity(0); // North velocity in [m/s]
    
      37109
          const double& v_E = n_velocity(1); // East velocity in [m/s]
    
          // Math: \mathbf{F}_{13}^n = \begin{bmatrix} \omega_{ie}\sin{\phi} & 0 & \frac{v_E}{(R_E + h)^2} \\ 0 & 0 & -\frac{v_N}{(R_N + h)^2} \\ \omega_{ie}\cos{\phi} + \frac{v_E}{(R_E + h)\cos^2{\phi}} & 0 & -\frac{v_E\tan{\phi}}{(R_E + h)^2} \end{bmatrix}
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_13 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          n_F_13(0, 0) = InsConst::omega_ie * std::sin(latitude);
    
      37109
          n_F_13(0, 2) = v_E / std::pow(R_E + height, 2.0);
    
      37109
          n_F_13(1, 2) = -v_N / std::pow(R_N + height, 2.0);
    
      111327
          n_F_13(2, 0) = InsConst::omega_ie * std::cos(latitude)
    
      37109
                         + v_E / ((R_E + height) * std::pow(std::cos(latitude), 2));
    
      37109
          n_F_13(2, 2) = -v_E * std::tan(latitude) / std::pow(R_E + height, 2.0);
    
      37109
          return n_F_13;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dpsi_dw(const Eigen::Matrix3d& n_Dcm_b)
    
      {
    
      37109
          return n_Dcm_b;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dv_dpsi(const Eigen::Vector3d& n_force_ib)
    
      {
    
      37109
          const auto& f_N = n_force_ib(0); // Specific force in North direction in [m/s^2]
    
      37109
          const auto& f_E = n_force_ib(1); // Specific force in East direction in [m/s^2]
    
      37109
          const auto& f_D = n_force_ib(2); // Specific force in Down direction in [m/s^2]
    
          // Math: \mathbf{F}_{21}^n = \begin{bmatrix} (-\mathbf{C}_{b}^n \hat{f}_{ib}^b) \land \end{bmatrix}
    
      37109
          Eigen::Matrix3d skewMat;
    
          // clang-format off
    
        3/6✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.

      37109
          skewMat <<   0 ,  f_D, -f_E,
    
        3/6✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.

      37109
                     -f_D,   0 ,  f_N,
    
        3/6✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 37109 times.
✗ Branch 8 not taken.

      37109
                      f_E, -f_N,   0 ;
    
          // clang-format on
    
      37109
          return skewMat;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dv_dv(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E)
    
      {
    
      37109
          const double& v_N = n_velocity(0); // North velocity in [m/s]
    
      37109
          const double& v_E = n_velocity(1); // East velocity in [m/s]
    
      37109
          const double& v_D = n_velocity(2); // Down velocity in [m/s]
    
          // Math: \mathbf{F}_{22}^n = \begin{bmatrix} \frac{v_D}{R_N+h} & -2\frac{v_E\tan{\phi}}{R_E+h}-2\omega_{ie}\sin{\phi} & \frac{v_N}{R_N+h} \\ \frac{v_E\tan{\phi}}{R_E+h}+2\omega_{ie}\sin{\phi} & \frac{v_N\tan{\phi}+v_D}{R_E+h} & \frac{v_E}{R_E+h}+2\omega_{ie}\cos{\phi} \\ -\frac{2v_N}{R_N+h} & -\frac{2v_E}{R_E+h}-2\omega_{ie}\cos{\phi} & 0 \end{bmatrix}
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_22 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          n_F_22(0, 0) = v_D / (R_N + height);
    
      74218
          n_F_22(0, 1) = -2.0 * v_E * std::tan(latitude) / (R_E + height)
    
      37109
                         - 2.0 * InsConst::omega_ie * std::sin(latitude);
    
      37109
          n_F_22(0, 2) = v_N / (R_N + height);
    
      74218
          n_F_22(1, 0) = v_E * std::tan(latitude) / (R_E + height)
    
      37109
                         + 2.0 * InsConst::omega_ie * std::sin(latitude);
    
      37109
          n_F_22(1, 1) = (v_N * std::tan(latitude) + v_D) / (R_E + height);
    
      74218
          n_F_22(1, 2) = v_E / (R_E + height)
    
      37109
                         + 2.0 * InsConst::omega_ie * std::cos(latitude);
    
      37109
          n_F_22(2, 0) = -2.0 * v_N / (R_N + height);
    
      74218
          n_F_22(2, 1) = -2.0 * v_E / (R_E + height)
    
      37109
                         - 2.0 * InsConst::omega_ie * std::cos(latitude);
    
      37109
          return n_F_22;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dv_dr(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E, double g_0, double r_eS_e)
    
      {
    
          // Math: \mathbf{F}_{23}^n = \begin{bmatrix} -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2}-\frac{v_Nv_D}{(R_N+h)^2} \\ \frac{v_Nv_E}{(R_E+h)\cos^2{\phi}}+2v_N\omega_{ie}\cos{\phi}-2v_D\omega_{ie}\sin{\phi} & 0 & -\frac{v_Nv_E\tan{\phi}+v_Ev_D}{(R_E+h)^2} \\ 2v_E\omega_{ie}\sin{\phi} & 0 & \frac{v_E^2}{(R_E+h)^2}+\frac{v_N^2}{(R_N+h)^2}-\frac{2g_0}{r_{eS}^e} \end{bmatrix}
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_23 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          const double& v_N = n_velocity(0);
    
      37109
          const double& v_E = n_velocity(1);
    
      37109
          const double& v_D = n_velocity(2);
    
      37109
          n_F_23(0, 0) = -std::pow(v_E, 2) / ((R_E + height) * std::pow(std::cos(latitude), 2.0))
    
      37109
                         - 2.0 * v_E * InsConst::omega_ie * std::cos(latitude);
    
      37109
          n_F_23(0, 2) = std::pow(v_E, 2) * std::tan(latitude) / std::pow(R_E + height, 2.0)
    
      37109
                         - (v_N * v_D) / std::pow(R_N + height, 2.0);
    
      74218
          n_F_23(1, 0) = v_N * v_E / ((R_E + height) * std::pow(std::cos(latitude), 2.0))
    
      37109
                         + 2.0 * v_N * InsConst::omega_ie * std::cos(latitude)
    
      37109
                         - 2.0 * v_D * InsConst::omega_ie * std::sin(latitude);
    
      37109
          n_F_23(1, 2) = -(v_N * v_E * std::tan(latitude) + v_E * v_D) / std::pow(R_E + height, 2.0);
    
      37109
          n_F_23(2, 0) = 2.0 * v_E * InsConst::omega_ie * std::sin(latitude);
    
      37109
          n_F_23(2, 2) = std::pow(v_E, 2) / std::pow(R_E + height, 2.0)
    
      37109
                         + std::pow(v_N, 2) / std::pow(R_N + height, 2.0)
    
      37109
                         - 2 * g_0 / r_eS_e;
    
      37109
          return n_F_23;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dv_df(const Eigen::Matrix3d& n_Dcm_b)
    
      {
    
      37109
          return n_Dcm_b;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dr_dv(double latitude, double height, double R_N, double R_E)
    
      {
    
          // Math: \mathbf{F}_{32}^n = \begin{bmatrix} \frac{1}{R_N + h} & 0 & 0 \\ 0 & \frac{1}{(R_E + h)\cos{\phi}} & 0 \\ 0 & 0 & -1 \end{bmatrix} \quad \text{P. Groves}\,(14.70)
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_32 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          n_F_32(0, 0) = 1.0 / (R_N + height);
    
      37109
          n_F_32(1, 1) = 1.0 / ((R_E + height) * std::cos(latitude));
    
      37109
          n_F_32(2, 2) = -1;
    
      37109
          return n_F_32;
    
      }
    
      37109
      Eigen::Matrix3d n_F_dr_dr(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E)
    
      {
    
      37109
          const double& v_N = n_velocity(0);
    
      37109
          const double& v_E = n_velocity(1);
    
          // Math: \mathbf{F}_{33}^n = \begin{bmatrix} 0 & 0 & -\frac{v_N}{(R_N + h)^2} \\ \frac{v_E \tan{\phi}}{(R_E + h) \cos{\phi}} & 0 & -\frac{v_E}{(R_E + h)^2 \cos{\phi}} \\ 0 & 0 & 0 \end{bmatrix} \quad \text{P. Groves}\,(14.71)
    
        2/4✓ Branch 1 taken 37109 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 37109 times.
✗ Branch 5 not taken.

      37109
          Eigen::Matrix3d n_F_33 = Eigen::Matrix3d::Zero(3, 3);
    
      37109
          n_F_33(0, 2) = -v_N / std::pow(R_N + height, 2.0);
    
      37109
          n_F_33(1, 0) = v_E * std::tan(latitude) / ((R_E + height) * std::cos(latitude));
    
      37109
          n_F_33(1, 2) = -v_E / (std::pow(R_E + height, 2.0) * std::cos(latitude));
    
      37109
          return n_F_33;
    
      }
    
      30665
      Eigen::Matrix3d n_F_df_df(const Eigen::Vector3d& beta_a)
    
      {
    
          // Math: \mathbf{F}_{a} = - \begin{bmatrix} \beta_{a,1} & 0 & 0 \\ 0 & \beta_{a,2} & 0 \\ 0 & 0 & \beta_{a,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3)
    
        3/6✓ Branch 1 taken 30665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30665 times.
✗ Branch 8 not taken.

      30665
          return -1.0 * beta_a.asDiagonal();
    
      }
    
      30665
      Eigen::Matrix3d n_F_dw_dw(const Eigen::Vector3d& beta_omega)
    
      {
    
          // Math: \mathbf{F}_{\omega} = - \begin{bmatrix} \beta_{\omega,1} & 0 & 0 \\ 0 & \beta_{\omega,2} & 0 \\ 0 & 0 & \beta_{\omega,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3)
    
        3/6✓ Branch 1 taken 30665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 30665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 30665 times.
✗ Branch 8 not taken.

      30665
          return -1.0 * beta_omega.asDiagonal();
    
      }
    
      } // 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			#include "ErrorEquations.hpp"
10
11			#include "Navigation/Constants.hpp"
12
13			namespace NAV
14			{
15
16		37109	Eigen::Matrix3d n_F_dpsi_dpsi(const Eigen::Vector3d& n_omega_in)
17			{
18			// Math: \mathbf{F}_{11}^n = -[\mathbf{\omega}_{in}^n \land]
19		37109	Eigen::Matrix3d skewMat;
20			// clang-format off
21	5/10 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 37109 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 37109 times. ✗ Branch 14 not taken.	37109	skewMat << 0 , n_omega_in(2), -n_omega_in(1),
22	5/10 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 37109 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 37109 times. ✗ Branch 14 not taken.	37109	-n_omega_in(2), 0 , n_omega_in(0),
23	5/10 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken. ✓ Branch 10 taken 37109 times. ✗ Branch 11 not taken. ✓ Branch 13 taken 37109 times. ✗ Branch 14 not taken.	37109	n_omega_in(1), -n_omega_in(0), 0 ;
24			// clang-format on
25		37109	return skewMat;
26			}
27
28		37109	Eigen::Matrix3d n_F_dpsi_dv(double latitude, double height, double R_N, double R_E)
29			{
30			// Math: \mathbf{F}_{12}^n = \begin{bmatrix} 0 & -\frac{1}{R_E + h} & 0 \\ \frac{1}{R_N + h} & 0 & 0 \\ 0 & \frac{\tan{\phi}}{R_E + h} & 0 \end{bmatrix}
31	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_12 = Eigen::Matrix3d::Zero(3, 3);
32
33		37109	n_F_12(0, 1) = -1 / (R_E + height);
34		37109	n_F_12(1, 0) = 1 / (R_N + height);
35		37109	n_F_12(2, 1) = std::tan(latitude) / (R_E + height);
36
37		37109	return n_F_12;
38			}
39
40		37109	Eigen::Matrix3d n_F_dpsi_dr(double latitude, double height, const Eigen::Vector3d& n_velocity, double R_N, double R_E)
41			{
42		37109	const double& v_N = n_velocity(0); // North velocity in [m/s]
43		37109	const double& v_E = n_velocity(1); // East velocity in [m/s]
44
45			// Math: \mathbf{F}_{13}^n = \begin{bmatrix} \omega_{ie}\sin{\phi} & 0 & \frac{v_E}{(R_E + h)^2} \\ 0 & 0 & -\frac{v_N}{(R_N + h)^2} \\ \omega_{ie}\cos{\phi} + \frac{v_E}{(R_E + h)\cos^2{\phi}} & 0 & -\frac{v_E\tan{\phi}}{(R_E + h)^2} \end{bmatrix}
46	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_13 = Eigen::Matrix3d::Zero(3, 3);
47
48		37109	n_F_13(0, 0) = InsConst::omega_ie * std::sin(latitude);
49		37109	n_F_13(0, 2) = v_E / std::pow(R_E + height, 2.0);
50		37109	n_F_13(1, 2) = -v_N / std::pow(R_N + height, 2.0);
51		111327	n_F_13(2, 0) = InsConst::omega_ie * std::cos(latitude)
52		37109	+ v_E / ((R_E + height) * std::pow(std::cos(latitude), 2));
53		37109	n_F_13(2, 2) = -v_E * std::tan(latitude) / std::pow(R_E + height, 2.0);
54
55		37109	return n_F_13;
56			}
57
58		37109	Eigen::Matrix3d n_F_dpsi_dw(const Eigen::Matrix3d& n_Dcm_b)
59			{
60		37109	return n_Dcm_b;
61			}
62
63		37109	Eigen::Matrix3d n_F_dv_dpsi(const Eigen::Vector3d& n_force_ib)
64			{
65		37109	const auto& f_N = n_force_ib(0); // Specific force in North direction in [m/s^2]
66		37109	const auto& f_E = n_force_ib(1); // Specific force in East direction in [m/s^2]
67		37109	const auto& f_D = n_force_ib(2); // Specific force in Down direction in [m/s^2]
68
69			// Math: \mathbf{F}_{21}^n = \begin{bmatrix} (-\mathbf{C}_{b}^n \hat{f}_{ib}^b) \land \end{bmatrix}
70		37109	Eigen::Matrix3d skewMat;
71			// clang-format off
72	3/6 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken.	37109	skewMat << 0 , f_D, -f_E,
73	3/6 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken.	37109	-f_D, 0 , f_N,
74	3/6 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 37109 times. ✗ Branch 8 not taken.	37109	f_E, -f_N, 0 ;
75			// clang-format on
76
77		37109	return skewMat;
78			}
79
80		37109	Eigen::Matrix3d n_F_dv_dv(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E)
81			{
82		37109	const double& v_N = n_velocity(0); // North velocity in [m/s]
83		37109	const double& v_E = n_velocity(1); // East velocity in [m/s]
84		37109	const double& v_D = n_velocity(2); // Down velocity in [m/s]
85
86			// Math: \mathbf{F}_{22}^n = \begin{bmatrix} \frac{v_D}{R_N+h} & -2\frac{v_E\tan{\phi}}{R_E+h}-2\omega_{ie}\sin{\phi} & \frac{v_N}{R_N+h} \\ \frac{v_E\tan{\phi}}{R_E+h}+2\omega_{ie}\sin{\phi} & \frac{v_N\tan{\phi}+v_D}{R_E+h} & \frac{v_E}{R_E+h}+2\omega_{ie}\cos{\phi} \\ -\frac{2v_N}{R_N+h} & -\frac{2v_E}{R_E+h}-2\omega_{ie}\cos{\phi} & 0 \end{bmatrix}
87	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_22 = Eigen::Matrix3d::Zero(3, 3);
88
89		37109	n_F_22(0, 0) = v_D / (R_N + height);
90		74218	n_F_22(0, 1) = -2.0 * v_E * std::tan(latitude) / (R_E + height)
91		37109	- 2.0 * InsConst::omega_ie * std::sin(latitude);
92		37109	n_F_22(0, 2) = v_N / (R_N + height);
93		74218	n_F_22(1, 0) = v_E * std::tan(latitude) / (R_E + height)
94		37109	+ 2.0 * InsConst::omega_ie * std::sin(latitude);
95		37109	n_F_22(1, 1) = (v_N * std::tan(latitude) + v_D) / (R_E + height);
96		74218	n_F_22(1, 2) = v_E / (R_E + height)
97		37109	+ 2.0 * InsConst::omega_ie * std::cos(latitude);
98		37109	n_F_22(2, 0) = -2.0 * v_N / (R_N + height);
99		74218	n_F_22(2, 1) = -2.0 * v_E / (R_E + height)
100		37109	- 2.0 * InsConst::omega_ie * std::cos(latitude);
101
102		37109	return n_F_22;
103			}
104
105		37109	Eigen::Matrix3d n_F_dv_dr(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E, double g_0, double r_eS_e)
106			{
107			// Math: \mathbf{F}_{23}^n = \begin{bmatrix} -\frac{v_E^2}{(R_E+h)\cos^2{\phi}}-2v_E\omega_{ie}\cos{\phi} & 0 & \frac{v_E^2\tan{\phi}}{(R_E+h)^2}-\frac{v_Nv_D}{(R_N+h)^2} \\ \frac{v_Nv_E}{(R_E+h)\cos^2{\phi}}+2v_N\omega_{ie}\cos{\phi}-2v_D\omega_{ie}\sin{\phi} & 0 & -\frac{v_Nv_E\tan{\phi}+v_Ev_D}{(R_E+h)^2} \\ 2v_E\omega_{ie}\sin{\phi} & 0 & \frac{v_E^2}{(R_E+h)^2}+\frac{v_N^2}{(R_N+h)^2}-\frac{2g_0}{r_{eS}^e} \end{bmatrix}
108	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_23 = Eigen::Matrix3d::Zero(3, 3);
109
110		37109	const double& v_N = n_velocity(0);
111		37109	const double& v_E = n_velocity(1);
112		37109	const double& v_D = n_velocity(2);
113
114		37109	n_F_23(0, 0) = -std::pow(v_E, 2) / ((R_E + height) * std::pow(std::cos(latitude), 2.0))
115		37109	- 2.0 * v_E * InsConst::omega_ie * std::cos(latitude);
116
117		37109	n_F_23(0, 2) = std::pow(v_E, 2) * std::tan(latitude) / std::pow(R_E + height, 2.0)
118		37109	- (v_N * v_D) / std::pow(R_N + height, 2.0);
119
120		74218	n_F_23(1, 0) = v_N * v_E / ((R_E + height) * std::pow(std::cos(latitude), 2.0))
121		37109	+ 2.0 * v_N * InsConst::omega_ie * std::cos(latitude)
122		37109	- 2.0 * v_D * InsConst::omega_ie * std::sin(latitude);
123
124		37109	n_F_23(1, 2) = -(v_N * v_E * std::tan(latitude) + v_E * v_D) / std::pow(R_E + height, 2.0);
125
126		37109	n_F_23(2, 0) = 2.0 * v_E * InsConst::omega_ie * std::sin(latitude);
127
128		37109	n_F_23(2, 2) = std::pow(v_E, 2) / std::pow(R_E + height, 2.0)
129		37109	+ std::pow(v_N, 2) / std::pow(R_N + height, 2.0)
130		37109	- 2 * g_0 / r_eS_e;
131
132		37109	return n_F_23;
133			}
134
135		37109	Eigen::Matrix3d n_F_dv_df(const Eigen::Matrix3d& n_Dcm_b)
136			{
137		37109	return n_Dcm_b;
138			}
139
140		37109	Eigen::Matrix3d n_F_dr_dv(double latitude, double height, double R_N, double R_E)
141			{
142			// Math: \mathbf{F}_{32}^n = \begin{bmatrix} \frac{1}{R_N + h} & 0 & 0 \\ 0 & \frac{1}{(R_E + h)\cos{\phi}} & 0 \\ 0 & 0 & -1 \end{bmatrix} \quad \text{P. Groves}\,(14.70)
143	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_32 = Eigen::Matrix3d::Zero(3, 3);
144		37109	n_F_32(0, 0) = 1.0 / (R_N + height);
145		37109	n_F_32(1, 1) = 1.0 / ((R_E + height) * std::cos(latitude));
146		37109	n_F_32(2, 2) = -1;
147
148		37109	return n_F_32;
149			}
150
151		37109	Eigen::Matrix3d n_F_dr_dr(const Eigen::Vector3d& n_velocity, double latitude, double height, double R_N, double R_E)
152			{
153		37109	const double& v_N = n_velocity(0);
154		37109	const double& v_E = n_velocity(1);
155
156			// Math: \mathbf{F}_{33}^n = \begin{bmatrix} 0 & 0 & -\frac{v_N}{(R_N + h)^2} \\ \frac{v_E \tan{\phi}}{(R_E + h) \cos{\phi}} & 0 & -\frac{v_E}{(R_E + h)^2 \cos{\phi}} \\ 0 & 0 & 0 \end{bmatrix} \quad \text{P. Groves}\,(14.71)
157	2/4 ✓ Branch 1 taken 37109 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 37109 times. ✗ Branch 5 not taken.	37109	Eigen::Matrix3d n_F_33 = Eigen::Matrix3d::Zero(3, 3);
158
159		37109	n_F_33(0, 2) = -v_N / std::pow(R_N + height, 2.0);
160		37109	n_F_33(1, 0) = v_E * std::tan(latitude) / ((R_E + height) * std::cos(latitude));
161		37109	n_F_33(1, 2) = -v_E / (std::pow(R_E + height, 2.0) * std::cos(latitude));
162
163		37109	return n_F_33;
164			}
165
166		30665	Eigen::Matrix3d n_F_df_df(const Eigen::Vector3d& beta_a)
167			{
168			// Math: \mathbf{F}_{a} = - \begin{bmatrix} \beta_{a,1} & 0 & 0 \\ 0 & \beta_{a,2} & 0 \\ 0 & 0 & \beta_{a,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3)
169	3/6 ✓ Branch 1 taken 30665 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 30665 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 30665 times. ✗ Branch 8 not taken.	30665	return -1.0 * beta_a.asDiagonal();
170			}
171
172		30665	Eigen::Matrix3d n_F_dw_dw(const Eigen::Vector3d& beta_omega)
173			{
174			// Math: \mathbf{F}_{\omega} = - \begin{bmatrix} \beta_{\omega,1} & 0 & 0 \\ 0 & \beta_{\omega,2} & 0 \\ 0 & 0 & \beta_{\omega,2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3)
175	3/6 ✓ Branch 1 taken 30665 times. ✗ Branch 2 not taken. ✓ Branch 4 taken 30665 times. ✗ Branch 5 not taken. ✓ Branch 7 taken 30665 times. ✗ Branch 8 not taken.	30665	return -1.0 * beta_omega.asDiagonal();
176			}
177
178			} // namespace NAV
179