| 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 Validate.hpp | ||
| 10 | /// @brief Ambiguity resolution validation algorithms | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-10-06 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include "Navigation/Math/Math.hpp" | ||
| 17 | #include "util/Assert.h" | ||
| 18 | #include "util/Eigen.hpp" | ||
| 19 | #include "util/Logger.hpp" | ||
| 20 | |||
| 21 | namespace NAV::Ambiguity | ||
| 22 | { | ||
| 23 | |||
| 24 | /// @brief Calculates the bootstrapped success rate | ||
| 25 | /// @param[in] D_LTDL_Q Diagonal entries from the L^T * D * L decomposition of Q | ||
| 26 | /// @return The bootstrapped success rate | ||
| 27 | /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.2.4, eq. 23.28 | ||
| 28 | template<typename Derived> | ||
| 29 | ✗ | double successRateBootstrapping(const Eigen::MatrixBase<Derived>& D_LTDL_Q) | |
| 30 | { | ||
| 31 | ✗ | INS_ASSERT_USER_ERROR(D_LTDL_Q.cols() == 1, "Parameter 'D_LTDL_Q' has to be a vector"); | |
| 32 | |||
| 33 | ✗ | double prod = 1.0; | |
| 34 | ✗ | for (Eigen::Index i = 0; i < D_LTDL_Q.rows(); i++) | |
| 35 | { | ||
| 36 | ✗ | prod *= 2.0 * math::normalCDF(0.5 / std::sqrt(D_LTDL_Q(i))) - 1; | |
| 37 | } | ||
| 38 | |||
| 39 | ✗ | return prod; | |
| 40 | } | ||
| 41 | |||
| 42 | /// @brief Difference test for the ambiguity resolution see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::DifferenceTest | ||
| 43 | /// @param[in] aFix1 Best integer ambiguities solution | ||
| 44 | /// @param[in] aFix2 Second best integer ambiguities solution | ||
| 45 | /// @param[in] aFloat Float ambiguities | ||
| 46 | /// @param[in] Qa Covariance matrix of the ambiguities | ||
| 47 | /// @param[in] criticalValue Critical value to check against (empirically determined value) | ||
| 48 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 49 | /// @note See \cite Verhagen2006 Verhagen 2006, eq. 31 | ||
| 50 | template<typename DerivedAFix1, typename DerivedAFix2, typename DerivedAFloat, typename DerivedQ> | ||
| 51 | 1 | bool differenceTest(const Eigen::MatrixBase<DerivedAFix1>& aFix1, const Eigen::MatrixBase<DerivedAFix2>& aFix2, | |
| 52 | const Eigen::MatrixBase<DerivedAFloat>& aFloat, const Eigen::MatrixBase<DerivedQ>& Qa, double criticalValue) | ||
| 53 | { | ||
| 54 | static_assert(DerivedAFix1::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix1::ColsAtCompileTime == 1); | ||
| 55 | static_assert(DerivedAFix2::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix2::ColsAtCompileTime == 1); | ||
| 56 | static_assert(DerivedAFloat::ColsAtCompileTime == Eigen::Dynamic || DerivedAFloat::ColsAtCompileTime == 1); | ||
| 57 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.cols() == 1, "Parameter 'aFix1' has to be a vector"); |
| 58 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix2.cols() == 1, "Parameter 'aFix2' has to be a vector"); |
| 59 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFloat.cols() == 1, "Parameter 'aFloat' has to be a vector"); |
| 60 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFix2.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 61 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFloat.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 62 | |||
| 63 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | return math::squaredNormVectorMatrix(aFloat - aFix2, Qa) - math::squaredNormVectorMatrix(aFloat - aFix1, Qa) >= criticalValue; |
| 64 | } | ||
| 65 | |||
| 66 | /// @brief Difference test for the ambiguity resolution see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::DifferenceTest | ||
| 67 | /// @param[in] sqNormFix1 Squared norm of the best integer ambiguities solution | ||
| 68 | /// @param[in] sqNormFix2 Squared norm of the second best integer ambiguities solution | ||
| 69 | /// @param[in] criticalValue Critical value to check against (empirically determined value) | ||
| 70 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 71 | /// @note See \cite Verhagen2006 Verhagen 2006, eq. 31 | ||
| 72 | bool differenceTest(double sqNormFix1, double sqNormFix2, double criticalValue); | ||
| 73 | |||
| 74 | /// @brief Ratio test for the ambiguity resolution see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::RatioTestCriticalValue | ||
| 75 | /// @param[in] aFix1 Best integer ambiguities solution | ||
| 76 | /// @param[in] aFix2 Second best integer ambiguities solution | ||
| 77 | /// @param[in] aFloat Float ambiguities | ||
| 78 | /// @param[in] Qa Covariance matrix of the ambiguities | ||
| 79 | /// @param[in] mu Critical value to check against, with range (0, 1] | ||
| 80 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 81 | /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.6.4, eq. 23.78 or \cite Verhagen2006 Verhagen 2006, eq. 28, 29 | ||
| 82 | template<typename DerivedAFix1, typename DerivedAFix2, typename DerivedAFloat, typename DerivedQ> | ||
| 83 | 1 | bool ratioTest(const Eigen::MatrixBase<DerivedAFix1>& aFix1, const Eigen::MatrixBase<DerivedAFix2>& aFix2, | |
| 84 | const Eigen::MatrixBase<DerivedAFloat>& aFloat, const Eigen::MatrixBase<DerivedQ>& Qa, double mu) | ||
| 85 | { | ||
| 86 | static_assert(DerivedAFix1::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix1::ColsAtCompileTime == 1); | ||
| 87 | static_assert(DerivedAFix2::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix2::ColsAtCompileTime == 1); | ||
| 88 | static_assert(DerivedAFloat::ColsAtCompileTime == Eigen::Dynamic || DerivedAFloat::ColsAtCompileTime == 1); | ||
| 89 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.cols() == 1, "Parameter 'aFix1' has to be a vector"); |
| 90 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix2.cols() == 1, "Parameter 'aFix2' has to be a vector"); |
| 91 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFloat.cols() == 1, "Parameter 'aFloat' has to be a vector"); |
| 92 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFix2.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 93 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFloat.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 94 | |||
| 95 |
4/8✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
|
1 | return math::squaredNormVectorMatrix(aFloat - aFix1, Qa) / math::squaredNormVectorMatrix(aFloat - aFix2, Qa) <= mu; |
| 96 | } | ||
| 97 | |||
| 98 | /// @brief Ratio test for the ambiguity resolution see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::RatioTestCriticalValue | ||
| 99 | /// @param[in] sqNormFix1 Squared norm of the best integer ambiguities solution | ||
| 100 | /// @param[in] sqNormFix2 Squared norm of the second best integer ambiguities solution | ||
| 101 | /// @param[in] mu Critical value to check against, with range (0, 1] | ||
| 102 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 103 | /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.6.4, eq. 23.78 or \cite Verhagen2006 Verhagen 2006, eq. 28, 29 | ||
| 104 | bool ratioTest(double sqNormFix1, double sqNormFix2, double mu); | ||
| 105 | |||
| 106 | /// @brief Look-up table for the critical value µ, depending on failure rate Pf_ILS | ||
| 107 | /// @param Pf Failure probability to not be higher | ||
| 108 | /// @param Pf_ILS Upper bound of the failure probability | ||
| 109 | /// @param nAmb Amount of ambiguities | ||
| 110 | /// @return Critical value µ | ||
| 111 | double criticalValueFailureRateLookup(double Pf, double Pf_ILS, size_t nAmb); | ||
| 112 | |||
| 113 | /// @brief Ratio test with a fixed-failure rate for ambiguity resolution, see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::RatioTestFailureRate | ||
| 114 | /// @param[in] Pf Fixed-failure rate | ||
| 115 | /// @param[in] aFix1 Best integer ambiguities solution | ||
| 116 | /// @param[in] aFix2 Second best integer ambiguities solution | ||
| 117 | /// @param[in] aFloat Float ambiguities | ||
| 118 | /// @param[in] Qa Covariance matrix of the ambiguities | ||
| 119 | /// @param[in] D_LTDL_Q Diagonal entries from the L^T * D * L decomposition of Q | ||
| 120 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 121 | /// @note See \cite Verhagen2013 Verhagen 2013 | ||
| 122 | template<typename DerivedAFix1, typename DerivedAFix2, typename DerivedAFloat, typename DerivedQ, typename DerivedD> | ||
| 123 | bool fixedFailureRateRatioTest(double Pf, const Eigen::MatrixBase<DerivedAFix1>& aFix1, const Eigen::MatrixBase<DerivedAFix2>& aFix2, | ||
| 124 | const Eigen::MatrixBase<DerivedAFloat>& aFloat, const Eigen::MatrixBase<DerivedQ>& Qa, | ||
| 125 | const Eigen::MatrixBase<DerivedD>& D_LTDL_Q) | ||
| 126 | { | ||
| 127 | // Bootstrapped failure rate is an upper bound for the ILS failure rate | ||
| 128 | double Pf_ILS = 1.0 - successRateBootstrapping(D_LTDL_Q); | ||
| 129 | LOG_DATA("Pf_ILS = {}", Pf_ILS); | ||
| 130 | |||
| 131 | // If Pf_ILS <= Pf, set µ equal to 1.0 which always accept the solution in the ratio test. So we can directly return true | ||
| 132 | if (Pf_ILS <= Pf) { return true; } | ||
| 133 | |||
| 134 | double mu = criticalValueFailureRateLookup(Pf, Pf_ILS, static_cast<size_t>(aFloat.rows())); | ||
| 135 | |||
| 136 | return ratioTest(aFix1, aFix2, aFloat, Qa, mu); | ||
| 137 | } | ||
| 138 | |||
| 139 | /// @brief Ratio test with a fixed-failure rate for ambiguity resolution, see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::RatioTestFailureRate | ||
| 140 | /// @param[in] Pf Fixed-failure rate | ||
| 141 | /// @param[in] sqNormFix1 Squared norm of the best integer ambiguities solution | ||
| 142 | /// @param[in] sqNormFix2 Squared norm of the second best integer ambiguities solution | ||
| 143 | /// @param[in] nAmb Number of ambiguities | ||
| 144 | /// @param[in] D_LTDL_Q Diagonal entries from the L^T * D * L decomposition of Q | ||
| 145 | /// @param[in] validateBootstrappedSuccessRate Whether to check the bootstrapped success rate before doing the ratio test | ||
| 146 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 147 | /// @note See \cite Verhagen2013 Verhagen 2013 | ||
| 148 | template<typename DerivedD> | ||
| 149 | ✗ | bool fixedFailureRateRatioTest(double Pf, double sqNormFix1, double sqNormFix2, size_t nAmb, const Eigen::MatrixBase<DerivedD>& D_LTDL_Q, bool validateBootstrappedSuccessRate = true) | |
| 150 | { | ||
| 151 | // Bootstrapped failure rate is an upper bound for the ILS failure rate | ||
| 152 | ✗ | double Pf_ILS = 1.0 - successRateBootstrapping(D_LTDL_Q); | |
| 153 | LOG_DATA("Pf_ILS = {}", Pf_ILS); | ||
| 154 | |||
| 155 | // If Pf_ILS <= Pf, set µ equal to 1.0 which always accept the solution in the ratio test. So we can directly return true | ||
| 156 | ✗ | if (validateBootstrappedSuccessRate && Pf_ILS <= Pf) { return true; } | |
| 157 | |||
| 158 | ✗ | double mu = criticalValueFailureRateLookup(Pf, Pf_ILS, nAmb); | |
| 159 | |||
| 160 | ✗ | return ratioTest(sqNormFix1, sqNormFix2, mu); | |
| 161 | } | ||
| 162 | |||
| 163 | /// @brief Projector test for the ambiguity resolution see NAV::AmbiguityResolutionParameters::ValidationAlgorithm::ProjectorTest | ||
| 164 | /// @param[in] aFix1 Best integer ambiguities solution | ||
| 165 | /// @param[in] aFix2 Second best integer ambiguities solution | ||
| 166 | /// @param[in] aFloat Float ambiguities | ||
| 167 | /// @param[in] Qa Covariance matrix of the ambiguities | ||
| 168 | /// @param[in] mu Critical value to check against, with range (0, 1] | ||
| 169 | /// @return True if the test passes and the best integer solution should be accepted | ||
| 170 | /// @note See \cite Verhagen2006 Verhagen 2006, eq. 35 | ||
| 171 | template<typename DerivedAFix1, typename DerivedAFix2, typename DerivedAFloat, typename DerivedQ> | ||
| 172 | 1 | bool projectorTest(const Eigen::MatrixBase<DerivedAFix1>& aFix1, const Eigen::MatrixBase<DerivedAFix2>& aFix2, | |
| 173 | const Eigen::MatrixBase<DerivedAFloat>& aFloat, const Eigen::MatrixBase<DerivedQ>& Qa, double mu) | ||
| 174 | { | ||
| 175 | static_assert(DerivedAFix1::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix1::ColsAtCompileTime == 1); | ||
| 176 | static_assert(DerivedAFix2::ColsAtCompileTime == Eigen::Dynamic || DerivedAFix2::ColsAtCompileTime == 1); | ||
| 177 | static_assert(DerivedAFloat::ColsAtCompileTime == Eigen::Dynamic || DerivedAFloat::ColsAtCompileTime == 1); | ||
| 178 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.cols() == 1, "Parameter 'aFix1' has to be a vector"); |
| 179 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix2.cols() == 1, "Parameter 'aFix2' has to be a vector"); |
| 180 |
1/2✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFloat.cols() == 1, "Parameter 'aFloat' has to be a vector"); |
| 181 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFix2.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 182 |
1/2✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
|
1 | INS_ASSERT_USER_ERROR(aFix1.rows() == aFloat.rows(), "Parameter 'aFix1' needs to have the same size as 'aFix2' and 'aFloat'"); |
| 183 | |||
| 184 |
7/14✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1 times.
✗ Branch 20 not taken.
|
1 | double proj = (aFix2 - aFix1).transpose() * Qa.inverse() * (aFloat - aFix1); |
| 185 |
2/4✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
|
1 | return std::abs(proj / math::squaredNormVectorMatrix(aFix2 - aFix1, Qa)) <= mu; |
| 186 | } | ||
| 187 | |||
| 188 | } // namespace NAV::Ambiguity | ||
| 189 |