INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Ambiguity/internal/Search.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 146 147 99.3%
Functions: 8 16 50.0%
Branches: 286 520 55.0%
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 Search.hpp
10 /// @brief Ambiguity Search algorithms
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-09-13
13 /// @note Algorithm mostly taken from \cite deJonge1996 de Jonge 1996.
14 /// Matlab code from https://www.tudelft.nl/citg/over-faculteit/afdelingen/geoscience-remote-sensing/research/lambda/lambda was used to clarify points in literature.
15
16 #pragma once
17
18 #include <Eigen/Core>
19 #include <cmath>
20 #include "Navigation/Math/Sort.hpp"
21 #include <Eigen/src/Core/ArithmeticSequence.h>
22 #include <gcem.hpp>
23 #include "EllipsoidalRegion.hpp"
24 #include <limits>
25
26 namespace NAV::Ambiguity
27 {
28
29 /// @brief Searches for the integer ambiguities by integer rounding
30 /// @param[in] a Float ambiguity vector [cycles]
31 /// @return Integer ambiguity vector
32 /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.2.2
33 template<typename Derived>
34 2 typename Derived::PlainObject integerSearchRounding(const Eigen::MatrixBase<Derived>& a)
35 {
36 static_assert(Derived::ColsAtCompileTime == Eigen::Dynamic || Derived::ColsAtCompileTime == 1);
37
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 2 INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector");
38
39
2/4
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 5 return a.unaryExpr([](const double& x) { return std::round(x); });
40 }
41
42 /// @brief Searches for the integer ambiguities by integer bootstrapping
43 /// @param[in] a Decorrelated float ambiguity vector [cycles]
44 /// @param[in] Qa Variance/covariance matrix of the ambiguities
45 /// @return Integer ambiguity vector
46 /// @note See \cite SpringerHandbookGNSS2017 Springer Handbook GNSS, ch. 23.2.3
47 template<typename DerivedA, typename DerivedQ>
48 4 typename DerivedA::PlainObject integerSearchBootstrapping(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Qa)
49 {
50 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
51
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector");
52
53
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 typename DerivedA::PlainObject a_fix = a;
54
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 typename DerivedA::PlainObject a_float = a;
55
56
2/2
✓ Branch 1 taken 6 times.
✓ Branch 2 taken 2 times.
 16 for (Eigen::Index n = 0; n < a.rows(); n++)
57 {
58
2/2
✓ Branch 0 taken 6 times.
✓ Branch 1 taken 6 times.
 24 for (Eigen::Index j = 0; j < n; j++)
59 {
60
5/10
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
 12 a_float(n) -= Qa(n, j) / Qa(j, j) * (a_float(j) - a_fix(j));
61 }
62
63
2/4
✓ Branch 1 taken 6 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
 12 a_fix(n) = std::round(a_float(n));
64 }
65
66 8 return a_fix;
67 ✗ }
68
69 /// @brief Performs an integer least-squares to search for integer candidates for the ambiguities
70 /// @param[in] a Decorrelated float ambiguity vector [cycles]
71 /// @param[in] Q Variance/covariance matrix of the ambiguities
72 /// @param[in] L_LTDL_Q Lower-triangular matrix from the L^T * D * L decomposition of Q
73 /// @param[in] D_LTDL_Q Diagonal entries from the L^T * D * L decomposition of Q
74 /// @param[in] numCandidates Requested number of candidates (default = 2)
75 /// @return Pair of: First a list of integer candidates (last column is the most likely), Vector with squared norm of the integer candidates
76 /// @note See \cite deJonge1996 de Jonge 1996, Algorithm FI71
77 template<typename DerivedA, typename DerivedQ, typename DerivedL, typename DerivedD>
78 std::pair<Eigen::MatrixXd, Eigen::VectorXd>
79 220 integerLeastSquaresSearch(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q,
80 const Eigen::MatrixBase<DerivedL>& L_LTDL_Q, const Eigen::MatrixBase<DerivedD>& D_LTDL_Q,
81 const Eigen::Index& numCandidates = 2)
82 {
83 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
84
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector");
85 static_assert(DerivedD::ColsAtCompileTime == Eigen::Dynamic || DerivedD::ColsAtCompileTime == 1);
86
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 INS_ASSERT_USER_ERROR(D_LTDL_Q.cols() == 1, "Parameter 'D_LTDL_Q' has to be a vector");
87
1/2
✓ Branch 2 taken 110 times.
✗ Branch 3 not taken.
 220 INS_ASSERT_USER_ERROR(a.rows() == D_LTDL_Q.rows(), "Parameter 'a' and 'D_LTDL_Q' must have same dimension");
88
89 using Eigen::seq;
90
91 220 auto n = a.rows(); // Amount of ambiguities
92
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 110 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 220 if (n == 1) { return {}; }
93
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 double chi2 = calcChi2(a, Q, L_LTDL_Q, D_LTDL_Q, numCandidates, 1.0); // Size of the ellipsoidal region
94 LOG_DATA("chi2 = {}", chi2);
95
96 // Q^-1 = L * D * L^T
97
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 typename DerivedL::PlainObject L = L_LTDL_Q.inverse();
98
4/8
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 110 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 110 times.
✗ Branch 11 not taken.
 220 typename DerivedD::PlainObject D = (1.0 / D_LTDL_Q.array()).matrix();
99 LOG_DATA("L =\n{}", L);
100 LOG_DATA("D = {}", D.transpose());
101
102
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd right = Eigen::VectorXd::Zero(n + 1);
103
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 right(n) = chi2;
104
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd left = Eigen::VectorXd::Zero(n + 1);
105
106
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd lef = Eigen::VectorXd::Zero(n);
107
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd end = Eigen::VectorXd::Zero(n);
108
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd dist = Eigen::VectorXd::Zero(n);
109
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd disall = Eigen::VectorXd::Zero(numCandidates);
110
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::MatrixXd cands = Eigen::MatrixXd::Zero(n, numCandidates);
111 220 double tmax = 0.0;
112 220 Eigen::Index imax = 0;
113
114 220 bool ende = false;
115 220 Eigen::Index ncan = 0;
116
117
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 Eigen::VectorXd dq = Eigen::VectorXd::Zero(n);
118
9/18
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 110 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 110 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 110 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 110 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 110 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 110 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 110 times.
✗ Branch 26 not taken.
 220 dq.head(n - 1) = D(seq(1, n - 1)).array() / D(seq(0, n - 2)).array();
119
2/4
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
 220 dq(n - 1) = 1.0 / D(n - 1);
120 LOG_DATA("dq = {}", dq.transpose());
121
122 220 Eigen::Index i = n;
123 220 Eigen::Index iold = i;
124
125 236 auto BACKTS = [](const Eigen::Index& n, Eigen::Index& i, const auto& end, auto& dist, const auto& lef, auto& left, auto& ende) {
126
6/6
✓ Branch 0 taken 1124 times.
✓ Branch 1 taken 104 times.
✓ Branch 2 taken 11 times.
✓ Branch 3 taken 1 times.
✓ Branch 4 taken 19 times.
✓ Branch 5 taken 5 times.
 1264 for (i++; i < n; i++)
127 {
128
5/6
✗ Branch 2 not taken.
✓ Branch 3 taken 1124 times.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 9 times.
✓ Branch 10 taken 6 times.
✓ Branch 11 taken 13 times.
 1154 if (dist(i) <= end(i))
129 {
130 8 dist(i)++;
131 8 left(i) = std::pow(dist(i) + lef(i), 2);
132 8 break;
133 }
134
6/6
✓ Branch 0 taken 104 times.
✓ Branch 1 taken 1020 times.
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 8 times.
✓ Branch 4 taken 5 times.
✓ Branch 5 taken 8 times.
 1146 if (i == n - 1) { ende = true; }
135 }
136 };
137
138 336 auto COLLECTs = [&chi2, &a]<typename DerD>(const Eigen::Index& n, const Eigen::Index& maxcan, const Eigen::MatrixBase<DerD>& D, const auto& lef, const auto& left, const auto& right,
139 auto& dist, auto& end, Eigen::Index& ncan, auto& disall, auto& cands, double& tmax, Eigen::Index& imax) {
140 246 auto STOREs = [&n, &a](const Eigen::Index& ican, const Eigen::Index& ipos, Eigen::Index& imax, const double& t, double& tmax, const auto& dist, auto& cands, auto& disall) {
141
18/36
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 107 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 107 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 107 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 107 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 2 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 2 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 2 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 2 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 2 times.
✗ Branch 32 not taken.
✓ Branch 34 taken 2 times.
✗ Branch 35 not taken.
✓ Branch 37 taken 21 times.
✗ Branch 38 not taken.
✓ Branch 40 taken 21 times.
✗ Branch 41 not taken.
✓ Branch 43 taken 21 times.
✗ Branch 44 not taken.
✓ Branch 46 taken 21 times.
✗ Branch 47 not taken.
✓ Branch 49 taken 21 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 21 times.
✗ Branch 53 not taken.
 130 cands(seq(0, n - 1), ipos) = dist(seq(0, n - 1)) + a;
142 LOG_DATA(" cands =\n{}", cands);
143 130 disall(ipos) = t;
144 LOG_DATA(" disall = {}", disall.transpose());
145 130 tmax = t;
146 130 imax = ipos;
147
6/6
✓ Branch 0 taken 110 times.
✓ Branch 1 taken 107 times.
✓ Branch 2 taken 3 times.
✓ Branch 3 taken 2 times.
✓ Branch 4 taken 62 times.
✓ Branch 5 taken 21 times.
 305 for (Eigen::Index i = 0; i < ican; i++)
148 {
149
6/6
✓ Branch 1 taken 1 times.
✓ Branch 2 taken 109 times.
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 2 times.
✓ Branch 7 taken 14 times.
✓ Branch 8 taken 48 times.
 175 if (disall(i) > tmax)
150 {
151 16 imax = i;
152 16 tmax = disall(i);
153 }
154 }
155 };
156
157
9/18
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 104 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 104 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 11 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 11 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 11 times.
✗ Branch 26 not taken.
 116 double t = chi2 - (right(0) - left(0)) * D(0);
158 LOG_DATA(" t = {}", t);
159
3/6
✓ Branch 1 taken 104 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11 times.
✗ Branch 8 not taken.
 116 end(0)++;
160 LOG_DATA(" maxcan = {}", maxcan);
161 LOG_DATA(" tmax = {}", tmax);
162 LOG_DATA(" end(0) = {}", end(0));
163 LOG_DATA(" dist(0) = {}", dist(0));
164
12/18
✓ Branch 1 taken 211 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 211 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 107 times.
✓ Branch 7 taken 104 times.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 3 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 2 times.
✓ Branch 15 taken 1 times.
✓ Branch 17 taken 36 times.
✗ Branch 18 not taken.
✓ Branch 20 taken 36 times.
✗ Branch 21 not taken.
✓ Branch 22 taken 25 times.
✓ Branch 23 taken 11 times.
 250 while (dist(0) <= end(0))
165 {
166 LOG_DATA(" ncan = {}", ncan);
167
4/6
✓ Branch 0 taken 107 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 2 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 15 times.
✓ Branch 5 taken 10 times.
 134 if (ncan < maxcan)
168 {
169 124 ncan++;
170
3/6
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 15 times.
✗ Branch 8 not taken.
 124 STOREs(ncan, ncan - 1, imax, t, tmax, dist, cands, disall);
171 }
172
2/6
✗ Branch 0 not taken.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 4 times.
 10 else if (t < tmax)
173 {
174
1/6
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 7 taken 6 times.
✗ Branch 8 not taken.
 6 STOREs(maxcan, imax, imax, t, tmax, dist, cands, disall);
175 }
176
9/18
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 107 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 2 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 2 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 2 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 25 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 25 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 25 times.
✗ Branch 26 not taken.
 134 t += (2 * (dist(0) + lef(0)) + 1) * D(0);
177 LOG_DATA(" t = {}", t);
178
3/6
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 25 times.
✗ Branch 8 not taken.
 134 dist(0)++;
179 LOG_DATA(" dist(0) = {}", dist(0));
180 }
181 };
182
183
2/2
✓ Branch 0 taken 1264 times.
✓ Branch 1 taken 110 times.
 2748 while (!ende)
184 {
185 2528 i--;
186 LOG_DATA("i = {}", i);
187
188
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1256 times.
 2528 if (iold <= i)
189 {
190
2/4
✓ Branch 1 taken 8 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8 times.
✗ Branch 5 not taken.
 16 lef(i) += L(i + 1, i);
191 }
192 else
193 {
194
6/12
✓ Branch 1 taken 1256 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1256 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1256 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1256 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1256 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1256 times.
✗ Branch 17 not taken.
 2512 lef(i) = L(seq(i + 1, n - 1), i).dot(dist(seq(i + 1, n - 1)));
195 }
196 LOG_DATA(" lef = {}", lef.transpose());
197 2528 iold = i;
198
4/8
✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1264 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1264 times.
✗ Branch 11 not taken.
 2528 right(i) = (right(i + 1) - left(i + 1)) * dq(i);
199 LOG_DATA(" right = {}", right.transpose());
200
1/2
✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
 2528 auto reach = std::sqrt(right(i));
201 LOG_DATA(" reach = {}", reach);
202
2/4
✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
 2528 auto delta = a(i) - reach - lef(i);
203 LOG_DATA(" delta = {}", delta);
204
2/4
✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
 2528 dist(i) = std::ceil(delta) - a(i);
205 LOG_DATA(" dist = {}", dist.transpose());
206
4/6
✓ Branch 1 taken 1264 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1264 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 2 times.
✓ Branch 7 taken 1262 times.
 2528 if (dist(i) > reach - lef(i)) // nothing at this level -> backtrack
207 {
208
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 4 BACKTS(n, i, end, dist, lef, left, ende);
209 4 continue;
210 }
211 // set the right border
212
2/4
✓ Branch 1 taken 1262 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1262 times.
✗ Branch 5 not taken.
 2524 end(i) = reach - lef(i) - 1;
213 LOG_DATA(" end = {}", end.transpose());
214
3/6
✓ Branch 1 taken 1262 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1262 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1262 times.
✗ Branch 9 not taken.
 2524 left(i) = std::pow(dist(i) + lef(i), 2);
215 LOG_DATA(" left = {}", left.transpose());
216
217
2/2
✓ Branch 0 taken 116 times.
✓ Branch 1 taken 1146 times.
 2524 if (i == 0)
218 {
219
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 232 COLLECTs(n, numCandidates, D, lef, left, right, dist, end, ncan, disall, cands, tmax, imax);
220
1/2
✓ Branch 1 taken 116 times.
✗ Branch 2 not taken.
 232 BACKTS(n, i, end, dist, lef, left, ende);
221 }
222 }
223
224 // Sort candidates by norm
225
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 auto p = sort_permutation(disall, std::less<>{});
226
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 apply_permutation_in_place(disall, p);
227
1/2
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
 220 apply_permutation_colwise_in_place(cands, p);
228
229
3/6
✓ Branch 1 taken 110 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 110 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 110 times.
✗ Branch 8 not taken.
 220 return { cands.rightCols(ncan), disall.tail(ncan) };
230 220 }
231
232 /// @brief Performs an integer least-squares to search for integer candidates for the ambiguities using the search-and-shrink technique (MLAMBDA)
233 /// @param[in] zh Decorrelated float ambiguity vector [cycles]
234 /// @param[in] L_LTDL_Q Lower-triangular matrix from the L^T * D * L decomposition of Q
235 /// @param[in] D_LTDL_Q Diagonal entries from the L^T * D * L decomposition of Q
236 /// @param[in] numCandidates Requested number of candidates (default = 2)
237 /// @return Pair of: First a list of integer candidates (last column is the most likely), Vector with squared norm of the integer candidates
238 /// @note See \cite chang2005 Chang 2005, Argorithm 3.3
239 /// @note See \cite deJonge1996 de Jonge 1996, ch. 4.8
240 /// @note See ssearch.m file from TUDelft Matlab code
241 template<typename DerivedA, typename DerivedL, typename DerivedD>
242 std::pair<Eigen::MatrixXd, Eigen::VectorXd>
243 214 integerLeastSquaresSearchAndShrink(const Eigen::MatrixBase<DerivedA>& zh, const Eigen::MatrixBase<DerivedL>& L_LTDL_Q,
244 const Eigen::MatrixBase<DerivedD>& D_LTDL_Q, const Eigen::Index& numCandidates = 2)
245 {
246 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
247
1/2
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
 214 INS_ASSERT_USER_ERROR(zh.cols() == 1, "Parameter 'zh' has to be a vector");
248 static_assert(DerivedD::ColsAtCompileTime == Eigen::Dynamic || DerivedD::ColsAtCompileTime == 1);
249
1/2
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
 214 INS_ASSERT_USER_ERROR(D_LTDL_Q.cols() == 1, "Parameter 'D' has to be a vector");
250
1/2
✓ Branch 2 taken 107 times.
✗ Branch 3 not taken.
 214 INS_ASSERT_USER_ERROR(zh.rows() == D_LTDL_Q.rows(), "Parameter 'zh' and 'D' must have same dimension");
251
252 using Eigen::seq;
253
254 214 auto n = zh.rows(); // Amount of ambiguities
255
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 107 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
 214 if (n == 1) { return {}; }
256
257 2509 auto sgn = [](auto x) {
258 2509 int sgn = gcem::sgn(x);
259
4/6
✓ Branch 0 taken 1414 times.
✓ Branch 1 taken 1010 times.
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 59 times.
✗ Branch 5 not taken.
 2509 return sgn ? sgn : 1;
260 };
261
262
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::VectorXd dist = Eigen::VectorXd::Zero(n);
263
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::VectorXd zb = Eigen::VectorXd::Zero(n);
264
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::VectorXd z = Eigen::VectorXd::Zero(n);
265
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::VectorXd step = Eigen::VectorXd::Zero(n);
266
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::MatrixXd S = Eigen::MatrixXd::Zero(n, n);
267
268
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::MatrixXd cands = Eigen::MatrixXd::Zero(n, numCandidates);
269
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 Eigen::VectorXd sqnorm = Eigen::VectorXd::Zero(numCandidates);
270
271 214 auto maxDist = std::numeric_limits<double>::infinity(); // current chi^2
272 214 auto k = n - 1;
273 214 Eigen::Index count = 0; // the number of candidates
274
275
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 zb(n - 1) = zh(n - 1);
276
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 z(n - 1) = std::round(zb(n - 1));
277
2/4
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
 214 double y = zb(n - 1) - z(n - 1);
278
1/2
✓ Branch 2 taken 107 times.
✗ Branch 3 not taken.
 214 step(n - 1) = sgn(y);
279 214 auto imax = numCandidates - 1;
280
1/2
✓ Branch 0 taken 2509 times.
✗ Branch 1 not taken.
 5018 for (size_t loop = 0; loop < 10000; loop++)
281 {
282
2/4
✓ Branch 1 taken 2509 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 2509 times.
✗ Branch 6 not taken.
 5018 double newDist = dist(k) + std::pow(y, 2) / D_LTDL_Q(k);
283
2/2
✓ Branch 0 taken 1366 times.
✓ Branch 1 taken 1143 times.
 5018 if (newDist < maxDist)
284 {
285
2/2
✓ Branch 0 taken 1036 times.
✓ Branch 1 taken 330 times.
 2732 if (k != 0) // Case 1: move down
286 {
287 2072 k--;
288
1/2
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
 2072 dist(k) = newDist;
289
11/22
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1036 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1036 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 1036 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 1036 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 1036 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 1036 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 1036 times.
✗ Branch 26 not taken.
✓ Branch 28 taken 1036 times.
✗ Branch 29 not taken.
✓ Branch 31 taken 1036 times.
✗ Branch 32 not taken.
 2072 S(k, seq(0, k)) = S(k + 1, seq(0, k)) + (z(k + 1) - zb(k + 1)) * L_LTDL_Q(k + 1, seq(0, k));
290
3/6
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1036 times.
✗ Branch 8 not taken.
 2072 zb(k) = zh(k) + S(k, k);
291
2/4
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
 2072 z(k) = std::round(zb(k));
292
2/4
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
 2072 y = zb(k) - z(k);
293
1/2
✓ Branch 2 taken 1036 times.
✗ Branch 3 not taken.
 2072 step(k) = sgn(y);
294 }
295 else // Case 2: store the found candidate and try next valid integer
296 {
297
2/2
✓ Branch 0 taken 219 times.
✓ Branch 1 taken 111 times.
 660 if (count < numCandidates) // store the first p − 1 initial points
298 {
299
5/8
✓ Branch 0 taken 112 times.
✓ Branch 1 taken 107 times.
✓ Branch 3 taken 112 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 112 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 219 times.
✗ Branch 8 not taken.
 438 if (count == 0 || newDist > sqnorm(imax)) { imax = count; } // Addition from Rtklib
300
2/4
✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 219 times.
✗ Branch 5 not taken.
 438 cands.col(count) = z;
301
1/2
✓ Branch 1 taken 219 times.
✗ Branch 2 not taken.
 438 sqnorm(count) = newDist;
302 438 count++;
303 }
304 else
305 {
306
3/4
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 4 times.
✓ Branch 4 taken 107 times.
 222 if (newDist < sqnorm(imax))
307 {
308
2/4
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 4 times.
✗ Branch 5 not taken.
 8 cands.col(imax) = z;
309
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 sqnorm(imax) = newDist;
310
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 8 sqnorm.maxCoeff(&imax);
311 }
312
1/2
✓ Branch 1 taken 111 times.
✗ Branch 2 not taken.
 222 maxDist = sqnorm(imax);
313 }
314
2/4
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
 660 z(0) += step(0); // next valid integer
315
2/4
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
 660 y = zb(0) - z(0);
316
3/6
✓ Branch 1 taken 330 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 330 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 330 times.
✗ Branch 9 not taken.
 660 step(0) = -step(0) - sgn(step(0));
317 }
318 }
319 else // Case 3: exit or move up
320 {
321
2/2
✓ Branch 0 taken 107 times.
✓ Branch 1 taken 1036 times.
 2286 if (k == n - 1) { break; }
322
323 2072 k++; // move up
324
2/4
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
 2072 z(k) += step(k); // next valid integer
325
2/4
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
 2072 y = zb(k) - z(k);
326
3/6
✓ Branch 1 taken 1036 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1036 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1036 times.
✗ Branch 9 not taken.
 2072 step(k) = -step(k) - sgn(step(k));
327 }
328 }
329
330 // Sort candidates by norm
331
1/2
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
 214 auto p = sort_permutation(sqnorm, std::less<>{});
332
1/2
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
 214 apply_permutation_in_place(sqnorm, p);
333
1/2
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
 214 apply_permutation_colwise_in_place(cands, p);
334
335
3/6
✓ Branch 1 taken 107 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 107 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 107 times.
✗ Branch 8 not taken.
 214 return { cands.rightCols(count), sqnorm.tail(count) };
336 214 }
337
338 } // namespace NAV::Ambiguity
339