| 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 "ProcessNoise.hpp" | ||
| 10 | |||
| 11 | #include <Eigen/Dense> | ||
| 12 | |||
| 13 | namespace NAV | ||
| 14 | { | ||
| 15 | |||
| 16 | ✗ | Eigen::Matrix3d G_RandomWalk(const Eigen::Vector3d& sigma2) | |
| 17 | { | ||
| 18 | // Math: \mathbf{G} = \begin{bmatrix} \sqrt{\sigma_{1}^2} & 0 & 0 \\ 0 & \sqrt{\sigma_{2}^2} & 0 \\ 0 & 0 & \sqrt{\sigma_{3}^2} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3) | ||
| 19 | |||
| 20 | ✗ | return Eigen::DiagonalMatrix<double, 3>{ sigma2.cwiseSqrt() }; | |
| 21 | } | ||
| 22 | |||
| 23 | ✗ | Eigen::Matrix3d G_GaussMarkov1(const Eigen::Vector3d& sigma2, const Eigen::Vector3d& beta) | |
| 24 | { | ||
| 25 | // Math: \mathbf{G} = \begin{bmatrix} \sqrt{2 \sigma_{1}^2 \beta_{1}} & 0 & 0 \\ 0 & \sqrt{2 \sigma_{2}^2 \beta_{2}} & 0 \\ 0 & 0 & \sqrt{2 \sigma_{3}^2 \beta_{3}} \end{bmatrix} \quad \text{T. Hobiger}\,(6.3) | ||
| 26 | ✗ | return Eigen::DiagonalMatrix<double, 3>{ (2.0 * beta.cwiseProduct(sigma2)).cwiseSqrt() }; | |
| 27 | } | ||
| 28 | |||
| 29 | 25776 | Eigen::Vector3d psdBiasGaussMarkov(const Eigen::Vector3d& sigma2_bd, const Eigen::Vector3d& tau_bd) | |
| 30 | { | ||
| 31 | // Math: u_{bias} = \sqrt{\frac{2\sigma_{bd}^2}{\tau_{bd}}} \qquad \text{Brown & Hwang, example 9.6} | ||
| 32 |
6/12✓ Branch 1 taken 25776 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 25776 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25776 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 25776 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 25776 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 25776 times.
✗ Branch 17 not taken.
|
25776 | return (2 * sigma2_bd.array() / tau_bd.array()).cwiseSqrt(); |
| 33 | } | ||
| 34 | |||
| 35 | 12888 | Eigen::Matrix3d Q_psi_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const double& tau_s) | |
| 36 | { | ||
| 37 |
9/18✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12888 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12888 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12888 times.
✗ Branch 24 not taken.
✓ Branch 26 taken 12888 times.
✗ Branch 27 not taken.
|
12888 | return (S_rg.asDiagonal() * tau_s + 1.0 / 3.0 * S_bgd.asDiagonal() * std::pow(tau_s, 3)) * Eigen::Matrix3d::Identity(); |
| 38 | } | ||
| 39 | |||
| 40 | 12888 | Eigen::Matrix3d ien_Q_dv_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s) | |
| 41 | { | ||
| 42 |
9/18✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 12888 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 12888 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 12888 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 12888 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 12888 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 12888 times.
✗ Branch 28 not taken.
|
12888 | return (0.5 * S_rg.asDiagonal() * std::pow(tau_s, 2) + 0.25 * S_bgd.asDiagonal() * std::pow(tau_s, 4)) * ien_F_21; |
| 43 | } | ||
| 44 | |||
| 45 | 12888 | Eigen::Matrix3d ien_Q_dv_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const double& tau_s) | |
| 46 | { | ||
| 47 |
8/16✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12888 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 12888 times.
✗ Branch 21 not taken.
✓ Branch 23 taken 12888 times.
✗ Branch 24 not taken.
|
12888 | return (S_ra.asDiagonal() * tau_s + 1.0 / 3.0 * S_bad.asDiagonal() * std::pow(tau_s, 3)) * Eigen::Matrix3d::Identity() |
| 48 |
12/24✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 12888 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 12888 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 12888 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 12888 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 12888 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 12888 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 12888 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 12888 times.
✗ Branch 37 not taken.
|
25776 | + (1.0 / 3.0 * S_rg.asDiagonal() * std::pow(tau_s, 3) + 0.2 * S_bgd.asDiagonal() * std::pow(tau_s, 5)) * ien_F_21 * ien_F_21.transpose(); |
| 49 | } | ||
| 50 | |||
| 51 | 12888 | Eigen::Matrix3d ien_Q_dv_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ien_F_21, const Eigen::Matrix3d& ien_Dcm_b, const double& tau_s) | |
| 52 | { | ||
| 53 |
6/12✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 12888 times.
✗ Branch 18 not taken.
|
12888 | return 1.0 / 3.0 * S_bgd.asDiagonal() * std::pow(tau_s, 3) * ien_F_21 * ien_Dcm_b; |
| 54 | } | ||
| 55 | |||
| 56 | 6444 | Eigen::Matrix3d n_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s) | |
| 57 | { | ||
| 58 |
10/20✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6444 times.
✗ Branch 31 not taken.
|
6444 | return (1.0 / 3.0 * S_rg.asDiagonal() * std::pow(tau_s, 3) + 0.2 * S_bgd.asDiagonal() * std::pow(tau_s, 5)) * T_rn_p * n_F_21; |
| 59 | } | ||
| 60 | |||
| 61 | 6444 | Eigen::Matrix3d ie_Q_dr_psi(const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s) | |
| 62 | { | ||
| 63 |
9/18✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
|
6444 | return (1.0 / 3.0 * S_rg.asDiagonal() * std::pow(tau_s, 3) + 0.2 * S_bgd.asDiagonal() * std::pow(tau_s, 5)) * ie_F_21; |
| 64 | } | ||
| 65 | |||
| 66 | 6444 | Eigen::Matrix3d n_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s) | |
| 67 | { | ||
| 68 |
8/16✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
|
6444 | return (0.5 * S_ra.asDiagonal() * std::pow(tau_s, 2) + 0.25 * S_bad.asDiagonal() * std::pow(tau_s, 4)) * T_rn_p |
| 69 |
13/26✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6444 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 6444 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6444 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 6444 times.
✗ Branch 40 not taken.
|
12888 | + (0.25 * S_rg.asDiagonal() * std::pow(tau_s, 4) + 1.0 / 6.0 * S_bgd.asDiagonal() * std::pow(tau_s, 6)) * T_rn_p * n_F_21 * n_F_21.transpose(); |
| 70 | } | ||
| 71 | |||
| 72 | 6444 | Eigen::Matrix3d ie_Q_dr_dv(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s) | |
| 73 | { | ||
| 74 |
9/18✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
|
6444 | return (0.5 * S_ra.asDiagonal() * std::pow(tau_s, 2) + 0.25 * S_bad.asDiagonal() * std::pow(tau_s, 4)) * Eigen::Matrix3d::Identity() |
| 75 |
12/24✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6444 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 6444 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6444 times.
✗ Branch 37 not taken.
|
12888 | + (0.25 * S_rg.asDiagonal() * std::pow(tau_s, 4) + 1.0 / 6.0 * S_bgd.asDiagonal() * std::pow(tau_s, 6)) * ie_F_21 * ie_F_21.transpose(); |
| 76 | } | ||
| 77 | |||
| 78 | 6444 | Eigen::Matrix3d n_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const double& tau_s) | |
| 79 | { | ||
| 80 |
9/18✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 12 taken 6444 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
|
6444 | return (1.0 / 3.0 * S_ra.asDiagonal() * std::pow(tau_s, 3) + 0.2 * S_bad.asDiagonal() * std::pow(tau_s, 5)) * T_rn_p * T_rn_p |
| 81 |
14/28✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6444 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 6444 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6444 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 6444 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 6444 times.
✗ Branch 43 not taken.
|
12888 | + (0.2 * S_rg.asDiagonal() * std::pow(tau_s, 5) + 1.0 / 7.0 * S_bgd.asDiagonal() * std::pow(tau_s, 7)) * T_rn_p * n_F_21 * n_F_21.transpose() * T_rn_p; |
| 82 | } | ||
| 83 | |||
| 84 | 6444 | Eigen::Matrix3d ie_Q_dr_dr(const Eigen::Vector3d& S_ra, const Eigen::Vector3d& S_bad, const Eigen::Vector3d& S_rg, const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const double& tau_s) | |
| 85 | { | ||
| 86 |
9/18✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
|
6444 | return (1.0 / 3.0 * S_ra.asDiagonal() * std::pow(tau_s, 3) + 0.2 * S_bad.asDiagonal() * std::pow(tau_s, 5)) * Eigen::Matrix3d::Identity() |
| 87 |
12/24✓ Branch 1 taken 6444 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 15 taken 6444 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 6444 times.
✗ Branch 19 not taken.
✓ Branch 21 taken 6444 times.
✗ Branch 22 not taken.
✓ Branch 24 taken 6444 times.
✗ Branch 25 not taken.
✓ Branch 27 taken 6444 times.
✗ Branch 28 not taken.
✓ Branch 30 taken 6444 times.
✗ Branch 31 not taken.
✓ Branch 33 taken 6444 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 6444 times.
✗ Branch 37 not taken.
|
12888 | + (0.2 * S_rg.asDiagonal() * std::pow(tau_s, 5) + 1.0 / 7.0 * S_bgd.asDiagonal() * std::pow(tau_s, 7)) * ie_F_21 * ie_F_21.transpose(); |
| 88 | } | ||
| 89 | |||
| 90 | 6444 | Eigen::Matrix3d n_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s) | |
| 91 | { | ||
| 92 |
6/12✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6444 times.
✗ Branch 18 not taken.
|
6444 | return 1.0 / 3.0 * S_bad.asDiagonal() * std::pow(tau_s, 3) * T_rn_p * n_Dcm_b; |
| 93 | } | ||
| 94 | |||
| 95 | 6444 | Eigen::Matrix3d ie_Q_dr_df(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s) | |
| 96 | { | ||
| 97 |
5/10✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
|
6444 | return 1.0 / 3.0 * S_bad.asDiagonal() * std::pow(tau_s, 3) * ie_Dcm_b; |
| 98 | } | ||
| 99 | |||
| 100 | 6444 | Eigen::Matrix3d n_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& n_F_21, const Eigen::Matrix3d& T_rn_p, const Eigen::Matrix3d& n_Dcm_b, const double& tau_s) | |
| 101 | { | ||
| 102 |
7/14✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6444 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 6444 times.
✗ Branch 21 not taken.
|
6444 | return 0.25 * S_bgd.asDiagonal() * std::pow(tau_s, 4) * T_rn_p * n_F_21 * n_Dcm_b; |
| 103 | } | ||
| 104 | |||
| 105 | 6444 | Eigen::Matrix3d ie_Q_dr_domega(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& ie_F_21, const Eigen::Matrix3d& ie_Dcm_b, const double& tau_s) | |
| 106 | { | ||
| 107 |
6/12✓ Branch 2 taken 6444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 6444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 6444 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 6444 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 6444 times.
✗ Branch 15 not taken.
✓ Branch 17 taken 6444 times.
✗ Branch 18 not taken.
|
6444 | return 0.25 * S_bgd.asDiagonal() * std::pow(tau_s, 4) * ie_F_21 * ie_Dcm_b; |
| 108 | } | ||
| 109 | |||
| 110 | 12888 | Eigen::Matrix3d Q_df_dv(const Eigen::Vector3d& S_bad, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s) | |
| 111 | { | ||
| 112 |
5/10✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
|
12888 | return 0.5 * S_bad.asDiagonal() * std::pow(tau_s, 2) * b_Dcm_ien; |
| 113 | } | ||
| 114 | |||
| 115 | 12888 | Eigen::Matrix3d Q_df_df(const Eigen::Vector3d& S_bad, const double& tau_s) | |
| 116 | { | ||
| 117 |
5/10✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
|
12888 | return S_bad.asDiagonal() * tau_s * Eigen::Matrix3d::Identity(); |
| 118 | } | ||
| 119 | |||
| 120 | 12888 | Eigen::Matrix3d Q_domega_psi(const Eigen::Vector3d& S_bgd, const Eigen::Matrix3d& b_Dcm_ien, const double& tau_s) | |
| 121 | { | ||
| 122 |
5/10✓ Branch 2 taken 12888 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 12888 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 12888 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 12888 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 12888 times.
✗ Branch 15 not taken.
|
12888 | return 0.5 * S_bgd.asDiagonal() * std::pow(tau_s, 2) * b_Dcm_ien; |
| 123 | } | ||
| 124 | |||
| 125 | 12888 | Eigen::Matrix3d Q_domega_domega(const Eigen::Vector3d& S_bgd, const double& tau_s) | |
| 126 | { | ||
| 127 |
5/10✓ Branch 1 taken 12888 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 12888 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 12888 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 12888 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 12888 times.
✗ Branch 14 not taken.
|
12888 | return S_bgd.asDiagonal() * tau_s * Eigen::Matrix3d::Identity(); |
| 128 | } | ||
| 129 | |||
| 130 | ✗ | Eigen::Matrix2d Q_gnss(const double& S_cPhi, const double& S_cf, const double& tau_s) | |
| 131 | { | ||
| 132 | ✗ | Eigen::Matrix2d Qg = Eigen::Matrix2d::Zero(); | |
| 133 | ✗ | Qg(0, 0) = S_cPhi * tau_s + 1. / 3. * S_cf * std::pow(tau_s, 3); | |
| 134 | ✗ | Qg(0, 1) = 0.5 * S_cf * std::pow(tau_s, 2); | |
| 135 | ✗ | Qg(1, 0) = 0.5 * S_cf * std::pow(tau_s, 2); | |
| 136 | ✗ | Qg(1, 1) = S_cf * tau_s; | |
| 137 | ✗ | return Qg; | |
| 138 | } | ||
| 139 | |||
| 140 | } // namespace NAV | ||
| 141 |