INSTINCT Code Coverage Report

Directory: src/
File: Navigation/Math/NumericalIntegration.hpp
Date: 2025-02-07 16:54:41
Exec Total Coverage
Lines: 23 29 79.3%
Functions: 4 12 33.3%
Branches: 50 84 59.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 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 <numeric>
18 #include <type_traits>
19 #include <gcem.hpp>
20
21 #include "util/Assert.h"
22 #include "util/Logger.hpp"
23
24 namespace NAV
25 {
26
27 namespace ButcherTableau
28 {
29
30 #if defined(__GNUC__) && !defined(__clang__)
31 #pragma GCC diagnostic push
32 #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" // NOLINT(clang-diagnostic-unknown-warning-option)
33 #endif
34
35 /// @brief Calculates explicit Runge-Kutta methods. Order is defined by the Butcher tableau
36 /// @param[in] y_n State vector at time t_n
37 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
38 /// @param[in] h Integration step in [s]
39 /// @param[in] f Time derivative function
40 /// @param[in] constParam Constant parameters passed to each time derivative function call
41 /// @param[in] t_n Time t_n
42 /// @return State vector at time t_(n+1)
43 template<typename Y, typename Z, std::floating_point Scalar, size_t s, std::array<std::array<Scalar, s + 1>, s + 1> butcherTableau>
44 120194 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)
45 {
46 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
47 "The sum of the last row in the Butcher tableau has to be 1");
48
2/2
✓ Branch 0 taken 296278 times.
✓ Branch 1 taken 69412 times.
 619600 for (size_t r = 0; r <= s; ++r)
49 {
50
2/2
✓ Branch 0 taken 490992 times.
✓ Branch 1 taken 296278 times.
 1295090 for (size_t c = r + 1; c <= s; ++c)
51 {
52
3/6
✓ Branch 1 taken 490992 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 490992 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 490992 times.
✗ Branch 7 not taken.
 795684 INS_ASSERT_USER_ERROR(butcherTableau.at(r).at(c) == 0.0, "All terms in the upper triangle have to be 0");
53 }
54 }
55
56 // std::string result = "y_(n+1) = y_n";
57
58 // std::string sum_b_str;
59 // std::string sum_b_val;
60
1/2
✓ Branch 1 taken 69412 times.
✗ Branch 2 not taken.
 120194 Y sum_b{};
61
3/4
✓ Branch 1 taken 226866 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 226866 times.
✓ Branch 4 taken 69412 times.
 499406 std::array<Y, s> k{};
62
2/2
✓ Branch 0 taken 226866 times.
✓ Branch 1 taken 69412 times.
 499406 for (size_t i = 1; i <= s; ++i)
63 {
64
1/2
✓ Branch 2 taken 226866 times.
✗ Branch 3 not taken.
 379212 auto b = butcherTableau[s].at(i);
65
1/2
✓ Branch 1 taken 226866 times.
✗ Branch 2 not taken.
 379212 auto c = butcherTableau.at(i - 1)[0];
66
1/2
✓ Branch 1 taken 226866 times.
✗ Branch 2 not taken.
 379212 Y sum_a{};
67 // std::string sum_a_str;
68 // std::string sum_a_val;
69
2/2
✓ Branch 0 taken 264126 times.
✓ Branch 1 taken 226866 times.
 795684 for (size_t j = 2; j <= i; ++j)
70 {
71
2/4
✓ Branch 1 taken 264126 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 264126 times.
✗ Branch 5 not taken.
 416472 auto a = butcherTableau.at(i - 1).at(j - 1);
72
5/8
✓ Branch 0 taken 157454 times.
✓ Branch 1 taken 106672 times.
✓ Branch 3 taken 157454 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 157454 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 157454 times.
✗ Branch 10 not taken.
 416472 if (j == 2) { sum_a = a * k.at(j - 2); }
73
3/6
✓ Branch 1 taken 106672 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 106672 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 106672 times.
✗ Branch 8 not taken.
 157454 else { sum_a += a * k.at(j - 2); }
74 // sum_a_str += fmt::format("a{}{} * k{}", i, j-1, j-1);
75 // sum_a_val += fmt::format("{:^3.1f} * k{}", a, j - 1);
76 // if (j < i) {
77 // sum_a_str += " + ";
78 // sum_a_val += " + ";
79 // }
80 }
81
82
5/8
✓ Branch 0 taken 69412 times.
✓ Branch 1 taken 157454 times.
✓ Branch 3 taken 69412 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 69412 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 69412 times.
✗ Branch 10 not taken.
 379212 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
83
6/12
✓ Branch 1 taken 157454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 157454 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 157454 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 157454 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 157454 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 157454 times.
✗ Branch 17 not taken.
 259018 else { k.at(i - 1) = f(y_n + sum_a * h, z.at(i - 1), constParam, t_n + c * h); }
84 // if (sum_a_str.empty()) { sum_a_str = "0"; }
85 // if (sum_a_val.empty()) { sum_a_val = "0"; }
86 // fmt::println("k{} = f(t_n + c{} * h, y_n + ({}) * h)", i, i, sum_a_str);
87 // 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()));
88
89
5/8
✓ Branch 0 taken 69412 times.
✓ Branch 1 taken 157454 times.
✓ Branch 3 taken 69412 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 69412 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 69412 times.
✗ Branch 10 not taken.
 379212 if (i == 1) { sum_b = b * k.at(i - 1); }
90
3/6
✓ Branch 1 taken 157454 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 157454 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 157454 times.
✗ Branch 8 not taken.
 259018 else { sum_b += b * k.at(i - 1); }
91 // sum_b_str += fmt::format("b{} * k{}", i, i);
92 // sum_b_val += fmt::format("{:.2f} * k{}", butcherTableau[s].at(i), i);
93 // if (i < s)
94 // {
95 // sum_b_str += " + ";
96 // sum_b_val += " + ";
97 // }
98 }
99
100 // std::cout << result + fmt::format(" + h * ({})", sum_b_str) << std::endl;
101 // std::cout << result + fmt::format(" + h * ({})", sum_b_val) << std::endl;
102
3/6
✓ Branch 1 taken 69412 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 69412 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 69412 times.
✗ Branch 8 not taken.
 120194 return y_n + h * sum_b;
103 }
104
105 #if defined(__GNUC__) && !defined(__clang__)
106 #pragma GCC diagnostic pop
107 #endif
108
109 /// @brief Butcher tableau for Runge-Kutta 1st order (explicit) / (Forward) Euler method
110 template<typename Scalar>
111 constexpr std::array<std::array<Scalar, 2>, 2> RK1 = { { { 0.0, /*|*/ },
112 //------------------
113 { 0.0, /*|*/ 1.0 } } };
114
115 /// @brief Butcher tableau for Runge-Kutta 2nd order (explicit) / Explicit midpoint method
116 template<typename Scalar>
117 constexpr std::array<std::array<Scalar, 3>, 3> RK2 = { { { 0.0, /*|*/ },
118 { 0.5, /*|*/ 0.5 },
119 //-----------------------
120 { 0.0, /*|*/ 0.0, 1.0 } } };
121
122 /// @brief Butcher tableau for Heun's method (2nd order) (explicit)
123 template<typename Scalar>
124 constexpr std::array<std::array<Scalar, 3>, 3> Heun2 = { { { 0.0, /*|*/ },
125 { 1.0, /*|*/ 1.0 },
126 //-----------------------
127 { 0.0, /*|*/ 0.5, 0.5 } } };
128
129 /// @brief Butcher tableau for Runge-Kutta 3rd order (explicit) / Simpson's rule
130 template<typename Scalar>
131 constexpr std::array<std::array<Scalar, 4>, 4> RK3 = { { { 0.0, /*|*/ },
132 { 0.5, /*|*/ 0.5 },
133 { 1.0, /*|*/ -1.0, 2.0 },
134 //----------------------------------------------
135 { 0.0, /*|*/ 1.0 / 6.0, 4.0 / 6.0, 1.0 / 6.0 } } };
136
137 /// @brief Butcher tableau for Heun's method (3nd order) (explicit)
138 template<typename Scalar>
139 constexpr std::array<std::array<Scalar, 4>, 4> Heun3 = { { { 0.0, /* |*/ },
140 { 1.0 / 3.0, /*|*/ 1.0 / 3.0 },
141 { 2.0 / 3.0, /*|*/ 0.0, 2.0 / 3.0 },
142 //----------------------------------------
143 { 0.0, /*|*/ 1.0 / 4.0, 0.0, 3.0 / 4.0 } } };
144
145 /// @brief Butcher tableau for Runge-Kutta 4th order (explicit)
146 template<typename Scalar>
147 constexpr std::array<std::array<Scalar, 5>, 5> RK4 = { { { 0.0, /*|*/ },
148 { 0.5, /*|*/ 0.5 },
149 { 0.5, /*|*/ 0.0, 0.5 },
150 { 1.0, /*|*/ 0.0, 0.0, 1.0 },
151 //------------------
152 { 0.0, /*|*/ 1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0 } } };
153
154 } // namespace ButcherTableau
155
156 /// @brief Runge-Kutta 1st order (explicit) / (Forward) Euler method
157 /// \anchor eq-RungeKutta1-explicit \f{equation}{ \label{eq:eq-RungeKutta1-explicit}
158 /// y_{n+1} = y_n + h f(t_n, y_n)
159 /// \f}
160 /// Butcher tableau:
161 /// \anchor eq-RungeKutta1-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta1-explicit-bt}
162 /// \renewcommand\arraystretch{1.2}
163 /// \begin{array}{c|c}
164 /// 0 \\ \hline
165 /// & 1 \\
166 /// \end{array}
167 /// \f}
168 /// @param[in] y_n State vector at time t_n
169 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
170 /// @param[in] h Integration step in [s]
171 /// @param[in] f Time derivative function
172 /// @param[in] constParam Constant parameters passed to each time derivative function call
173 /// @param[in] t_n Time t_n
174 template<typename Y, typename Z, std::floating_point Scalar>
175 ✗ 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)
176 {
177 ✗ return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 1, ButcherTableau::RK1<Scalar>>(y_n, z, h, f, constParam, t_n);
178 }
179
180 /// @brief Runge-Kutta 2nd order (explicit) / Explicit midpoint method
181 /// \anchor eq-RungeKutta2-explicit \f{equation}{ \label{eq:eq-RungeKutta2-explicit}
182 /// \begin{aligned}
183 /// y_{n+1} &= y_n + h k_2 \\[1em]
184 /// k_1 &= f(t_n, y_n) \\
185 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
186 /// \end{aligned}
187 /// \f}
188 /// Butcher tableau:
189 /// \anchor eq-RungeKutta2-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta2-explicit-bt}
190 /// \renewcommand\arraystretch{1.2}
191 /// \begin{array}{c|cc}
192 /// 0 \\
193 /// \frac{1}{2} & \frac{1}{2} \\ \hline
194 /// & 0 & 1 \\
195 /// \end{array}
196 /// \f}
197 /// @param[in] y_n State vector at time t_n
198 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
199 /// @param[in] h Integration step in [s]
200 /// @param[in] f Time derivative function
201 /// @param[in] constParam Constant parameters passed to each time derivative function call
202 /// @param[in] t_n Time t_n
203 template<typename Y, typename Z, std::floating_point Scalar>
204 ✗ 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)
205 {
206 ✗ return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::RK2<Scalar>>(y_n, z, h, f, constParam, t_n);
207 }
208
209 /// @brief Heun's method (2nd order) (explicit)
210 /// \anchor eq-Heun2 \f{equation}{ \label{eq:eq-Heun2}
211 /// \begin{aligned}
212 /// y_{n+1} &= y_n + \frac{h}{2} (k_1 + k_2) \\[1em]
213 /// k_1 &= f(t_n, y_n) \\
214 /// k_2 &= f(t_n + h, y_n + h k_1) \\
215 /// \end{aligned}
216 /// \f}
217 /// Butcher tableau:
218 /// \anchor eq-Heun2-bt \f{equation}{ \label{eq:eq-Heun2-bt}
219 /// \renewcommand\arraystretch{1.2}
220 /// \begin{array}{c|cc}
221 /// 0 \\
222 /// 1 & 1 \\ \hline
223 /// & \frac{1}{2} & \frac{1}{2} \\
224 /// \end{array}
225 /// \f}
226 /// @param[in] y_n State vector at time t_n
227 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
228 /// @param[in] h Integration step in [s]
229 /// @param[in] f Time derivative function
230 /// @param[in] constParam Constant parameters passed to each time derivative function call
231 /// @param[in] t_n Time t_n
232 template<typename Y, typename Z, std::floating_point Scalar>
233 ✗ 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)
234 {
235 ✗ return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::Heun2<Scalar>>(y_n, z, h, f, constParam, t_n);
236 }
237
238 /// @brief Runge-Kutta 3rd order (explicit) / Simpson's rule
239 /// \anchor eq-RungeKutta3-explicit \f{equation}{ \label{eq:eq-RungeKutta3-explicit}
240 /// \begin{aligned}
241 /// y_{n+1} &= y_n + \frac{h}{6} ( k_1 + 4 k_2 + k_3 ) \\[1em]
242 /// k_1 &= f(t_n, y_n) \\
243 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
244 /// k_3 &= f(t_n + h, y_n + h (-k_1 + 2 k_2)) \\
245 /// \end{aligned}
246 /// \f}
247 /// Butcher tableau:
248 /// \anchor eq-RungeKutta3-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta3-explicit-bt}
249 /// \renewcommand\arraystretch{1.2}
250 /// \begin{array}{c|ccc}
251 /// 0 \\
252 /// \frac{1}{2} & \frac{1}{2} \\
253 /// 1 & -1 & 2 \\ \hline
254 /// & \frac{1}{6} & \frac{4}{6} & \frac{1}{6} \\
255 /// \end{array}
256 /// \f}
257 /// @param[in] y_n State vector at time t_n
258 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
259 /// @param[in] h Integration step in [s]
260 /// @param[in] f Time derivative function
261 /// @param[in] constParam Constant parameters passed to each time derivative function call
262 /// @param[in] t_n Time t_n
263 template<typename Y, typename Z, std::floating_point Scalar>
264 50782 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)
265 {
266 50782 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::RK3<Scalar>>(y_n, z, h, f, constParam, t_n);
267 }
268
269 /// @brief Heun's method (3nd order) (explicit)
270 /// \anchor eq-Heun3 \f{equation}{ \label{eq:eq-Heun3}
271 /// \begin{aligned}
272 /// y_{n+1} &= y_n + \frac{h}{4} (k_1 + 3 k_3) \\[1em]
273 /// k_1 &= f(t_n, y_n) \\
274 /// k_2 &= f(t_n + \frac{h}{3}, y_n + \frac{h}{3} k_1) \\
275 /// k_3 &= f(t_n + \frac{2 h}{3}, y_n + \frac{2 h}{3} k_2) \\
276 /// \end{aligned}
277 /// \f}
278 /// Butcher tableau:
279 /// \anchor eq-Heun3-bt \f{equation}{ \label{eq:eq-Heun3-bt}
280 /// \renewcommand\arraystretch{1.2}
281 /// \begin{array}{c|ccc}
282 /// 0 \\
283 /// \frac{1}{3} & \frac{1}{3} \\
284 /// \frac{2}{3} & 0 & \frac{2}{3} \\ \hline
285 /// & \frac{1}{4} & 0 & \frac{3}{4} \\
286 /// \end{array}
287 /// \f}
288 /// @param[in] y_n State vector at time t_n
289 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
290 /// @param[in] h Integration step in [s]
291 /// @param[in] f Time derivative function
292 /// @param[in] constParam Constant parameters passed to each time derivative function call
293 /// @param[in] t_n Time t_n
294 template<typename Y, typename Z, std::floating_point Scalar>
295 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)
296 {
297 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::Heun3<Scalar>>(y_n, z, h, f, constParam, t_n);
298 }
299
300 /// @brief Runge-Kutta 4th order (explicit)
301 /// \anchor eq-RungeKutta4-explicit \f{equation}{ \label{eq:eq-RungeKutta4-explicit}
302 /// \begin{aligned}
303 /// y_{n+1} &= y_n + \frac{h}{6} ( k_1 + 2 k_2 + 2 k_3 + k_4 ) \\[1em]
304 /// k_1 &= f(t_n, y_n) \\
305 /// k_2 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_1) \\
306 /// k_3 &= f(t_n + \frac{h}{2}, y_n + \frac{h}{2} k_2) \\
307 /// k_4 &= f(t_n + h, y_n + h k_3) \\
308 /// \end{aligned}
309 /// \f}
310 /// Butcher tableau:
311 /// \anchor eq-RungeKutta4-explicit-bt \f{equation}{ \label{eq:eq-RungeKutta4-explicit-bt}
312 /// \renewcommand\arraystretch{1.2}
313 /// \begin{array}{c|cccc}
314 /// 0 \\
315 /// \frac{1}{2} & \frac{1}{2} \\
316 /// \frac{1}{2} & 0 & \frac{1}{2} \\
317 /// 1 & 0 & 0 & 1 \\ \hline
318 /// & \frac{1}{6} & \frac{1}{3} & \frac{1}{3} & \frac{1}{6} \\
319 /// \end{array}
320 /// \f}
321 /// @param[in] y_n State vector at time t_n
322 /// @param[in] z Array of measurements, one for each evaluation point of the Runge Kutta
323 /// @param[in] h Integration step in [s]
324 /// @param[in] f Time derivative function
325 /// @param[in] constParam Constant parameters passed to each time derivative function call
326 /// @param[in] t_n Time t_n
327 template<typename Y, typename Z, std::floating_point Scalar>
328 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)
329 {
330 18630 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 4, ButcherTableau::RK4<Scalar>>(y_n, z, h, f, constParam, t_n);
331 }
332
333 } // namespace NAV
334