INSTINCT Code Coverage Report

Directory: src/
File: Navigation/GNSS/Ambiguity/internal/Decorrelation.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 46 46 100.0%
Functions: 10 10 100.0%
Branches: 76 142 53.5%
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 Decorrelation.hpp
10 /// @brief Ambiguity decorrelation algorithms
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-09-04
13
14 #pragma once
15
16 #include <tuple>
17
18 #include "Navigation/Math/Math.hpp"
19 #include "util/Eigen.hpp"
20 #include "util/Logger.hpp"
21 #include "util/Assert.h"
22 #include <Eigen/src/Core/ArithmeticSequence.h>
23 #include <Eigen/src/Core/MatrixBase.h>
24 #include <Eigen/src/Core/util/Meta.h>
25
26 namespace NAV::Ambiguity
27 {
28
29 namespace internal
30 {
31
32 /// @brief \cite chang2005 Chang 2005, Integer Gauss Transformations algorithm
33 /// @param[in, out] L (L^T * D * L) decomposition of Q_z
34 /// @param[in] i Row index
35 /// @param[in] j Col index
36 /// @param[in, out] a Float ambiguity vector [cycles]
37 /// @param[in, out] Z Decorrelation transformation matrix
38 /// @param[in] n Dimension
39 template<typename DerivedL, typename DerivedZ, typename DerivedA>
40 12019024 void gauss(Eigen::MatrixBase<DerivedL>& L, Eigen::Index i, Eigen::Index j, Eigen::MatrixBase<DerivedA>& a, Eigen::MatrixBase<DerivedZ>& Z, Eigen::Index n)
41 {
42 using Eigen::seq;
43
44
1/2
✓ Branch 1 taken 6009512 times.
✗ Branch 2 not taken.
 12019024 auto mu = std::round(L(i, j));
45
2/2
✓ Branch 0 taken 955439 times.
✓ Branch 1 taken 5054073 times.
 12019024 if (mu != 0)
46 {
47
6/12
✓ Branch 1 taken 955439 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 955439 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 955439 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 955439 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 955439 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 955439 times.
✗ Branch 17 not taken.
 1910878 L(seq(i, n - 1), j) -= mu * L(seq(i, n - 1), i);
48
6/12
✓ Branch 1 taken 955439 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 955439 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 955439 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 955439 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 955439 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 955439 times.
✗ Branch 17 not taken.
 1910878 Z(seq(0, n - 1), j) -= mu * Z(seq(0, n - 1), i);
49
2/4
✓ Branch 1 taken 955439 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 955439 times.
✗ Branch 5 not taken.
 1910878 a(j) -= mu * a(i);
50 }
51 12019024 }
52
53 /// @brief \cite chang2005 Chang 2005, Permutations algorithm
54 /// @param[in, out] L (L^T * D * L) decomposition of Q_z
55 /// @param[in, out] D (L^T * D * L) decomposition of Q_z
56 /// @param[in] k Index
57 /// @param[in] delta Delta parameter
58 /// @param[in, out] a Float ambiguity vector [cycles]
59 /// @param[in, out] Z Z trafo
60 /// @param[in] n Dimension
61 template<typename DerivedL, typename DerivedD, typename DerivedA, typename DerivedZ>
62 817862 void permute(Eigen::MatrixBase<DerivedL>& L, Eigen::MatrixBase<DerivedD>& D, Eigen::Index k, double delta, Eigen::MatrixBase<DerivedA>& a, Eigen::MatrixBase<DerivedZ>& Z, Eigen::Index n)
63 {
64 using Eigen::seq;
65
66
1/2
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
 817862 auto eta = D(k) / delta;
67
2/4
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
 817862 auto lambda = D(k + 1) * L(k + 1, k) / delta;
68
2/4
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
 817862 D(k) = eta * D(k + 1);
69
1/2
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
 817862 D(k + 1) = delta;
70
71
9/18
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 408931 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 408931 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 408931 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 408931 times.
✗ Branch 17 not taken.
✓ Branch 19 taken 408931 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 408931 times.
✗ Branch 23 not taken.
✓ Branch 25 taken 408931 times.
✗ Branch 26 not taken.
 1635724 L(seq(k, k + 1), seq(0, k - 1)) = (Eigen::MatrixXd(2, 2) << -L(k + 1, k), 1,
72
1/2
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
 817862 eta, lambda)
73 817862 .finished()
74
4/8
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 408931 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 408931 times.
✗ Branch 11 not taken.
 1635724 * L(seq(k, k + 1), seq(0, k - 1));
75
1/2
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
 817862 L(k + 1, k) = lambda;
76
5/10
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 408931 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 408931 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 408931 times.
✗ Branch 14 not taken.
 817862 L(seq(k + 2, n - 1), k).swap(L(seq(k + 2, n - 1), k + 1));
77
3/6
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 408931 times.
✗ Branch 8 not taken.
 817862 Z.col(k).swap(Z.col(k + 1));
78
2/4
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 408931 times.
✗ Branch 5 not taken.
 817862 std::swap(a(k), a(k + 1));
79 817862 }
80
81 } // namespace internal
82
83 /// @brief Decorrelates the ambiguities
84 /// @param[in] a Float ambiguity vector [cycles]
85 /// @param[in] Q Variance/covariance matrix of the ambiguities
86 /// @return [Qz, Z, L, D, z] L, D are a (L^T * D * L) decomposition of Q_z
87 /// @note See \cite chang2005 Chang 2005, Reduction algorithm
88 template<typename DerivedA, typename DerivedQ>
89 [[nodiscard]] std::optional<std::tuple<typename DerivedQ::PlainObject, // Qz
90 typename DerivedQ::PlainObject, // Z
91 typename DerivedQ::PlainObject, // L
92 typename DerivedA::PlainObject, // D
93 typename DerivedA::PlainObject>> // z
94 2421 decorrelate_ztrafo(const Eigen::MatrixBase<DerivedA>& a, const Eigen::MatrixBase<DerivedQ>& Q)
95 {
96 static_assert(DerivedA::ColsAtCompileTime == Eigen::Dynamic || DerivedA::ColsAtCompileTime == 1);
97
1/2
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
 2421 INS_ASSERT_USER_ERROR(a.cols() == 1, "Parameter 'a' has to be a vector");
98
99 using Eigen::seq;
100
101 2421 auto n = a.rows(); // Amount of ambiguities
102
1/2
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
 2421 auto ltdl_decomp = math::LtDLdecomp_choleskyFact(Q);
103
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 1212 times.
 2421 if (!ltdl_decomp) { return {}; }
104
1/2
✓ Branch 2 taken 1212 times.
✗ Branch 3 not taken.
 2421 auto [L, D] = *ltdl_decomp;
105
106
1/2
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
 2421 typename DerivedQ::PlainObject Z;
107
1/2
✓ Branch 1 taken 1205 times.
✗ Branch 2 not taken.
 2407 if constexpr (DerivedQ::RowsAtCompileTime == Eigen::Dynamic) { Z.setIdentity(n, n); }
108
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 14 else { Z.setIdentity(); }
109
110
1/2
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
 2421 typename DerivedA::PlainObject z = a;
111
112 2421 auto k = n - 2;
113 2421 auto k1 = n - 2;
114
115
2/2
✓ Branch 0 taken 5592233 times.
✓ Branch 1 taken 1212 times.
 11173279 while (k >= 0)
116 {
117
2/2
✓ Branch 0 taken 440093 times.
✓ Branch 1 taken 5152140 times.
 11170858 if (k <= k1)
118 {
119
2/2
✓ Branch 0 taken 6009512 times.
✓ Branch 1 taken 440093 times.
 12882591 for (auto i = k + 1; i < n; i++)
120 {
121
1/2
✓ Branch 1 taken 6009512 times.
✗ Branch 2 not taken.
 12003304 internal::gauss(L, i, k, z, Z, n);
122 }
123 }
124
3/6
✓ Branch 1 taken 5592233 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 5592233 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 5592233 times.
✗ Branch 9 not taken.
 11170858 auto delta = D(k) + std::pow(L(k + 1, k), 2) * D(k + 1);
125
3/4
✓ Branch 1 taken 5592233 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 408931 times.
✓ Branch 4 taken 5183302 times.
 11170858 if (delta + 1e-6 < D(k + 1))
126 {
127
1/2
✓ Branch 1 taken 408931 times.
✗ Branch 2 not taken.
 817077 internal::permute(L, D, k, delta, z, Z, n);
128 817077 k1 = k;
129 817077 k = n - 2;
130 }
131 else
132 {
133 10353781 k--;
134 }
135 }
136
137
4/8
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1212 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1212 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1212 times.
✗ Branch 11 not taken.
 2421 typename DerivedQ::PlainObject Qz = Z.transpose() * Q * Z;
138
139
2/4
✓ Branch 1 taken 1212 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
 2421 return std::make_tuple(Qz, Z, L, D, z);
140 2411 }
141
142 } // namespace NAV::Ambiguity
143