NumericalIntegration.hpp

Go to the documentation of this file.

// This file is part of INSTINCT, the INS Toolkit for Integrated
// Navigation Concepts and Training by the Institute of Navigation of
// the University of Stuttgart, Germany.
//
// This Source Code Form is subject to the terms of the Mozilla Public
// License, v. 2.0. If a copy of the MPL was not distributed with this
// file, You can obtain one at https://mozilla.org/MPL/2.0/.
 
 
#pragma once
 
#include <array>
#include <numeric>
#include <type_traits>
#include <gcem.hpp>
 
#include "util/Assert.h"
#include "util/Logger.hpp"
 
namespace NAV
{
 
namespace ButcherTableau
{
 
#pragma GCC diagnostic push
#if !defined(__clang__)
    #pragma GCC diagnostic ignored "-Wmaybe-uninitialized" // NOLINT(clang-diagnostic-unknown-warning-option)
#endif
 
template<typename Y, typename Z, typename Scalar, size_t s, std::array<std::array<Scalar, s + 1>, s + 1> butcherTableau,
         typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    static_assert(gcem::abs(static_cast<Scalar>(1.0) - std::accumulate(butcherTableau[s].begin(), butcherTableau[s].end(), static_cast<Scalar>(0.0))) < 1e-8,
                  "The sum of the last row in the Butcher tableau has to be 1");
    for (size_t r = 0; r <= s; ++r)
    {
        for (size_t c = r + 1; c <= s; ++c)
        {
            INS_ASSERT_USER_ERROR(butcherTableau.at(r).at(c) == 0.0, "All terms in the upper triangle have to be 0");
        }
    }
 
    // std::string result = "y_(n+1) = y_n";
 
    // std::string sum_b_str;
    // std::string sum_b_val;
    Y sum_b{};
    std::array<Y, s> k{};
    for (size_t i = 1; i <= s; ++i)
    {
        auto b = butcherTableau[s].at(i);
        auto c = butcherTableau.at(i - 1)[0];
        Y sum_a{};
        // std::string sum_a_str;
        // std::string sum_a_val;
        for (size_t j = 2; j <= i; ++j)
        {
            auto a = butcherTableau.at(i - 1).at(j - 1);
            if (j == 2) { sum_a = a * k.at(j - 2); }
            else { sum_a += a * k.at(j - 2); }
            // sum_a_str += fmt::format("a{}{} * k{}", i, j-1, j-1);
            // sum_a_val += fmt::format("{:^3.1f} * k{}", a, j - 1);
            // if (j < i) {
            //     sum_a_str += " + ";
            //     sum_a_val += " + ";
            // }
        }
 
        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
        else { k.at(i - 1) = f(y_n + sum_a * h, z.at(i - 1), constParam, t_n + c * h); }
        // if (sum_a_str.empty()) { sum_a_str = "0"; }
        // if (sum_a_val.empty()) { sum_a_val = "0"; }
        // fmt::println("k{} = f(t_n +  c{} * h, y_n + ({}) * h)", i, i, sum_a_str);
        // 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()));
 
        if (i == 1) { sum_b = b * k.at(i - 1); }
        else { sum_b += b * k.at(i - 1); }
        // sum_b_str += fmt::format("b{} * k{}", i, i);
        // sum_b_val += fmt::format("{:.2f} * k{}", butcherTableau[s].at(i), i);
        // if (i < s)
        // {
        //     sum_b_str += " + ";
        //     sum_b_val += " + ";
        // }
    }
 
    // std::cout << result + fmt::format(" + h * ({})", sum_b_str) << std::endl;
    // std::cout << result + fmt::format(" + h * ({})", sum_b_val) << std::endl;
    return y_n + h * sum_b;
}
 
#pragma GCC diagnostic pop
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 2>, 2> RK1 = { { { 0.0, /*|*/ },
                                                         //------------------
                                                         { 0.0, /*|*/ 1.0 } } };
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 3>, 3> RK2 = { { { 0.0, /*|*/ },
                                                         { 0.5, /*|*/ 0.5 },
                                                         //-----------------------
                                                         { 0.0, /*|*/ 0.0, 1.0 } } };
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 3>, 3> Heun2 = { { { 0.0, /*|*/ },
                                                           { 1.0, /*|*/ 1.0 },
                                                           //-----------------------
                                                           { 0.0, /*|*/ 0.5, 0.5 } } };
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 4>, 4> RK3 = { { { 0.0, /*|*/ },
                                                         { 0.5, /*|*/ 0.5 },
                                                         { 1.0, /*|*/ -1.0, 2.0 },
                                                         //----------------------------------------------
                                                         { 0.0, /*|*/ 1.0 / 6.0, 4.0 / 6.0, 1.0 / 6.0 } } };
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 4>, 4> Heun3 = { { { 0.0, /*      |*/ },
                                                           { 1.0 / 3.0, /*|*/ 1.0 / 3.0 },
                                                           { 2.0 / 3.0, /*|*/ 0.0, 2.0 / 3.0 },
                                                           //----------------------------------------
                                                           { 0.0, /*|*/ 1.0 / 4.0, 0.0, 3.0 / 4.0 } } };
 
template<typename Scalar>
constexpr std::array<std::array<Scalar, 5>, 5> RK4 = { { { 0.0, /*|*/ },
                                                         { 0.5, /*|*/ 0.5 },
                                                         { 0.5, /*|*/ 0.0, 0.5 },
                                                         { 1.0, /*|*/ 0.0, 0.0, 1.0 },
                                                         //------------------
                                                         { 0.0, /*|*/ 1.0 / 6.0, 1.0 / 3.0, 1.0 / 3.0, 1.0 / 6.0 } } };
 
} // namespace ButcherTableau
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 1, ButcherTableau::RK1<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::RK2<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 2, ButcherTableau::Heun2<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::RK3<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 3, ButcherTableau::Heun3<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
template<typename Y, typename Z, typename Scalar, typename = std::enable_if_t<std::is_floating_point_v<Scalar>>>
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)
{
    return ButcherTableau::RungeKuttaExplicit<Y, Z, Scalar, 4, ButcherTableau::RK4<Scalar>>(y_n, z, h, f, constParam, t_n);
}
 
} // namespace NAV