0.4.1
Loading...
Searching...
No Matches
NumericalIntegration.hpp
Go to the documentation of this file.
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
24namespace NAV
25{
26
27namespace 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)
43template<typename Y, typename Z, std::floating_point Scalar, size_t s, std::array<std::array<Scalar, s + 1>, s + 1> butcherTableau>
44inline 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 for (size_t r = 0; r <= s; ++r)
49 {
50 for (size_t c = r + 1; c <= s; ++c)
51 {
52 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 Y sum_b{};
61 std::array<Y, s> k{};
62 for (size_t i = 1; i <= s; ++i)
63 {
64 auto b = butcherTableau[s].at(i);
65 auto c = butcherTableau.at(i - 1)[0];
66 Y sum_a{};
67 // std::string sum_a_str;
68 // std::string sum_a_val;
69 for (size_t j = 2; j <= i; ++j)
70 {
71 auto a = butcherTableau.at(i - 1).at(j - 1);
72 if (j == 2) { sum_a = a * k.at(j - 2); }
73 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 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 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 if (i == 1) { sum_b = b * k.at(i - 1); }
90 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 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
110template<typename Scalar>
111constexpr 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
116template<typename Scalar>
117constexpr 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)
123template<typename Scalar>
124constexpr 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
130template<typename Scalar>
131constexpr 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)
138template<typename Scalar>
139constexpr 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)
146template<typename Scalar>
147constexpr 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
174template<typename Y, typename Z, std::floating_point Scalar>
175Y 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
203template<typename Y, typename Z, std::floating_point Scalar>
204Y 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
232template<typename Y, typename Z, std::floating_point Scalar>
233Y 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
263template<typename Y, typename Z, std::floating_point Scalar>
264Y 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 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
294template<typename Y, typename Z, std::floating_point Scalar>
295Y 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
327template<typename Y, typename Z, std::floating_point Scalar>
328Y 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 return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 4, ButcherTableau::RK4<Scalar>>(y_n, z, h, f, constParam, t_n);
331}
332
333} // namespace NAV
Assert.h
Assertion helpers.
INS_ASSERT_USER_ERROR
#define INS_ASSERT_USER_ERROR(_EXP, _MSG)
Assert function with message.
Definition Assert.h:21
Logger.hpp
Utility class for logging to console and file.
NAV::ButcherTableau::RK4
constexpr std::array< std::array< Scalar, 5 >, 5 > RK4
Butcher tableau for Runge-Kutta 4th order (explicit)
Definition NumericalIntegration.hpp:147
NAV::ButcherTableau::RungeKuttaExplicit
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)
Calculates explicit Runge-Kutta methods. Order is defined by the Butcher tableau.
Definition NumericalIntegration.hpp:44
NAV::ButcherTableau::Heun3
constexpr std::array< std::array< Scalar, 4 >, 4 > Heun3
Butcher tableau for Heun's method (3nd order) (explicit)
Definition NumericalIntegration.hpp:139
NAV::ButcherTableau::Heun2
constexpr std::array< std::array< Scalar, 3 >, 3 > Heun2
Butcher tableau for Heun's method (2nd order) (explicit)
Definition NumericalIntegration.hpp:124
NAV::ButcherTableau::RK3
constexpr std::array< std::array< Scalar, 4 >, 4 > RK3
Butcher tableau for Runge-Kutta 3rd order (explicit) / Simpson's rule.
Definition NumericalIntegration.hpp:131
NAV::ButcherTableau::RK2
constexpr std::array< std::array< Scalar, 3 >, 3 > RK2
Butcher tableau for Runge-Kutta 2nd order (explicit) / Explicit midpoint method.
Definition NumericalIntegration.hpp:117
NAV::ButcherTableau::RK1
constexpr std::array< std::array< Scalar, 2 >, 2 > RK1
Butcher tableau for Runge-Kutta 1st order (explicit) / (Forward) Euler method.
Definition NumericalIntegration.hpp:111
NAV::RungeKutta3
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)
Runge-Kutta 3rd order (explicit) / Simpson's rule .
Definition NumericalIntegration.hpp:264
NAV::Heun3
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)
Heun's method (3nd order) (explicit) .
Definition NumericalIntegration.hpp:295
NAV::RungeKutta1
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)
Runge-Kutta 1st order (explicit) / (Forward) Euler method .
Definition NumericalIntegration.hpp:175
NAV::RungeKutta4
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)
Runge-Kutta 4th order (explicit) .
Definition NumericalIntegration.hpp:328
NAV::RungeKutta2
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)
Runge-Kutta 2nd order (explicit) / Explicit midpoint method .
Definition NumericalIntegration.hpp:204
NAV::Heun2
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)
Heun's method (2nd order) (explicit) .
Definition NumericalIntegration.hpp:233