| 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 EllipsoidalRegion.hpp | ||
| 10 | /// @brief Calculate the ellipsoidal region Chi^2 | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-09-29 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cmath> | ||
| 17 | #include <Eigen/Core> | ||
| 18 | #include <gcem.hpp> | ||
| 19 | |||
| 20 | namespace NAV::Ambiguity | ||
| 21 | { | ||
| 22 | |||
| 23 | namespace internal | ||
| 24 | { | ||
| 25 | |||
| 26 | /// @brief Calculates \f$ \chi^2 \f$, the size of the ellipsoidal region, via volume of the ellipsoidal region | ||
| 27 | /// | ||
| 28 | /// The volume, expressed in \f$ [\text{cycles}^n] \f$, of the ellipsoidal region is (\cite deJonge1996 de Jonge 1996, ch. 4.9, eq. 4.19) | ||
| 29 | /// \anchor eq-GNSS-chi2-volume \f{equation}{ \label{eq:eq-GNSS-chi2-volume} | ||
| 30 | /// E_n = \chi^n \sqrt{|Q_{\hat{a}}|} V_n | ||
| 31 | /// \f} | ||
| 32 | /// | ||
| 33 | /// The volume function is (\cite deJonge1996 de Jonge 1996, ch. 4.9, eq. 4.20) | ||
| 34 | /// \anchor eq-GNSS-chi2-volume-function \f{equation}{ \label{eq:eq-GNSS-chi2-volume-function} | ||
| 35 | /// V_n = \frac{2}{n} \frac{\pi^{\frac{n}{2}}}{\Gamma \left( \frac{n}{2} \right)} | ||
| 36 | /// \f} | ||
| 37 | /// | ||
| 38 | /// The determinant of the vairance covariance matrix is (\cite deJonge1996 de Jonge 1996, ch. 4.9, eq. 4.24) | ||
| 39 | /// \anchor eq-GNSS-chi2-volume-detQ \f{equation}{ \label{eq:eq-GNSS-chi2-volume-detQ} | ||
| 40 | /// |Q_{\hat{a}}| = \prod_{i=1}^{n} \sigma^2_{\hat{a}_{i|i+1, \dots, n}} | ||
| 41 | /// \f} | ||
| 42 | /// | ||
| 43 | /// The volume \f$ E_n \f$ is good indicator for the number of candidates (\cite deJonge1996 de Jonge 1996, ch. 4.10) | ||
| 44 | /// \anchor eq-GNSS-chi2-volume-cand \f{equation}{ \label{eq:eq-GNSS-chi2-volume-cand} | ||
| 45 | /// n_{\text{cand}} = \text{(int)}E_n | ||
| 46 | /// \f} | ||
| 47 | /// | ||
| 48 | /// @param[in] D Vector containing all the variances of the ambiguities (L^T * D * L decomposition) | ||
| 49 | /// @param[in] ncand Requested number of candidates (default = 2) | ||
| 50 | /// @param[in] factor Multiplication factor for the volume of the resulting search ellipsoid (default = 1.5) | ||
| 51 | /// @return Size of the ellipsoidal region \f$ \chi^2 \f$ | ||
| 52 | template<typename DerivedD> | ||
| 53 | 14 | double calcChi2_volume(const Eigen::MatrixBase<DerivedD>& D, const Eigen::Index& ncand = 2, double factor = 1.5) | |
| 54 | { | ||
| 55 | 14 | auto n = static_cast<double>(D.rows()); | |
| 56 | |||
| 57 | // Volume function (eq. 4.20) | ||
| 58 | 14 | double Vn = 2.0 * std::pow(M_PI, n / 2.0) / (n * std::tgamma(n / 2.0)); | |
| 59 | // Determinant of the variance covariance matrix (eq. 4.24) | ||
| 60 | 14 | double detQ = D(0); | |
| 61 |
2/2✓ Branch 2 taken 14 times.
✓ Branch 3 taken 7 times.
|
42 | for (Eigen::Index i = 1; i < D.rows(); i++) { detQ *= D(i); } |
| 62 | |||
| 63 | 14 | return factor * std::pow(static_cast<double>(ncand) / (std::sqrt(detQ * Vn)), 2.0 / n); // Matlab code has: std::sqrt(detQ * Vn), paper not | |
| 64 | } | ||
| 65 | |||
| 66 | /// @brief Calculates \f$ \chi^2 \f$, the size of the ellipsoidal region, via bootrapping | ||
| 67 | /// | ||
| 68 | /// \f$ \boldsymbol{\check{z}}_B \f$ is a good candidate for setting the size of the search space (\cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.4.2, eq. 23.58) | ||
| 69 | /// \anchor eq-GNSS-chi2-bootstrap \f{equation}{ \label{eq:eq-GNSS-chi2-bootstrap} | ||
| 70 | /// \chi^2 = || \boldsymbol{\hat{z}} - \boldsymbol{\check{z}}_B ||_{\mathbf{Q}_{z}}^2 = (\boldsymbol{\hat{z}} - \boldsymbol{\check{z}}_B)^T \mathbf{Q}_{z}^{-1} (\boldsymbol{\hat{z}} - \boldsymbol{\check{z}}_B) | ||
| 71 | /// \f} | ||
| 72 | /// | ||
| 73 | /// @param[in] a Float ambiguity vector [cycles] | ||
| 74 | /// @param[in] Q Variance/covariance matrix of the ambiguities | ||
| 75 | /// @param[in] L_LTDL_Q Lower-triangular matrix from the L^T * D * L decomposition of Q_z | ||
| 76 | /// @param[in] ncand Requested number of candidates (default = 2) | ||
| 77 | /// @return Size of the ellipsoidal region \f$ \chi^2 \f$ | ||
| 78 | /// @note See \cite deJonge1996 de Jonge 1996, ch. 4.11 | ||
| 79 | template<typename DerivedA, typename DerivedQ, typename DerivedL> | ||
| 80 | 234 | double calcChi2_bootstrap(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q, | |
| 81 | const Eigen::MatrixBase<DerivedL>& L_LTDL_Q, const Eigen::Index& ncand = 2) | ||
| 82 | { | ||
| 83 |
2/4✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 117 times.
✗ Branch 5 not taken.
|
234 | typename DerivedL::PlainObject Qz_inv = Q.inverse(); |
| 84 | |||
| 85 |
1/2✓ Branch 2 taken 117 times.
✗ Branch 3 not taken.
|
234 | std::vector<double> chi(static_cast<size_t>(a.rows()) + 1); |
| 86 |
3/3✓ Branch 1 taken 48 times.
✓ Branch 2 taken 1355 times.
✓ Branch 3 taken 105 times.
|
3016 | for (Eigen::Index k = a.rows(); k >= 0; k--) |
| 87 | { | ||
| 88 |
1/2✓ Branch 1 taken 1391 times.
✗ Branch 2 not taken.
|
2782 | typename DerivedA::PlainObject a_fix = a; |
| 89 |
1/2✓ Branch 1 taken 1391 times.
✗ Branch 2 not taken.
|
2782 | typename DerivedA::PlainObject a_float = a; |
| 90 | |||
| 91 |
2/2✓ Branch 1 taken 18272 times.
✓ Branch 2 taken 1391 times.
|
39326 | for (Eigen::Index i = a.rows() - 1; i >= 0; i--) |
| 92 | { | ||
| 93 | 36544 | double da = 0.0; | |
| 94 |
2/2✓ Branch 1 taken 174845 times.
✓ Branch 2 taken 18272 times.
|
386234 | for (Eigen::Index j = a.rows() - 1; j >= i; j--) |
| 95 | { | ||
| 96 |
3/6✓ Branch 1 taken 174845 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 174845 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 174845 times.
✗ Branch 8 not taken.
|
349690 | da += L_LTDL_Q(j, i) * (a_float(j) - a_fix(j)); |
| 97 | } | ||
| 98 |
1/2✓ Branch 1 taken 18272 times.
✗ Branch 2 not taken.
|
36544 | a_float(i) -= da; |
| 99 | |||
| 100 |
2/2✓ Branch 0 taken 16998 times.
✓ Branch 1 taken 1274 times.
|
36544 | if (i != k - 1) // Other candidates with small norms can be found through rounding all ambiguities but one to their nearest integer |
| 101 | { | ||
| 102 |
2/4✓ Branch 1 taken 16998 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 16998 times.
✗ Branch 5 not taken.
|
33996 | a_fix(i) = std::round(a_float(i)); |
| 103 | } | ||
| 104 | else // and one ambiguity to the next-nearest integer. | ||
| 105 | { | ||
| 106 |
1/2✓ Branch 1 taken 1274 times.
✗ Branch 2 not taken.
|
2548 | auto nearest = std::round(a_float(i)); |
| 107 |
2/4✓ Branch 1 taken 1274 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 1274 times.
✗ Branch 6 not taken.
|
2548 | a_fix(i) = nearest + gcem::sgn(a_float(i) - nearest); |
| 108 | } | ||
| 109 | } | ||
| 110 | |||
| 111 |
7/14✓ Branch 1 taken 1391 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1391 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1391 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1391 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1391 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1391 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1391 times.
✗ Branch 20 not taken.
|
2782 | chi.at(static_cast<size_t>(k)) = (a - a_fix).transpose() * Qz_inv * (a - a_fix); |
| 112 | } | ||
| 113 |
1/2✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
|
234 | std::ranges::sort(chi); |
| 114 | |||
| 115 |
1/2✓ Branch 1 taken 117 times.
✗ Branch 2 not taken.
|
468 | return chi.at(static_cast<size_t>(ncand - 1)) + 1e-6; // Add a small amount to avoid boundary problems |
| 116 | 234 | } | |
| 117 | |||
| 118 | } // namespace internal | ||
| 119 | |||
| 120 | /// @brief Calculates \f$ \chi^2 \f$, the size of the ellipsoidal region | ||
| 121 | /// @param[in] a Float ambiguity vector [cycles] | ||
| 122 | /// @param[in] Q Variance/covariance matrix of the ambiguities | ||
| 123 | /// @param[in] L_LTDL_Q Lower-triangular matrix from the L^T * D * L decomposition of Q | ||
| 124 | /// @param[in] D_LTDL_Q Diagonal matrix from the L^T * D * L decomposition of Q | ||
| 125 | /// @param[in] ncand Requested number of candidates (default = 2) | ||
| 126 | /// @param[in] factor Multiplication factor for the volume of the resulting search ellipsoid (default = 1.5) | ||
| 127 | /// @return Size of the ellipsoidal region \f$ \chi^2 \f$ | ||
| 128 | template<typename DerivedA, typename DerivedQ, typename DerivedL, typename DerivedD> | ||
| 129 | 115 | double calcChi2(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q, | |
| 130 | const Eigen::MatrixBase<DerivedL>& L_LTDL_Q, const Eigen::MatrixBase<DerivedD>& D_LTDL_Q, | ||
| 131 | const Eigen::Index& ncand = 2, double factor = 1.5) | ||
| 132 | { | ||
| 133 |
0/2✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
115 | if (ncand <= a.rows() + 1) // We get one more chi candidate then the amount of ambiguities |
| 134 | { | ||
| 135 | 113 | return internal::calcChi2_bootstrap(a, Q, L_LTDL_Q, ncand); | |
| 136 | } | ||
| 137 | |||
| 138 | // Setting chi^2 over the volume is not accurate for number of candidates k less than a few (de Jonge 1996, ch. 4.10) | ||
| 139 | // so this calculation can be inaccurate | ||
| 140 | 2 | return internal::calcChi2_volume(D_LTDL_Q, ncand, factor); | |
| 141 | } | ||
| 142 | |||
| 143 | } // namespace NAV::Ambiguity | ||
| 144 |