INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/NumericalIntegration.hpp
Date: 2025-11-25 23:34:18
Exec Total Coverage
Lines: 49 57 86.0%
Functions: 4 12 33.3%
Branches: 125 426 29.3%
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 NumericalIntegration.hpp
10 /// @brief Provides Numerical integration methods
11 /// @author T. Topp (topp@ins.uni-stuttgart.de)
12 /// @date 2023-12-11
13
14 #pragma once
15
16 #include <array>
17 #include <cmath>
18 #include <numeric>
19 #include <type_traits>
20 #include <gcem.hpp>
21
22 #include "util/Eigen.hpp"
23 #include <unsupported/Eigen/MatrixFunctions>
24
25 #include "Navigation/Math/Math.hpp"
26
27 #include "util/Assert.h"
28 #include "util/Logger.hpp"
29
30 namespace NAV
31 {
32
33 namespace ButcherTableau
34 {
35
36 #if defined(__GNUC__) && !defined(__clang__)
37 #pragma GCC diagnostic push
38 #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" // NOLINT(clang-diagnostic-unknown-warning-option)
39 #endif
40
41 /// @brief Calculates explicit Runge-Kutta methods. Order is defined by the Butcher tableau
42 /// @param[in] y_n State vector at time t_n
43 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
44 /// @param[in] h Integration step in [s]
45 /// @param[in] f Time derivative function
46 /// @param[in] constParam Constant parameters passed to each time derivative function call
47 /// @param[in] t_n Time t_n
48 /// @return State vector at time t_(n+1)
49 template<typename Y, typename Z, std::floating_point Scalar, size_t s, std::array<std::array<Scalar, s + 1>, s + 1> butcherTableau>
50 18630 inline Y RungeKuttaExplicit(const Y& y_n, const std::array<Z, s>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n)
51 {
52 static_assert(gcem::abs(static_cast<Scalar>(1.0) - std::accumulate(butcherTableau[s].begin(), butcherTableau[s].end(), static_cast<Scalar>(0.0))) < 1e-8, // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
53 "The sum of the last row in the Butcher tableau has to be 1");
54
2/2
✓ Branch 0 taken 93150 times.
✓ Branch 1 taken 18630 times.
 111780 for (size_t r = 0; r <= s; ++r)
55 {
56
2/2
✓ Branch 0 taken 186300 times.
✓ Branch 1 taken 93150 times.
 279450 for (size_t c = r + 1; c <= s; ++c)
57 {
58
3/6
✓ Branch 1 taken 186300 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 186300 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 186300 times.
✗ Branch 7 not taken.
 186300 INS_ASSERT_USER_ERROR(butcherTableau.at(r).at(c) == 0.0, "All terms in the upper triangle have to be 0");
59 }
60 }
61
62 // std::string result = "y_(n+1) = y_n";
63
64 // std::string sum_b_str;
65 // std::string sum_b_val;
66
1/2
✓ Branch 1 taken 18630 times.
✗ Branch 2 not taken.
 18630 Y sum_b{};
67
3/4
✓ Branch 1 taken 74520 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 74520 times.
✓ Branch 4 taken 18630 times.
 93150 std::array<Y, s> k{};
68
2/2
✓ Branch 0 taken 74520 times.
✓ Branch 1 taken 18630 times.
 93150 for (size_t i = 1; i <= s; ++i)
69 {
70
1/2
✓ Branch 2 taken 74520 times.
✗ Branch 3 not taken.
 74520 auto b = butcherTableau[s].at(i);
71
1/2
✓ Branch 1 taken 74520 times.
✗ Branch 2 not taken.
 74520 auto c = butcherTableau.at(i - 1)[0];
72
1/2
✓ Branch 1 taken 74520 times.
✗ Branch 2 not taken.
 74520 Y sum_a{};
73 // std::string sum_a_str;
74 // std::string sum_a_val;
75
2/2
✓ Branch 0 taken 111780 times.
✓ Branch 1 taken 74520 times.
 186300 for (size_t j = 2; j <= i; ++j)
76 {
77
2/4
✓ Branch 1 taken 111780 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 111780 times.
✗ Branch 5 not taken.
 111780 auto a = butcherTableau.at(i - 1).at(j - 1);
78
5/8
✓ Branch 0 taken 55890 times.
✓ Branch 1 taken 55890 times.
✓ Branch 3 taken 55890 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 55890 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 55890 times.
✗ Branch 10 not taken.
 111780 if (j == 2) { sum_a = a * k.at(j - 2); }
79
3/6
✓ Branch 1 taken 55890 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55890 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 55890 times.
✗ Branch 8 not taken.
 55890 else { sum_a += a * k.at(j - 2); }
80 // sum_a_str += fmt::format("a{}{} * k{}", i, j-1, j-1);
81 // sum_a_val += fmt::format("{:^3.1f} * k{}", a, j - 1);
82 // if (j < i) {
83 // sum_a_str += " + ";
84 // sum_a_val += " + ";
85 // }
86 }
87
88
5/8
✓ Branch 0 taken 18630 times.
✓ Branch 1 taken 55890 times.
✓ Branch 3 taken 18630 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18630 times.
✗ Branch 10 not taken.
 74520 if (i < 2) { k.at(i - 1) = f(y_n, z.at(i - 1), constParam, t_n + c * h); } // Do not add sum_a, because can have nan values
89
6/12
✓ Branch 1 taken 55890 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55890 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 55890 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 55890 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 55890 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 55890 times.
✗ Branch 17 not taken.
 55890 else { k.at(i - 1) = f(y_n + sum_a * h, z.at(i - 1), constParam, t_n + c * h); }
90 // if (sum_a_str.empty()) { sum_a_str = "0"; }
91 // if (sum_a_val.empty()) { sum_a_val = "0"; }
92 // fmt::println("k{} = f(t_n + c{} * h, y_n + ({}) * h)", i, i, sum_a_str);
93 // fmt::println("k{} = f(t_n + {:^3.1f} * h, y_n + ({:^{w}}) * h)", i, c, sum_a_val, fmt::arg("w",sum_a_str.length()));
94
95
5/8
✓ Branch 0 taken 18630 times.
✓ Branch 1 taken 55890 times.
✓ Branch 3 taken 18630 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 18630 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 18630 times.
✗ Branch 10 not taken.
 74520 if (i == 1) { sum_b = b * k.at(i - 1); }
96
3/6
✓ Branch 1 taken 55890 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 55890 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 55890 times.
✗ Branch 8 not taken.
 55890 else { sum_b += b * k.at(i - 1); }
97 // sum_b_str += fmt::format("b{} * k{}", i, i);
98 // sum_b_val += fmt::format("{:.2f} * k{}", butcherTableau[s].at(i), i);
99 // if (i < s)
100 // {
101 // sum_b_str += " + ";
102 // sum_b_val += " + ";
103 // }
104 }
105
106 // std::cout << result + fmt::format(" + h * ({})", sum_b_str) << std::endl;
107 // std::cout << result + fmt::format(" + h * ({})", sum_b_val) << std::endl;
108
3/6
✓ Branch 1 taken 18630 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 18630 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 18630 times.
✗ Branch 8 not taken.
 18630 return y_n + h * sum_b;
109 }
110
111 /// @brief Calculates explicit Runge-Kutta methods. Order is defined by the Butcher tableau
112 /// @param[in] y_n State vector at time t_n
113 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
114 /// @param[in] h Integration step in [s]
115 /// @param[in] f Time derivative function
116 /// @param[in] constParam Constant parameters passed to each time derivative function call
117 /// @param[in] t_n Time t_n
118 /// @return State vector at time t_(n+1)
119 template<typename Derived, typename Z, std::floating_point Scalar, size_t s, std::array<std::array<Scalar, s + 1>, s + 1> butcherTableau>
120 99994 inline typename Derived::PlainObject RungeKuttaExplicitQuat(const Eigen::MatrixBase<Derived>& y_n, const std::array<Z, s>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n)
121 {
122 static_assert(gcem::abs(static_cast<Scalar>(1.0) - std::accumulate(butcherTableau[s].begin(), butcherTableau[s].end(), static_cast<Scalar>(0.0))) < 1e-8, // NOLINT(boost-use-ranges,modernize-use-ranges) // There is no ranges::accumulate
123 "The sum of the last row in the Butcher tableau has to be 1");
124
2/2
✓ Branch 0 taken 199988 times.
✓ Branch 1 taken 49997 times.
 499970 for (size_t r = 0; r <= s; ++r)
125 {
126
2/2
✓ Branch 0 taken 299982 times.
✓ Branch 1 taken 199988 times.
 999940 for (size_t c = r + 1; c <= s; ++c)
127 {
128
3/6
✓ Branch 1 taken 299982 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 299982 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 299982 times.
✗ Branch 7 not taken.
 599964 INS_ASSERT_USER_ERROR(butcherTableau.at(r).at(c) == 0.0, "All terms in the upper triangle have to be 0");
129 }
130 }
131
132 // using DerivativeVector = Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>;
133 using DerivativeVector = decltype(f(y_n, z.front(), constParam, t_n));
134 99994 Eigen::Index N = y_n.rows() - 4;
135
136 // Calculates y_n + y_delta, but treats
137 // - the last 4 entries in y_n as a quaternion
138 // - the last 3 entries in y_delta as angular rate measurements
139 249985 auto applyDerivative = [&]<typename Derived2>(const Eigen::MatrixBase<Derived2>& y_delta) {
140 149991 Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime> y_nh;
141 if constexpr (Derived::RowsAtCompileTime == Eigen::Dynamic) { y_nh = Eigen::Vector<typename Derived::Scalar, Derived::RowsAtCompileTime>::Zero(y_n.rows()); }
142
10/100
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✓ Branch 33 taken 49997 times.
✗ Branch 34 not taken.
✓ Branch 36 taken 49997 times.
✗ Branch 37 not taken.
✓ Branch 39 taken 49997 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 49997 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 49997 times.
✗ Branch 46 not taken.
✓ Branch 49 taken 99994 times.
✗ Branch 50 not taken.
✓ Branch 52 taken 99994 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 99994 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 99994 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 99994 times.
✗ Branch 62 not taken.
✗ Branch 65 not taken.
✗ Branch 66 not taken.
✗ Branch 68 not taken.
✗ Branch 69 not taken.
✗ Branch 71 not taken.
✗ Branch 72 not taken.
✗ Branch 74 not taken.
✗ Branch 75 not taken.
✗ Branch 77 not taken.
✗ Branch 78 not taken.
✗ Branch 81 not taken.
✗ Branch 82 not taken.
✗ Branch 84 not taken.
✗ Branch 85 not taken.
✗ Branch 87 not taken.
✗ Branch 88 not taken.
✗ Branch 90 not taken.
✗ Branch 91 not taken.
✗ Branch 93 not taken.
✗ Branch 94 not taken.
✗ Branch 97 not taken.
✗ Branch 98 not taken.
✗ Branch 100 not taken.
✗ Branch 101 not taken.
✗ Branch 103 not taken.
✗ Branch 104 not taken.
✗ Branch 106 not taken.
✗ Branch 107 not taken.
✗ Branch 109 not taken.
✗ Branch 110 not taken.
✗ Branch 113 not taken.
✗ Branch 114 not taken.
✗ Branch 116 not taken.
✗ Branch 117 not taken.
✗ Branch 119 not taken.
✗ Branch 120 not taken.
✗ Branch 122 not taken.
✗ Branch 123 not taken.
✗ Branch 125 not taken.
✗ Branch 126 not taken.
✗ Branch 129 not taken.
✗ Branch 130 not taken.
✗ Branch 132 not taken.
✗ Branch 133 not taken.
✗ Branch 135 not taken.
✗ Branch 136 not taken.
✗ Branch 138 not taken.
✗ Branch 139 not taken.
✗ Branch 141 not taken.
✗ Branch 142 not taken.
✗ Branch 145 not taken.
✗ Branch 146 not taken.
✗ Branch 148 not taken.
✗ Branch 149 not taken.
✗ Branch 151 not taken.
✗ Branch 152 not taken.
✗ Branch 154 not taken.
✗ Branch 155 not taken.
✗ Branch 157 not taken.
✗ Branch 158 not taken.
 149991 y_nh.head(N) = y_n.head(N) + y_delta.head(N);
143
144
12/120
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
✗ Branch 17 not taken.
✗ Branch 18 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✓ Branch 39 taken 49997 times.
✗ Branch 40 not taken.
✓ Branch 42 taken 49997 times.
✗ Branch 43 not taken.
✓ Branch 45 taken 49997 times.
✗ Branch 46 not taken.
✓ Branch 48 taken 49997 times.
✗ Branch 49 not taken.
✓ Branch 52 taken 49997 times.
✗ Branch 53 not taken.
✓ Branch 55 taken 49997 times.
✗ Branch 56 not taken.
✓ Branch 58 taken 99994 times.
✗ Branch 59 not taken.
✓ Branch 61 taken 99994 times.
✗ Branch 62 not taken.
✓ Branch 64 taken 99994 times.
✗ Branch 65 not taken.
✓ Branch 67 taken 99994 times.
✗ Branch 68 not taken.
✓ Branch 71 taken 99994 times.
✗ Branch 72 not taken.
✓ Branch 74 taken 99994 times.
✗ Branch 75 not taken.
✗ Branch 77 not taken.
✗ Branch 78 not taken.
✗ Branch 80 not taken.
✗ Branch 81 not taken.
✗ Branch 83 not taken.
✗ Branch 84 not taken.
✗ Branch 86 not taken.
✗ Branch 87 not taken.
✗ Branch 90 not taken.
✗ Branch 91 not taken.
✗ Branch 93 not taken.
✗ Branch 94 not taken.
✗ Branch 96 not taken.
✗ Branch 97 not taken.
✗ Branch 99 not taken.
✗ Branch 100 not taken.
✗ Branch 102 not taken.
✗ Branch 103 not taken.
✗ Branch 105 not taken.
✗ Branch 106 not taken.
✗ Branch 109 not taken.
✗ Branch 110 not taken.
✗ Branch 112 not taken.
✗ Branch 113 not taken.
✗ Branch 115 not taken.
✗ Branch 116 not taken.
✗ Branch 118 not taken.
✗ Branch 119 not taken.
✗ Branch 121 not taken.
✗ Branch 122 not taken.
✗ Branch 124 not taken.
✗ Branch 125 not taken.
✗ Branch 128 not taken.
✗ Branch 129 not taken.
✗ Branch 131 not taken.
✗ Branch 132 not taken.
✗ Branch 134 not taken.
✗ Branch 135 not taken.
✗ Branch 137 not taken.
✗ Branch 138 not taken.
✗ Branch 140 not taken.
✗ Branch 141 not taken.
✗ Branch 143 not taken.
✗ Branch 144 not taken.
✗ Branch 147 not taken.
✗ Branch 148 not taken.
✗ Branch 150 not taken.
✗ Branch 151 not taken.
✗ Branch 153 not taken.
✗ Branch 154 not taken.
✗ Branch 156 not taken.
✗ Branch 157 not taken.
✗ Branch 159 not taken.
✗ Branch 160 not taken.
✗ Branch 162 not taken.
✗ Branch 163 not taken.
✗ Branch 166 not taken.
✗ Branch 167 not taken.
✗ Branch 169 not taken.
✗ Branch 170 not taken.
✗ Branch 172 not taken.
✗ Branch 173 not taken.
✗ Branch 175 not taken.
✗ Branch 176 not taken.
✗ Branch 178 not taken.
✗ Branch 179 not taken.
✗ Branch 181 not taken.
✗ Branch 182 not taken.
✗ Branch 185 not taken.
✗ Branch 186 not taken.
✗ Branch 188 not taken.
✗ Branch 189 not taken.
 299982 Eigen::Map<Eigen::Quaternion<typename Derived::Scalar>>(y_nh.template tail<4>().data()) =
145
4/40
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✗ Branch 8 not taken.
✗ Branch 9 not taken.
✗ Branch 12 not taken.
✗ Branch 13 not taken.
✓ Branch 15 taken 49997 times.
✗ Branch 16 not taken.
✓ Branch 19 taken 49997 times.
✗ Branch 20 not taken.
✓ Branch 22 taken 99994 times.
✗ Branch 23 not taken.
✓ Branch 26 taken 99994 times.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 33 not taken.
✗ Branch 34 not taken.
✗ Branch 36 not taken.
✗ Branch 37 not taken.
✗ Branch 40 not taken.
✗ Branch 41 not taken.
✗ Branch 43 not taken.
✗ Branch 44 not taken.
✗ Branch 47 not taken.
✗ Branch 48 not taken.
✗ Branch 50 not taken.
✗ Branch 51 not taken.
✗ Branch 54 not taken.
✗ Branch 55 not taken.
✗ Branch 57 not taken.
✗ Branch 58 not taken.
✗ Branch 61 not taken.
✗ Branch 62 not taken.
✗ Branch 64 not taken.
✗ Branch 65 not taken.
✗ Branch 68 not taken.
✗ Branch 69 not taken.
 149991 Eigen::Map<const Eigen::Quaternion<typename Derived::Scalar>>(y_n.template tail<4>().data())
146 // * math::expMapMatrix(y_delta.template tail<3>()); // matrix exponential (~2% faster than normal multiplication because no need for quaternion normalization afterwards)
147 * math::expMapQuat(y_delta.template tail<3>()); // quaternionic exponential (~3% faster than normal multiplication because no normalization and no exp matrix)
148 149991 return y_nh;
149 };
150
151 // std::string result = "y_(n+1) = y_n";
152
153 // std::string sum_b_str;
154 // std::string sum_b_val;
155
1/2
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
 99994 DerivativeVector sum_b{};
156
3/4
✓ Branch 1 taken 149991 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 149991 times.
✓ Branch 4 taken 49997 times.
 399976 std::array<DerivativeVector, s> k{};
157
2/2
✓ Branch 0 taken 149991 times.
✓ Branch 1 taken 49997 times.
 399976 for (size_t i = 1; i <= s; ++i)
158 {
159
1/2
✓ Branch 2 taken 149991 times.
✗ Branch 3 not taken.
 299982 auto b = butcherTableau[s].at(i);
160
1/2
✓ Branch 1 taken 149991 times.
✗ Branch 2 not taken.
 299982 auto c = butcherTableau.at(i - 1)[0];
161
1/2
✓ Branch 1 taken 149991 times.
✗ Branch 2 not taken.
 299982 DerivativeVector sum_a{};
162 // std::string sum_a_str;
163 // std::string sum_a_val;
164
2/2
✓ Branch 0 taken 149991 times.
✓ Branch 1 taken 149991 times.
 599964 for (size_t j = 2; j <= i; ++j)
165 {
166
2/4
✓ Branch 1 taken 149991 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 149991 times.
✗ Branch 5 not taken.
 299982 auto a = butcherTableau.at(i - 1).at(j - 1);
167
5/8
✓ Branch 0 taken 99994 times.
✓ Branch 1 taken 49997 times.
✓ Branch 3 taken 99994 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 99994 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 99994 times.
✗ Branch 10 not taken.
 299982 if (j == 2) { sum_a = a * k.at(j - 2); }
168
3/6
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 49997 times.
✗ Branch 8 not taken.
 99994 else { sum_a += a * k.at(j - 2); }
169 // sum_a_str += fmt::format("a{}{} * k{}", i, j-1, j-1);
170 // sum_a_val += fmt::format("{:^3.1f} * k{}", a, j - 1);
171 // if (j < i) {
172 // sum_a_str += " + ";
173 // sum_a_val += " + ";
174 // }
175 }
176
177
6/10
✓ Branch 0 taken 49997 times.
✓ Branch 1 taken 99994 times.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49997 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 49997 times.
✗ Branch 13 not taken.
 299982 if (i < 2) { k.at(i - 1) = f(y_n, z.at(i - 1), constParam, t_n + c * h); } // Do not add sum_a, because can have nan values
178
5/10
✓ Branch 1 taken 99994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99994 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 99994 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 99994 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 99994 times.
✗ Branch 14 not taken.
 199988 else { k.at(i - 1) = f(applyDerivative(sum_a * h), z.at(i - 1), constParam, t_n + c * h); }
179 // if (sum_a_str.empty()) { sum_a_str = "0"; }
180 // if (sum_a_val.empty()) { sum_a_val = "0"; }
181 // fmt::println("k{} = f(t_n + c{} * h, y_n + ({}) * h)", i, i, sum_a_str);
182 // fmt::println("k{} = f(t_n + {:^3.1f} * h, y_n + ({:^{w}}) * h)", i, c, sum_a_val, fmt::arg("w",sum_a_str.length()));
183
184
5/8
✓ Branch 0 taken 49997 times.
✓ Branch 1 taken 99994 times.
✓ Branch 3 taken 49997 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 49997 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 49997 times.
✗ Branch 10 not taken.
 299982 if (i == 1) { sum_b = b * k.at(i - 1); }
185
3/6
✓ Branch 1 taken 99994 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 99994 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 99994 times.
✗ Branch 8 not taken.
 199988 else { sum_b += b * k.at(i - 1); }
186 // sum_b_str += fmt::format("b{} * k{}", i, i);
187 // sum_b_val += fmt::format("{:.2f} * k{}", butcherTableau[s].at(i), i);
188 // if (i < s)
189 // {
190 // sum_b_str += " + ";
191 // sum_b_val += " + ";
192 // }
193 }
194
195 // std::cout << result + fmt::format(" + h * ({})", sum_b_str) << std::endl;
196 // std::cout << result + fmt::format(" + h * ({})", sum_b_val) << std::endl;
197
2/4
✓ Branch 1 taken 49997 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 49997 times.
✗ Branch 5 not taken.
 99994 return applyDerivative(h * sum_b);
198 }
199
200 #if defined(__GNUC__) && !defined(__clang__)
201 #pragma GCC diagnostic pop
202 #endif
203
204 /// @brief Butcher tableau for Runge-Kutta 1st order (explicit) / (Forward) Euler method
205 template<typename Scalar>
206 constexpr std::array<std::array<Scalar, 2>, 2> RK1 = { { { 0.0, /*|*/ },
207 //------------------
208 { 0.0, /*|*/ 1.0 } } };
209
210 /// @brief Butcher tableau for Runge-Kutta 2nd order (explicit) / Explicit midpoint method
211 template<typename Scalar>
212 constexpr std::array<std::array<Scalar, 3>, 3> RK2 = { { { 0.0, /*|*/ },
213 { 0.5, /*|*/ 0.5 },
214 //-----------------------
215 { 0.0, /*|*/ 0.0, 1.0 } } };
216
217 /// @brief Butcher tableau for Heun's method (2nd order) (explicit)
218 template<typename Scalar>
219 constexpr std::array<std::array<Scalar, 3>, 3> Heun2 = { { { 0.0, /*|*/ },
220 { 1.0, /*|*/ 1.0 },
221 //-----------------------
222 { 0.0, /*|*/ 0.5, 0.5 } } };
223
224 /// @brief Butcher tableau for Runge-Kutta 3rd order (explicit) / Simpson's rule
225 template<typename Scalar>
226 constexpr std::array<std::array<Scalar, 4>, 4> RK3 = { { { 0.0, /*|*/ },
227 { 0.5, /*|*/ 0.5 },
228 { 1.0, /*|*/ -1.0, 2.0 },
229 //----------------------------------------------
230 { 0.0, /*|*/ 1.0 / 6.0, 4.0 / 6.0, 1.0 / 6.0 } } };
231
232 /// @brief Butcher tableau for Heun's method (3nd order) (explicit)
233 template<typename Scalar>
234 constexpr std::array<std::array<Scalar, 4>, 4> Heun3 = { { { 0.0, /* |*/ },
235 { 1.0 / 3.0, /*|*/ 1.0 / 3.0 },
236 { 2.0 / 3.0, /*|*/ 0.0, 2.0 / 3.0 },
237 //----------------------------------------
238 { 0.0, /*|*/ 1.0 / 4.0, 0.0, 3.0 / 4.0 } } };
239
240 /// @brief Butcher tableau for Runge-Kutta 4th order (explicit)
241 template<typename Scalar>
242 constexpr std::array<std::array<Scalar, 5>, 5> RK4 = { { { 0.0, /*|*/ },
243 { 0.5, /*|*/ 0.5 },
244 { 0.5, /*|*/ 0.0, 0.5 },
245 { 1.0, /*|*/ 0.0, 0.0, 1.0 },
246 //------------------
247 { 0.0, /*|*/ 1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0 } } };
248
249 } // namespace ButcherTableau
250
251 /// @brief Runge-Kutta 1st order (explicit) / (Forward) Euler method
252 /// \anchor eq-RungeKutta1-explicit \f{equation}{ \label{eq:eq-RungeKutta1-explicit}
253 /// y_{n+1} = y_n + h f(t_n, y_n)
254 /// \f}
255 /// Butcher tableau:
256 /// \anchor eq-RungeKutta1-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta1-explicit-bt}
257 /// \renewcommand\arraystretch{1.2}
258 /// \begin{array}{c|c}
259 /// 0 \\ \hline
260 /// & 1 \\
261 /// \end{array}
262 /// \f}
263 /// @param[in] y_n State vector at time t_n
264 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
265 /// @param[in] h Integration step in [s]
266 /// @param[in] f Time derivative function
267 /// @param[in] constParam Constant parameters passed to each time derivative function call
268 /// @param[in] t_n Time t_n
269 template<typename Y, typename Z, std::floating_point Scalar>
270 Y RungeKutta1(const Y& y_n, const std::array<Z, 1>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
271 {
272 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 1, ButcherTableau::RK1<Scalar>>(y_n, z, h, f, constParam, t_n);
273 }
274
275 /// @brief Runge-Kutta 1st order (explicit) / (Forward) Euler method with Quaternion as last entry in state
276 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
277 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
278 /// @param[in] h Integration step in [s]
279 /// @param[in] f Time derivative function
280 /// @param[in] constParam Constant parameters passed to each time derivative function call
281 /// @param[in] t_n Time t_n
282 template<typename Y, typename Z, std::floating_point Scalar>
283 ✗ Y RungeKutta1Quat(const Y& y_n, const std::array<Z, 1>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
284 {
285 ✗ return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 1, ButcherTableau::RK1<Scalar>>(y_n, z, h, f, constParam, t_n);
286 }
287
288 /// @brief Runge-Kutta 2nd order (explicit) / Explicit midpoint method
289 /// \anchor eq-RungeKutta2-explicit \f{equation}{ \label{eq:eq-RungeKutta2-explicit}
290 /// \begin{aligned}
291 /// y_{n+1} &= y_n + h k_2 \\[1em]
292 /// k_1 &= f(t_n, y_n) \\
293 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
294 /// \end{aligned}
295 /// \f}
296 /// Butcher tableau:
297 /// \anchor eq-RungeKutta2-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta2-explicit-bt}
298 /// \renewcommand\arraystretch{1.2}
299 /// \begin{array}{c|cc}
300 /// 0 \\
301 /// \frac{1}{2} & \frac{1}{2} \\ \hline
302 /// & 0 & 1 \\
303 /// \end{array}
304 /// \f}
305 /// @param[in] y_n State vector at time t_n
306 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
307 /// @param[in] h Integration step in [s]
308 /// @param[in] f Time derivative function
309 /// @param[in] constParam Constant parameters passed to each time derivative function call
310 /// @param[in] t_n Time t_n
311 template<typename Y, typename Z, std::floating_point Scalar>
312 Y RungeKutta2(const Y& y_n, const std::array<Z, 2>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
313 {
314 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::RK2<Scalar>>(y_n, z, h, f, constParam, t_n);
315 }
316
317 /// @brief Runge-Kutta 2nd order (explicit) / Explicit midpoint method with Quaternion as last entry in state
318 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
319 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
320 /// @param[in] h Integration step in [s]
321 /// @param[in] f Time derivative function
322 /// @param[in] constParam Constant parameters passed to each time derivative function call
323 /// @param[in] t_n Time t_n
324 template<typename Y, typename Z, std::floating_point Scalar>
325 ✗ Y RungeKutta2Quat(const Y& y_n, const std::array<Z, 2>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
326 {
327 ✗ return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 2, ButcherTableau::RK2<Scalar>>(y_n, z, h, f, constParam, t_n);
328 }
329
330 /// @brief Heun's method (2nd order) (explicit)
331 /// \anchor eq-Heun2 \f{equation}{ \label{eq:eq-Heun2}
332 /// \begin{aligned}
333 /// y_{n+1} &= y_n + \frac{h}{2} (k_1 + k_2) \\[1em]
334 /// k_1 &= f(t_n, y_n) \\
335 /// k_2 &= f(t_n + h, y_n + h k_1) \\
336 /// \end{aligned}
337 /// \f}
338 /// Butcher tableau:
339 /// \anchor eq-Heun2-bt \f{equation}{ \label{eq:eq-Heun2-bt}
340 /// \renewcommand\arraystretch{1.2}
341 /// \begin{array}{c|cc}
342 /// 0 \\
343 /// 1 & 1 \\ \hline
344 /// & \frac{1}{2} & \frac{1}{2} \\
345 /// \end{array}
346 /// \f}
347 /// @param[in] y_n State vector at time t_n
348 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
349 /// @param[in] h Integration step in [s]
350 /// @param[in] f Time derivative function
351 /// @param[in] constParam Constant parameters passed to each time derivative function call
352 /// @param[in] t_n Time t_n
353 template<typename Y, typename Z, std::floating_point Scalar>
354 Y Heun2(const Y& y_n, const std::array<Z, 2>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
355 {
356 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::Heun2<Scalar>>(y_n, z, h, f, constParam, t_n);
357 }
358
359 /// @brief Heun's method (2nd order) (explicit) with Quaternion as last entry in state
360 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
361 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
362 /// @param[in] h Integration step in [s]
363 /// @param[in] f Time derivative function
364 /// @param[in] constParam Constant parameters passed to each time derivative function call
365 /// @param[in] t_n Time t_n
366 template<typename Y, typename Z, std::floating_point Scalar>
367 ✗ Y Heun2Quat(const Y& y_n, const std::array<Z, 2>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
368 {
369 ✗ return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 2, ButcherTableau::Heun2<Scalar>>(y_n, z, h, f, constParam, t_n);
370 }
371
372 /// @brief Runge-Kutta 3rd order (explicit) / Simpson's rule
373 /// \anchor eq-RungeKutta3-explicit \f{equation}{ \label{eq:eq-RungeKutta3-explicit}
374 /// \begin{aligned}
375 /// y_{n+1} &= y_n + \frac{h}{6} ( k_1 + 4 k_2 + k_3 ) \\[1em]
376 /// k_1 &= f(t_n, y_n) \\
377 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
378 /// k_3 &= f(t_n + h, y_n + h (-k_1 + 2 k_2)) \\
379 /// \end{aligned}
380 /// \f}
381 /// Butcher tableau:
382 /// \anchor eq-RungeKutta3-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta3-explicit-bt}
383 /// \renewcommand\arraystretch{1.2}
384 /// \begin{array}{c|ccc}
385 /// 0 \\
386 /// \frac{1}{2} & \frac{1}{2} \\
387 /// 1 & -1 & 2 \\ \hline
388 /// & \frac{1}{6} & \frac{4}{6} & \frac{1}{6} \\
389 /// \end{array}
390 /// \f}
391 /// @param[in] y_n State vector at time t_n
392 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
393 /// @param[in] h Integration step in [s]
394 /// @param[in] f Time derivative function
395 /// @param[in] constParam Constant parameters passed to each time derivative function call
396 /// @param[in] t_n Time t_n
397 template<typename Y, typename Z, std::floating_point Scalar>
398 Y RungeKutta3(const Y& y_n, const std::array<Z, 3>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
399 {
400 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::RK3<Scalar>>(y_n, z, h, f, constParam, t_n);
401 }
402
403 /// @brief Runge-Kutta 3rd order (explicit) / Simpson's rule with Quaternion as last entry in state
404 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
405 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
406 /// @param[in] h Integration step in [s]
407 /// @param[in] f Time derivative function
408 /// @param[in] constParam Constant parameters passed to each time derivative function call
409 /// @param[in] t_n Time t_n
410 template<typename Y, typename Z, std::floating_point Scalar>
411 49997 Y RungeKutta3Quat(const Y& y_n, const std::array<Z, 3>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
412 {
413 49997 return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 3, ButcherTableau::RK3<Scalar>>(y_n, z, h, f, constParam, t_n);
414 }
415
416 /// @brief Heun's method (3nd order) (explicit)
417 /// \anchor eq-Heun3 \f{equation}{ \label{eq:eq-Heun3}
418 /// \begin{aligned}
419 /// y_{n+1} &= y_n + \frac{h}{4} (k_1 + 3 k_3) \\[1em]
420 /// k_1 &= f(t_n, y_n) \\
421 /// k_2 &= f(t_n + \frac{h}{3}, y_n + \frac{h}{3} k_1) \\
422 /// k_3 &= f(t_n + \frac{2 h}{3}, y_n + \frac{2 h}{3} k_2) \\
423 /// \end{aligned}
424 /// \f}
425 /// Butcher tableau:
426 /// \anchor eq-Heun3-bt \f{equation}{ \label{eq:eq-Heun3-bt}
427 /// \renewcommand\arraystretch{1.2}
428 /// \begin{array}{c|ccc}
429 /// 0 \\
430 /// \frac{1}{3} & \frac{1}{3} \\
431 /// \frac{2}{3} & 0 & \frac{2}{3} \\ \hline
432 /// & \frac{1}{4} & 0 & \frac{3}{4} \\
433 /// \end{array}
434 /// \f}
435 /// @param[in] y_n State vector at time t_n
436 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
437 /// @param[in] h Integration step in [s]
438 /// @param[in] f Time derivative function
439 /// @param[in] constParam Constant parameters passed to each time derivative function call
440 /// @param[in] t_n Time t_n
441 template<typename Y, typename Z, std::floating_point Scalar>
442 Y Heun3(const Y& y_n, const std::array<Z, 3>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
443 {
444 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::Heun3<Scalar>>(y_n, z, h, f, constParam, t_n);
445 }
446
447 /// @brief Heun's method (3nd order) (explicit) with Quaternion as last entry in state
448 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
449 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
450 /// @param[in] h Integration step in [s]
451 /// @param[in] f Time derivative function
452 /// @param[in] constParam Constant parameters passed to each time derivative function call
453 /// @param[in] t_n Time t_n
454 template<typename Y, typename Z, std::floating_point Scalar>
455 Y Heun3Quat(const Y& y_n, const std::array<Z, 3>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
456 {
457 return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 3, ButcherTableau::Heun3<Scalar>>(y_n, z, h, f, constParam, t_n);
458 }
459
460 /// @brief Runge-Kutta 4th order (explicit)
461 /// \anchor eq-RungeKutta4-explicit \f{equation}{ \label{eq:eq-RungeKutta4-explicit}
462 /// \begin{aligned}
463 /// y_{n+1} &= y_n + \frac{h}{6} ( k_1 + 2 k_2 + 2 k_3 + k_4 ) \\[1em]
464 /// k_1 &= f(t_n, y_n) \\
465 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
466 /// k_3 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_2) \\
467 /// k_4 &= f(t_n + h, y_n + h k_3) \\
468 /// \end{aligned}
469 /// \f}
470 /// Butcher tableau:
471 /// \anchor eq-RungeKutta4-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta4-explicit-bt}
472 /// \renewcommand\arraystretch{1.2}
473 /// \begin{array}{c|cccc}
474 /// 0 \\
475 /// \frac{1}{2} & \frac{1}{2} \\
476 /// \frac{1}{2} & 0 & \frac{1}{2} \\
477 /// 1 & 0 & 0 & 1 \\ \hline
478 /// & \frac{1}{6} & \frac{1}{3} & \frac{1}{3} & \frac{1}{6} \\
479 /// \end{array}
480 /// \f}
481 /// @param[in] y_n State vector at time t_n
482 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
483 /// @param[in] h Integration step in [s]
484 /// @param[in] f Time derivative function
485 /// @param[in] constParam Constant parameters passed to each time derivative function call
486 /// @param[in] t_n Time t_n
487 template<typename Y, typename Z, std::floating_point Scalar>
488 18630 Y RungeKutta4(const Y& y_n, const std::array<Z, 4>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
489 {
490 18630 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 4, ButcherTableau::RK4<Scalar>>(y_n, z, h, f, constParam, t_n);
491 }
492
493 /// @brief Runge-Kutta 4th order (explicit) with Quaternion as last entry in state
494 /// @param[in] y_n State vector at time t_n (last 4 entries are treated as a quaternion)
495 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
496 /// @param[in] h Integration step in [s]
497 /// @param[in] f Time derivative function
498 /// @param[in] constParam Constant parameters passed to each time derivative function call
499 /// @param[in] t_n Time t_n
500 template<typename Y, typename Z, std::floating_point Scalar>
501 ✗ Y RungeKutta4Quat(const Y& y_n, const std::array<Z, 4>& z, const Scalar& h, const auto& f, const auto& constParam, const Scalar& t_n = 0)
502 {
503 ✗ return ButcherTableau::RungeKuttaExplicitQuat<Y, Z, Scalar, 4, ButcherTableau::RK4<Scalar>>(y_n, z, h, f, constParam, t_n);
504 }
505
506 } // namespace NAV
507