| 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 AmbiguityResolution.hpp | ||
| 10 | /// @brief Ambiguity resolution algorithms | ||
| 11 | /// @author T. Topp (topp@ins.uni-stuttgart.de) | ||
| 12 | /// @date 2023-09-20 | ||
| 13 | |||
| 14 | #pragma once | ||
| 15 | |||
| 16 | #include <cstddef> | ||
| 17 | #include <cstdint> | ||
| 18 | #include <optional> | ||
| 19 | #include "util/Eigen.hpp" | ||
| 20 | #include "util/Logger.hpp" | ||
| 21 | #include <fmt/format.h> | ||
| 22 | #include <nlohmann/json.hpp> | ||
| 23 | using json = nlohmann::json; ///< json namespace | ||
| 24 | |||
| 25 | #include "internal/Decorrelation.hpp" | ||
| 26 | #include "internal/Search.hpp" | ||
| 27 | #include "internal/Validate.hpp" | ||
| 28 | |||
| 29 | #include "Navigation/Math/Math.hpp" | ||
| 30 | |||
| 31 | namespace NAV | ||
| 32 | { | ||
| 33 | |||
| 34 | /// Ambiguity resolution strategies | ||
| 35 | enum class AmbiguityResolutionStrategy : uint8_t | ||
| 36 | { | ||
| 37 | Continuous, ///< Estimate ambiguities every epoch | ||
| 38 | FixAndHold, ///< Do not change the ambiguity once it is fixed | ||
| 39 | COUNT, ///< Amount of items in the enum | ||
| 40 | }; | ||
| 41 | |||
| 42 | /// @brief Converts the enum to a string | ||
| 43 | /// @param[in] ambiguityResolutionStrategy Enum value to convert into text | ||
| 44 | /// @return String representation of the enum | ||
| 45 | const char* to_string(AmbiguityResolutionStrategy ambiguityResolutionStrategy); | ||
| 46 | |||
| 47 | /// @brief Ambiguity resolution algorithms and parameters | ||
| 48 | struct AmbiguityResolutionParameters | ||
| 49 | { | ||
| 50 | /// @brief Decorrelation algorithms | ||
| 51 | enum class DecorrelationAlgorithm : uint8_t | ||
| 52 | { | ||
| 53 | None, ///< Do not decorrelate | ||
| 54 | Z_Transformation, ///< Z-Transformation | ||
| 55 | COUNT, ///< Amount of items in the enum | ||
| 56 | }; | ||
| 57 | |||
| 58 | /// @brief Search algorithms | ||
| 59 | enum class SearchAlgorithm : uint8_t | ||
| 60 | { | ||
| 61 | None, ///< Disable the search | ||
| 62 | IntegerRounding, ///< Integer Rounding (IR) | ||
| 63 | IntegerBootstrapping, ///< Integer Bootstrapping (IB) | ||
| 64 | IntegerLeastSquaresSearch, ///< Integer least-squares (ILS) Search (LAMBDA) | ||
| 65 | IntegerLeastSquaresSearchAndShrink, ///< Integer least-squares (ILS) Search-and-Shrink (MLAMBDA) | ||
| 66 | COUNT, ///< Amount of items in the enum | ||
| 67 | }; | ||
| 68 | |||
| 69 | /// @brief Validation algorithms | ||
| 70 | /// | ||
| 71 | /// Define the best fitting integer solution \f$ \mathbf{\check{a}} \f$ and second best integer solution as \f$ \mathbf{\check{a}}' \f$ (\cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.6.4, eq. 23.79) | ||
| 72 | /// \anchor eq-ambRes-val \f{equation}{ \label{eq:eq-ambRes-val} | ||
| 73 | /// \begin{aligned} | ||
| 74 | /// \mathbf{\check{a}} &= \text{arg} \min_{z \in \mathbb{Z}^n} ||\mathbf{\hat{a}} - \mathbf{z}||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}} \\ | ||
| 75 | /// \mathbf{\check{a}}' &= \text{arg} \min_{z \in \mathbb{Z}^n, z \neq \mathbf{\check{a}}} ||\mathbf{\hat{a}} - \mathbf{z}||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}} | ||
| 76 | /// \end{aligned} | ||
| 77 | /// \f} | ||
| 78 | enum class ValidationAlgorithm : uint8_t | ||
| 79 | { | ||
| 80 | /// Do not validate the solution (always accept the integer solution, if one is found) | ||
| 81 | None, | ||
| 82 | /// Accept if (see \cite Verhagen2006 Verhagen 2006, eq. 31, see NAV::Ambiguity::differenceTest) | ||
| 83 | /// \anchor eq-ambRes-diff \f{equation}{ \label{eq:eq-ambRes-diff} | ||
| 84 | /// ||\mathbf{\hat{a}} - \mathbf{\check{a}}'||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}} | ||
| 85 | /// - ||\mathbf{\hat{a}} - \mathbf{\check{a}} ||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}} \ge c | ||
| 86 | /// \f} | ||
| 87 | DifferenceTest, | ||
| 88 | /// Accept if (see \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.6.4, eq. 23.78 or \cite Verhagen2006 Verhagen 2006, eq. 28, 29, see NAV::Ambiguity::ratioTest) | ||
| 89 | /// \anchor eq-ambRes-ratio \f{equation}{ \label{eq:eq-ambRes-ratio} | ||
| 90 | /// \frac{||\mathbf{\hat{a}} - \mathbf{\check{a}} ||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}}} | ||
| 91 | /// {||\mathbf{\hat{a}} - \mathbf{\check{a}}'||^2_{\mathbf{Q_{\mathbf{\hat{a}}\mathbf{\hat{a}}}}}} \le \mu | ||
| 92 | /// ,\quad 0 < \mu \le 1, \text{given by the user} | ||
| 93 | /// \f} | ||
| 94 | RatioTestCriticalValue, | ||
| 95 | /// Accept if \eqref{eq-ambRes-ratio}, but with \f$ \mu \f$ calculated from given failure rate \f$ P_F \f$ (see \cite Verhagen2013 Verhagen 2013, see NAV::Ambiguity::fixedFailureRateRatioTest) | ||
| 96 | RatioTestFailureRate, | ||
| 97 | /// Accept if (see \cite Verhagen2006 Verhagen 2006, eq. 35, see NAV::Ambiguity::projectorTest) | ||
| 98 | /// \anchor eq-ambRes-proj \f{equation}{ \label{eq:eq-ambRes-proj} | ||
| 99 | /// \left| \dfrac{(\mathbf{\check{a}}' - \mathbf{\check{a}})^T \mathbf{Q}_{\mathbf{\hat{a}}}^{-1} (\mathbf{\hat{a}} - \mathbf{\check{a}}) } | ||
| 100 | /// {|| \mathbf{\check{a}}' - \mathbf{\check{a}} ||_{\mathbf{Q}_{\mathbf{\hat{a}}}}} \right| \le \mu | ||
| 101 | /// \f} | ||
| 102 | /// It projects \f$ \mathbf{\hat{a}} - \mathbf{\check{a}} \f$ orthogonally on the direction of \f$ \mathbf{\check{a}}' - \mathbf{\check{a}} \f$, in the metric of \f$ \mathbf{Q}_{\mathbf{\hat{a}}} \f$ | ||
| 103 | ProjectorTest, | ||
| 104 | /// Amount of items in the enum | ||
| 105 | COUNT, | ||
| 106 | }; | ||
| 107 | |||
| 108 | /// Decorrelation algorithm | ||
| 109 | DecorrelationAlgorithm decorrelationAlgorithm = DecorrelationAlgorithm::Z_Transformation; | ||
| 110 | /// Search algorithm | ||
| 111 | SearchAlgorithm searchAlgorithm = SearchAlgorithm::IntegerLeastSquaresSearchAndShrink; | ||
| 112 | /// Validation with Bootstrapped success rate (Bootstrapped failure rate is an upper bound for the ILS failure rate) | ||
| 113 | bool validationBootstrappedSuccessRate = true; | ||
| 114 | /// Validation algorithm | ||
| 115 | ValidationAlgorithm validationAlgorithm = ValidationAlgorithm::RatioTestCriticalValue; | ||
| 116 | |||
| 117 | /// @brief Critical value c for the the difference test | ||
| 118 | double validationTestCriticalValueC = 10.0; | ||
| 119 | /// @brief Critical value µ for the the ratio and projector test (0, 1] | ||
| 120 | double validationTestCriticalValueMu = 1.0 / 3.0; | ||
| 121 | /// @brief Failure rate for the ratio test (used to calculate µ) | ||
| 122 | double validationRatioTestFailureRate = 0.001; | ||
| 123 | /// @brief Attempt partial fixing of ambiguities | ||
| 124 | bool partialFixing = false; | ||
| 125 | |||
| 126 | /// Possible failure rates for the look-up tables | ||
| 127 | static constexpr std::array<double, 2> allowedFailureRateValues = { { 0.001, 0.01 } }; | ||
| 128 | }; | ||
| 129 | |||
| 130 | /// @brief Converts the enum to a string | ||
| 131 | /// @param[in] decorrelationAlgorithm Enum value to convert into text | ||
| 132 | /// @return String representation of the enum | ||
| 133 | const char* to_string(AmbiguityResolutionParameters::DecorrelationAlgorithm decorrelationAlgorithm); | ||
| 134 | |||
| 135 | /// @brief Converts the enum to a string | ||
| 136 | /// @param[in] searchAlgorithm Enum value to convert into text | ||
| 137 | /// @return String representation of the enum | ||
| 138 | const char* to_string(AmbiguityResolutionParameters::SearchAlgorithm searchAlgorithm); | ||
| 139 | |||
| 140 | /// @brief Converts the enum to a string | ||
| 141 | /// @param[in] searchAlgorithm Enum value to convert into text | ||
| 142 | /// @return String representation of the enum | ||
| 143 | const char* to_string_short(AmbiguityResolutionParameters::SearchAlgorithm searchAlgorithm); | ||
| 144 | |||
| 145 | /// @brief Converts the enum to a string | ||
| 146 | /// @param[in] validationAlgorithm Enum value to convert into text | ||
| 147 | /// @return String representation of the enum | ||
| 148 | const char* to_string(AmbiguityResolutionParameters::ValidationAlgorithm validationAlgorithm); | ||
| 149 | |||
| 150 | /// @brief Shows a ComboBox to select the ambiguity resolution algorithms | ||
| 151 | /// @param[in] id Unique id for ImGui. | ||
| 152 | /// @param[in, out] params Reference to the ambiguity resolution parameter struct | ||
| 153 | /// @param[in] width GUI item width | ||
| 154 | bool GuiAmbiguityResolution(const char* id, AmbiguityResolutionParameters& params, float width = 310.0F); | ||
| 155 | |||
| 156 | /// @brief Possible failures | ||
| 157 | enum class AmbiguityResolutionFailure : uint8_t | ||
| 158 | { | ||
| 159 | None, ///< No failure | ||
| 160 | NoSearchAlgorithm, ///< No Search algorithm selected | ||
| 161 | Decorrelation, ///< Decorrelation failed | ||
| 162 | NoCandidatesFound, ///< No candidates were found with the search | ||
| 163 | ValidationFailed, ///< Validation rejected the result | ||
| 164 | }; | ||
| 165 | |||
| 166 | /// @brief Ambiguity resolution result | ||
| 167 | template<typename Scalar, int nAmb, int nReal> | ||
| 168 | struct AmbiguityResolutionResult | ||
| 169 | { | ||
| 170 | /// @brief Fixed ambiguity and their squared norm | ||
| 171 | struct FixedAmbiguity | ||
| 172 | { | ||
| 173 | /// @brief Constructor | ||
| 174 | /// @param sqnorm Squared norm | ||
| 175 | /// @param a Fixed ambiguity vector [cycles] | ||
| 176 | template<typename Derived> | ||
| 177 | 620 | FixedAmbiguity(double sqnorm, const Eigen::MatrixBase<Derived>& a) | |
| 178 | 620 | : sqnorm(sqnorm), a(a) | |
| 179 | 620 | {} | |
| 180 | |||
| 181 | double sqnorm; ///< Squared norm | ||
| 182 | Eigen::Vector<Scalar, nAmb> a; ///< Fixed ambiguity vector [cycles] | ||
| 183 | }; | ||
| 184 | |||
| 185 | AmbiguityResolutionFailure failure = AmbiguityResolutionFailure::None; ///< Failure mode | ||
| 186 | double ambiguityCriticalValueRatio{}; ///< Ambiguity Critical Value µ ∈ (0, 1] (R1/R2 ≤ µ) | ||
| 187 | |||
| 188 | size_t nFixed = 0; ///< Number of fixed ambiguities (differs from vector size in case of partial fixing) | ||
| 189 | std::vector<FixedAmbiguity> fixedAmb; ///< Sorted vector of fixed ambiguities and their norms | ||
| 190 | Eigen::Vector<Scalar, nReal> b; ///< Fixed non-integer float states (e.g. Pos, Vel, ...) | ||
| 191 | Eigen::Matrix<Scalar, nReal, nReal> Qb; ///< Fixed variance/covariance matrix of the non-integer float states | ||
| 192 | }; | ||
| 193 | |||
| 194 | /// @brief Tries resolving the ambiguities | ||
| 195 | /// @param a Float ambiguity vector [cycles] | ||
| 196 | /// @param Qa Variance/covariance matrix of the ambiguities | ||
| 197 | /// @param b Non-integer float states (e.g. Pos, Vel, ...) | ||
| 198 | /// @param Qb Variance/covariance matrix of the non-integer float states | ||
| 199 | /// @param Qab Upper right part of the variance/covariance matrix (correlation between ambiguities and other states) | ||
| 200 | /// @param Qba Lower left part of the variance/covariance matrix (correlation between ambiguities and other states) | ||
| 201 | /// @param params Ambiguity resolution algorithm and parameters | ||
| 202 | /// @param nameId NameId for debugging | ||
| 203 | /// @return The result struct if the ambiguities could be fixed and validated | ||
| 204 | template<typename DerivedA, typename DerivedQa, typename DerivedB, typename DerivedQb, typename DerivedQab, typename DerivedQba> | ||
| 205 | AmbiguityResolutionResult<typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::RowsAtCompileTime> | ||
| 206 | 411 | ResolveAmbiguities(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQa>& Qa, | |
| 207 | const Eigen::MatrixBase<DerivedB>& b, const Eigen::MatrixBase<DerivedQb>& Qb, | ||
| 208 | const Eigen::MatrixBase<DerivedQab>& Qab, const Eigen::MatrixBase<DerivedQba>& Qba, | ||
| 209 | const AmbiguityResolutionParameters& params, | ||
| 210 | [[maybe_unused]] const std::string& nameId) | ||
| 211 | { | ||
| 212 | using DecorrelationAlgorithm = AmbiguityResolutionParameters::DecorrelationAlgorithm; | ||
| 213 | using SearchAlgorithm = AmbiguityResolutionParameters::SearchAlgorithm; | ||
| 214 | using ValidationAlgorithm = AmbiguityResolutionParameters::ValidationAlgorithm; | ||
| 215 | |||
| 216 | using Eigen::seq, Eigen::last; | ||
| 217 | |||
| 218 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | AmbiguityResolutionResult<typename DerivedA::Scalar, DerivedA::RowsAtCompileTime, DerivedB::RowsAtCompileTime> result; |
| 219 | |||
| 220 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 207 times.
|
411 | if (params.searchAlgorithm == SearchAlgorithm::None) // If we do not search, do not do any work like decorrelation or validation |
| 221 | { | ||
| 222 | ✗ | result.failure = AmbiguityResolutionFailure::NoSearchAlgorithm; | |
| 223 | ✗ | return result; | |
| 224 | } | ||
| 225 | |||
| 226 | LOG_DATA("{}: Qa = \n{}", nameId, Eigen::MatrixXd(Qa)); | ||
| 227 | LOG_DATA("{}: a = {}", nameId, a.transpose()); | ||
| 228 | LOG_DATA("{}: Qb = \n{}", nameId, Eigen::MatrixXd(Qb)); | ||
| 229 | LOG_DATA("{}: b = {}", nameId, b.transpose()); | ||
| 230 | LOG_DATA("{}: Qab = \n{}", nameId, Eigen::MatrixXd(Qab)); | ||
| 231 | LOG_DATA("{}: Qba = \n{}", nameId, Eigen::MatrixXd(Qba)); | ||
| 232 | |||
| 233 | // Avoid integer overflows by reducing the ambiguities between -1.0 and +1.0 | ||
| 234 |
3/6✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 207 times.
✗ Branch 8 not taken.
|
411 | Eigen::VectorXd ambIntPart = a.template cast<int>().template cast<double>(); |
| 235 |
2/4✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
|
411 | Eigen::VectorXd ambFracPart = a - ambIntPart; |
| 236 | LOG_DATA("{}: ambFracPart = {}", nameId, ambFracPart.transpose()); | ||
| 237 | |||
| 238 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | typename DerivedQa::PlainObject Qz; // Decorrelated ambiguity covariance matrix |
| 239 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | typename DerivedQa::PlainObject Z; // Decorrelation transformation matrix |
| 240 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | typename DerivedQa::PlainObject L; // Lower-triangular matrix from the L^T * D * L decomposition of Q |
| 241 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | Eigen::VectorXd D; // Diagonal entries from the L^T * D * L decomposition of Q |
| 242 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | Eigen::VectorXd z; // Decorrelated float ambiguity vector [cycles] |
| 243 | |||
| 244 |
1/3✓ Branch 0 taken 207 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
|
411 | switch (params.decorrelationAlgorithm) |
| 245 | { | ||
| 246 | 411 | case DecorrelationAlgorithm::Z_Transformation: | |
| 247 |
3/6✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 207 times.
✗ Branch 8 not taken.
|
822 | if (auto decorrelated_ztrafo = Ambiguity::decorrelate_ztrafo(ambFracPart, Qa)) |
| 248 | { | ||
| 249 |
1/2✓ Branch 3 taken 207 times.
✗ Branch 4 not taken.
|
411 | std::tie(Qz, Z, L, D, z) = *decorrelated_ztrafo; |
| 250 | } | ||
| 251 | else | ||
| 252 | { | ||
| 253 | ✗ | LOG_DEBUG("{}: Decorrelation failed", nameId); | |
| 254 | ✗ | result.failure = AmbiguityResolutionFailure::Decorrelation; | |
| 255 | ✗ | return result; | |
| 256 | } | ||
| 257 | 411 | break; | |
| 258 | ✗ | case DecorrelationAlgorithm::None: | |
| 259 | case DecorrelationAlgorithm::COUNT: | ||
| 260 | ✗ | Qz = Qa; | |
| 261 | ✗ | z = a; | |
| 262 | ✗ | if (auto ltdl_decomp = math::LtDLdecomp_choleskyFact(Qa)) | |
| 263 | { | ||
| 264 | ✗ | std::tie(L, D) = *ltdl_decomp; | |
| 265 | } | ||
| 266 | else | ||
| 267 | { | ||
| 268 | ✗ | LOG_DEBUG("{}: Decorrelation failed", nameId); | |
| 269 | } | ||
| 270 | ✗ | break; | |
| 271 | } | ||
| 272 | LOG_DATA("{}: z = {}", nameId, z.transpose()); | ||
| 273 | LOG_DATA("{}: Qz = \n{}", nameId, Eigen::MatrixXd(Qz)); | ||
| 274 | LOG_DATA("{}: Z = \n{}", nameId, Z); | ||
| 275 | LOG_DATA("{}: L = \n{}", nameId, L); | ||
| 276 | LOG_DATA("{}: D = {}", nameId, D.transpose()); | ||
| 277 | |||
| 278 | 411 | auto n = z.rows(); | |
| 279 | 411 | int k = 0; | |
| 280 | // if (params.partialFixing) // Partial fixing is done by giving a subset to this function. So not relevant here | ||
| 281 | // { | ||
| 282 | // k = -1; | ||
| 283 | // double P0 = 0.995; | ||
| 284 | // double Ps = 0.0; | ||
| 285 | // do // Decorrelated ambiguities are sorted by standard deviation. Largest entry in D is first entry. So remove from top | ||
| 286 | // { | ||
| 287 | // k++; | ||
| 288 | // Ps = Ambiguity::successRateBootstrapping(D(seq(k, last))); | ||
| 289 | // LOG_DATA("{}: Ps(k = {}) = {}", nameId, k, Ps); | ||
| 290 | // } while (Ps < P0 && k < n - 1); | ||
| 291 | // if (Ps < P0) { return {}; } | ||
| 292 | // } | ||
| 293 | if (k != 0) | ||
| 294 | { | ||
| 295 | LOG_TRACE("{}: Doing partial ambiguity fixing for only {} of {} ambiguities", nameId, n - k, n); | ||
| 296 | LOG_DATA("{}: z = {}", nameId, z(seq(k, last)).transpose()); | ||
| 297 | LOG_DATA("{}: Qz = \n{}", nameId, Eigen::MatrixXd(Qz(seq(k, last), seq(k, last)))); | ||
| 298 | LOG_DATA("{}: L = \n{}", nameId, L(seq(k, last), seq(k, last))); | ||
| 299 | LOG_DATA("{}: D = {}", nameId, D(seq(k, last)).transpose()); | ||
| 300 | } | ||
| 301 | |||
| 302 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | Eigen::MatrixXd cands; |
| 303 |
1/2✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
|
411 | Eigen::VectorXd sqnorm; |
| 304 | 411 | int numCandidates = 2; | |
| 305 | |||
| 306 |
2/6✗ Branch 0 not taken.
✗ Branch 1 not taken.
✓ Branch 2 taken 105 times.
✓ Branch 3 taken 102 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
411 | switch (params.searchAlgorithm) |
| 307 | { | ||
| 308 | ✗ | case SearchAlgorithm::IntegerRounding: | |
| 309 | ✗ | cands = Ambiguity::integerSearchRounding(z(seq(k, last))); | |
| 310 | ✗ | sqnorm = Eigen::VectorXd::Constant(1, 1.0); | |
| 311 | ✗ | break; | |
| 312 | ✗ | case SearchAlgorithm::IntegerBootstrapping: | |
| 313 | ✗ | cands = Ambiguity::integerSearchBootstrapping(z(seq(k, last)), Qz(seq(k, last), seq(k, last))); | |
| 314 | ✗ | sqnorm = Eigen::VectorXd::Constant(1, 1.0); | |
| 315 | ✗ | break; | |
| 316 | 207 | case SearchAlgorithm::IntegerLeastSquaresSearch: | |
| 317 |
6/12✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 105 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 105 times.
✗ Branch 17 not taken.
|
207 | std::tie(cands, sqnorm) = Ambiguity::integerLeastSquaresSearch(z(seq(k, last)), Qz(seq(k, last), seq(k, last)), |
| 318 |
5/10✓ Branch 1 taken 105 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 105 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 105 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 105 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 105 times.
✗ Branch 14 not taken.
|
207 | L(seq(k, last), seq(k, last)), D(seq(k, last)), numCandidates); |
| 319 | 207 | break; | |
| 320 | 204 | case SearchAlgorithm::IntegerLeastSquaresSearchAndShrink: | |
| 321 |
8/16✓ Branch 1 taken 102 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 102 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 102 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 102 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 102 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 102 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 102 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 102 times.
✗ Branch 23 not taken.
|
204 | std::tie(cands, sqnorm) = Ambiguity::integerLeastSquaresSearchAndShrink(z(seq(k, last)), L(seq(k, last), seq(k, last)), D(seq(k, last)), numCandidates); |
| 322 | 204 | break; | |
| 323 | ✗ | case SearchAlgorithm::None: | |
| 324 | case SearchAlgorithm::COUNT: | ||
| 325 | ✗ | break; | |
| 326 | } | ||
| 327 | |||
| 328 | #if LOG_LEVEL <= LOG_LEVEL_DATA | ||
| 329 | if (k != 0) | ||
| 330 | { | ||
| 331 | Eigen::MatrixXd print(cands.cols(), cands.rows() + 1); | ||
| 332 | for (Eigen::Index i = 0; i < cands.cols(); i++) | ||
| 333 | { | ||
| 334 | print(i, 0) = sqnorm(i); | ||
| 335 | print(i, Eigen::seq(1, Eigen::last)) = cands.col(i).transpose(); | ||
| 336 | } | ||
| 337 | LOG_DATA("{}: sqnorm, cand (1 candidate each row)\n{}", nameId, print); | ||
| 338 | } | ||
| 339 | #endif | ||
| 340 | |||
| 341 |
1/2✗ Branch 1 not taken.
✓ Branch 2 taken 207 times.
|
411 | if (cands.cols() == 0) |
| 342 | { | ||
| 343 | LOG_DATA("{}: No candidates found ", nameId); | ||
| 344 | ✗ | result.failure = AmbiguityResolutionFailure::NoCandidatesFound; | |
| 345 | ✗ | return result; | |
| 346 | } | ||
| 347 | |||
| 348 | // Partial fixing is done by giving a subset to this function. So not relevant here | ||
| 349 | // if (params.partialFixing) // Adjust first k-1 ambiguities based on correlation with the fixed ambiguities | ||
| 350 | // { | ||
| 351 | // LOG_DATA("{}: Adjusting first k-1 ambiguities based on correlation with the fixed ambiguities", nameId); | ||
| 352 | // Eigen::MatrixXd zfixed = Eigen::MatrixXd::Zero(n, numCandidates); | ||
| 353 | // Eigen::MatrixXd QP = Qz(seq(0, k - 1), seq(k, last)) * Qz(seq(k, last), seq(k, last)).inverse(); | ||
| 354 | // for (int i = 0; i < numCandidates; i++) | ||
| 355 | // { | ||
| 356 | // zfixed(seq(0, k - 1), i) = z(seq(0, k - 1)) - QP * (z(seq(k, last)) - cands(Eigen::all, i)); | ||
| 357 | // } | ||
| 358 | // zfixed(seq(k, last), Eigen::all) = cands; | ||
| 359 | // cands = zfixed; | ||
| 360 | |||
| 361 | // #if LOG_LEVEL <= LOG_LEVEL_DATA | ||
| 362 | // { | ||
| 363 | // Eigen::MatrixXd print(cands.cols(), cands.rows() + 1); | ||
| 364 | // for (Eigen::Index i = 0; i < cands.cols(); i++) | ||
| 365 | // { | ||
| 366 | // print(i, 0) = sqnorm(i); | ||
| 367 | // print(i, Eigen::seq(1, Eigen::last)) = cands.col(i).transpose(); | ||
| 368 | // } | ||
| 369 | // LOG_DATA("{}: sqnorm, cand (1 candidate each row)\n{}", nameId, print); | ||
| 370 | // } | ||
| 371 | // #endif | ||
| 372 | // } | ||
| 373 | |||
| 374 |
2/2✓ Branch 1 taken 313 times.
✓ Branch 2 taken 207 times.
|
1031 | for (Eigen::Index i = 0; i < cands.cols(); i++) |
| 375 | { | ||
| 376 |
1/2✓ Branch 0 taken 313 times.
✗ Branch 1 not taken.
|
620 | if (params.decorrelationAlgorithm != DecorrelationAlgorithm::None) |
| 377 | { | ||
| 378 | // Back transformation | ||
| 379 |
6/12✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 313 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 313 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 313 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 313 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 313 times.
✗ Branch 17 not taken.
|
620 | cands.col(i) = Z.transpose().inverse() * cands.col(i); |
| 380 | |||
| 381 | // Z is an integer preserving transformation | ||
| 382 | // If cands is only partially fixed, then the output wont have integers at all | ||
| 383 | } | ||
| 384 | // Reapply the integer part from before | ||
| 385 |
2/4✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 313 times.
✗ Branch 5 not taken.
|
620 | cands.col(i) += ambIntPart; |
| 386 | } | ||
| 387 | |||
| 388 | #if LOG_LEVEL <= LOG_LEVEL_DATA | ||
| 389 | { | ||
| 390 | Eigen::MatrixXd print(cands.cols(), cands.rows() + 1); | ||
| 391 | for (Eigen::Index i = 0; i < cands.cols(); i++) | ||
| 392 | { | ||
| 393 | print(i, 0) = sqnorm(i); | ||
| 394 | print(i, Eigen::seq(1, Eigen::last)) = cands.col(i).transpose(); | ||
| 395 | } | ||
| 396 | LOG_DATA("{}: Back transformed results - sqnorm, cand (1 candidate each row){}\n{}", nameId, | ||
| 397 | k != 0 ? " (not integer, because only partial fix)" : "", print); | ||
| 398 | } | ||
| 399 | #endif | ||
| 400 | |||
| 401 |
2/2✓ Branch 1 taken 106 times.
✓ Branch 2 taken 101 times.
|
411 | if (cands.cols() > 1) // If we found only one candidate, the second one was very unlikely. Therefore we can accept this one without testing |
| 402 | { | ||
| 403 |
2/4✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106 times.
✗ Branch 5 not taken.
|
209 | result.ambiguityCriticalValueRatio = sqnorm(0) / sqnorm(1); |
| 404 |
1/6✗ Branch 0 not taken.
✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
|
209 | switch (params.validationAlgorithm) |
| 405 | { | ||
| 406 | ✗ | case ValidationAlgorithm::DifferenceTest: | |
| 407 | ✗ | if (!Ambiguity::differenceTest(sqnorm(0), sqnorm(1), params.validationTestCriticalValueC)) | |
| 408 | { | ||
| 409 | ✗ | result.failure = AmbiguityResolutionFailure::ValidationFailed; | |
| 410 | ✗ | return result; | |
| 411 | } | ||
| 412 | ✗ | break; | |
| 413 | 209 | case ValidationAlgorithm::RatioTestCriticalValue: | |
| 414 |
4/8✓ Branch 1 taken 106 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 106 times.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✓ Branch 10 taken 106 times.
|
209 | if (!Ambiguity::ratioTest(sqnorm(0), sqnorm(1), params.validationTestCriticalValueMu)) |
| 415 | { | ||
| 416 | ✗ | result.failure = AmbiguityResolutionFailure::ValidationFailed; | |
| 417 | ✗ | return result; | |
| 418 | } | ||
| 419 | 209 | break; | |
| 420 | ✗ | case ValidationAlgorithm::RatioTestFailureRate: | |
| 421 | ✗ | if (!Ambiguity::fixedFailureRateRatioTest(params.validationRatioTestFailureRate, sqnorm(0), sqnorm(1), | |
| 422 | ✗ | static_cast<size_t>(n - k), D, params.validationBootstrappedSuccessRate)) | |
| 423 | { | ||
| 424 | ✗ | result.failure = AmbiguityResolutionFailure::ValidationFailed; | |
| 425 | ✗ | return result; | |
| 426 | } | ||
| 427 | ✗ | break; | |
| 428 | ✗ | case ValidationAlgorithm::ProjectorTest: | |
| 429 | ✗ | if (!Ambiguity::projectorTest(cands.col(0), cands.col(1), a(seq(k, last)), Qa(seq(k, last), seq(k, last)), | |
| 430 | ✗ | params.validationTestCriticalValueMu)) | |
| 431 | { | ||
| 432 | ✗ | result.failure = AmbiguityResolutionFailure::ValidationFailed; | |
| 433 | ✗ | return result; | |
| 434 | } | ||
| 435 | ✗ | break; | |
| 436 | ✗ | case ValidationAlgorithm::None: | |
| 437 | case ValidationAlgorithm::COUNT: | ||
| 438 | ✗ | break; | |
| 439 | } | ||
| 440 | } | ||
| 441 | |||
| 442 |
7/14✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 207 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 207 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 207 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 207 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 207 times.
✗ Branch 20 not taken.
|
411 | result.b = b - Qba * Qa.inverse() * (a - cands.col(0)); |
| 443 |
5/10✓ Branch 1 taken 207 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 207 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 207 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 207 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 207 times.
✗ Branch 14 not taken.
|
411 | result.Qb = Qb - Qba * Qa.inverse() * Qab; |
| 444 | |||
| 445 |
2/2✓ Branch 1 taken 313 times.
✓ Branch 2 taken 207 times.
|
1031 | for (Eigen::Index i = 0; i < cands.cols(); i++) |
| 446 | { | ||
| 447 |
3/6✓ Branch 1 taken 313 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 313 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 313 times.
✗ Branch 8 not taken.
|
620 | result.fixedAmb.emplace_back(sqnorm(i), cands.col(i)); |
| 448 | } | ||
| 449 | 411 | result.nFixed = static_cast<size_t>(n - k); | |
| 450 | |||
| 451 | 411 | return result; | |
| 452 | 411 | } | |
| 453 | |||
| 454 | /// @brief Converts the provided object into json | ||
| 455 | /// @param[out] j Json object which gets filled with the info | ||
| 456 | /// @param[in] obj Object to convert into json | ||
| 457 | void to_json(json& j, const AmbiguityResolutionParameters& obj); | ||
| 458 | /// @brief Converts the provided json object into a node object | ||
| 459 | /// @param[in] j Json object with the needed values | ||
| 460 | /// @param[out] obj Object to fill from the json | ||
| 461 | void from_json(const json& j, AmbiguityResolutionParameters& obj); | ||
| 462 | |||
| 463 | } // namespace NAV | ||
| 464 |