Lodestar
An integrated real-time control package in C++
RungeKuttaFehlberg45.hpp
1 //
2 // Created by Hamza El-Kebir on 6/19/21.
3 //
4 
5 #ifndef LODESTAR_RUNGEKUTTAFEHLBERG45_HPP
6 #define LODESTAR_RUNGEKUTTAFEHLBERG45_HPP
7 
8 #include "ButcherTableau.hpp"
9 
10 namespace ls {
11  namespace primitives {
12  namespace detail {
13  class RungeKuttaFehlberg45Impl {
14  friend class RungeKuttaFehlberg45;
15 
16  protected:
23  RungeKuttaFehlberg45Impl()
24  {
25  btSimple.setNode<0>(1.0 / 4.0);
26  btSimple.setCoefficient<0, 0>(1.0 / 4.0);
27 
28  btSimple.setNode<1>(3.0 / 8.0);
29  btSimple.setCoefficient<1, 0>(3.0 / 32.0);
30  btSimple.setCoefficient<1, 1>(9.0 / 32.0);
31 
32  btSimple.setNode<2>(12.0 / 13.0);
33  btSimple.setCoefficient<2, 0>(1932.0 / 2197.0);
34  btSimple.setCoefficient<2, 1>(-7200.0 / 2197.0);
35  btSimple.setCoefficient<2, 2>(7296.0 / 2197.0);
36 
37  btSimple.setNode<3>(1.0);
38  btSimple.setCoefficient<3, 0>(439.0 / 216.0);
39  btSimple.setCoefficient<3, 1>(-8.0);
40  btSimple.setCoefficient<3, 2>(3680.0 / 513.0);
41  btSimple.setCoefficient<3, 3>(-845.0 / 4104.0);
42 
43  btSimple.setNode<4>(1.0 / 2.0);
44  btSimple.setCoefficient<4, 0>(-8.0 / 27.0);
45  btSimple.setCoefficient<4, 1>(2.0);
46  btSimple.setCoefficient<4, 2>(-3544.0 / 2565.0);
47  btSimple.setCoefficient<4, 3>(1859.0 / 4104.0);
48  btSimple.setCoefficient<4, 4>(-11.0 / 40.0);
49 
50  btSimple.setWeight<0>(16.0 / 135.0);
51  btSimple.setWeight<1>(0.0);
52  btSimple.setWeight<2>(6656.0 / 12825.0);
53  btSimple.setWeight<3>(28561.0 / 56430.0);
54  btSimple.setWeight<4>(-9.0 / 50.0);
55  btSimple.setWeight<5>(2.0 / 55.0);
56 
57  // Extended
58 
59  btExtended.setNode<0>(1.0 / 4.0);
60  btExtended.setCoefficient<0, 0>(1.0 / 4.0);
61 
62  btExtended.setNode<1>(3.0 / 8.0);
63  btExtended.setCoefficient<1, 0>(3.0 / 32.0);
64  btExtended.setCoefficient<1, 1>(9.0 / 32.0);
65 
66  btExtended.setNode<2>(12.0 / 13.0);
67  btExtended.setCoefficient<2, 0>(1932.0 / 2197.0);
68  btExtended.setCoefficient<2, 1>(-7200.0 / 2197.0);
69  btExtended.setCoefficient<2, 2>(7296.0 / 2197.0);
70 
71  btExtended.setNode<3>(1.0);
72  btExtended.setCoefficient<3, 0>(439.0 / 216.0);
73  btExtended.setCoefficient<3, 1>(-8.0);
74  btExtended.setCoefficient<3, 2>(3680.0 / 513.0);
75  btExtended.setCoefficient<3, 3>(-845.0 / 4104.0);
76 
77  btExtended.setNode<4>(1.0 / 2.0);
78  btExtended.setCoefficient<4, 0>(-8.0 / 27.0);
79  btExtended.setCoefficient<4, 1>(2.0);
80  btExtended.setCoefficient<4, 2>(-3544.0 / 2565.0);
81  btExtended.setCoefficient<4, 3>(1859.0 / 4104.0);
82  btExtended.setCoefficient<4, 4>(-11.0 / 40.0);
83 
84  btExtended.setWeight<0, true>(16.0 / 135.0);
85  btExtended.setWeight<1, true>(0.0);
86  btExtended.setWeight<2, true>(6656.0 / 12825.0);
87  btExtended.setWeight<3, true>(28561.0 / 56430.0);
88  btExtended.setWeight<4, true>(-9.0 / 50.0);
89  btExtended.setWeight<5, true>(2.0 / 55.0);
90 
91  btExtended.setWeight<0, false>(25.0 / 216.0);
92  btExtended.setWeight<1, false>(0.0);
93  btExtended.setWeight<2, false>(1408.0 / 2565.0);
94  btExtended.setWeight<3, false>(2197.0 / 4104.0);
95  btExtended.setWeight<4, false>(-1.0 / 5.0);
96  btExtended.setWeight<5, false>(0.0);
97  }
98 
99  ls::primitives::ButcherTableau<6, false> btSimple;
100  ls::primitives::ButcherTableau<6, true> btExtended;
101  };
102 
103  class RungeKuttaFehlberg45 {
104  public:
105  static ls::primitives::ButcherTableau<6, true> &getButcherTableau()
106  {
107  static RungeKuttaFehlberg45Impl *boshImpl = new RungeKuttaFehlberg45Impl();
108 
109  return boshImpl->btExtended;
110  }
111 
112  static ls::primitives::ButcherTableau<6, false> &getButcherTableauSimple()
113  {
114  static RungeKuttaFehlberg45Impl *boshImpl = new RungeKuttaFehlberg45Impl();
115 
116  return boshImpl->btSimple;
117  }
118 
119  private:
120  RungeKuttaFehlberg45() = default;
121 
122  // Delete copy/move so extra instances can't be created/moved.
123  RungeKuttaFehlberg45(const RungeKuttaFehlberg45 &) = delete;
124 
125  RungeKuttaFehlberg45 &operator=(const RungeKuttaFehlberg45 &) = delete;
126 
127  RungeKuttaFehlberg45(RungeKuttaFehlberg45 &&) = delete;
128 
129  RungeKuttaFehlberg45 &operator=(RungeKuttaFehlberg45 &&) = delete;
130  };
131  }
132 
133  template<typename TType, typename TScalarType = double>
134  class RungeKuttaFehlberg45 {
135  public:
136  typedef std::function<TType(TScalarType, TType)> TDFunction;
137 
138  static const short int kLowerErrorOrder = 4;
139  static const short int kLigherErrorOrder = 5;
140  static const short int kStages = 6;
141 
151  static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N = 1);
152 
164  static TType integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N = 1);
165 
166  // TODO: Add integrate function with maximum allowable error (adaptive step size).
167  };
168 
169  template<typename TType, typename TScalarType>
170  void RungeKuttaFehlberg45<TType, TScalarType>::integrateSimple(const TDFunction &f, TScalarType &t, TType &y,
171  const TScalarType h, size_t N)
172  {
173  for (int i = 0; i < N; i++) {
174  y = detail::RungeKuttaFehlberg45::getButcherTableauSimple().template execute(f, y, t, h);
175  t += h;
176  }
177  }
178 
179  template<typename TType, typename TScalarType>
180  TType RungeKuttaFehlberg45<TType, TScalarType>::integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y,
181  const TScalarType h, size_t N)
182  {
183  std::pair<TType, TType> ye;
184  for (int i = 0; i < N; i++) {
185  ye = detail::RungeKuttaFehlberg45::getButcherTableau().template execute<TType, 0, true>(f, y, t, h);
186  y = ye.first;
187  t += h;
188  }
189 
190  return ye.second;
191  }
192  }
193 }
194 
195 #endif //LODESTAR_RUNGEKUTTAFEHLBERG45_HPP
ls::primitives::detail::RungeKuttaFehlberg45Impl::RungeKuttaFehlberg45Impl
RungeKuttaFehlberg45Impl()
Constructor initializes Butcher tables.
Definition: RungeKuttaFehlberg45.hpp:23
ls::primitives::detail::RungeKuttaFehlberg45Impl
Definition: RungeKuttaFehlberg45.hpp:13
ls::primitives::ButcherTableau< 6, false >
ls::primitives::detail::RungeKuttaFehlberg45
Definition: RungeKuttaFehlberg45.hpp:103
ls::primitives::RungeKuttaFehlberg45
Definition: RungeKuttaFehlberg45.hpp:134
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::primitives::RungeKuttaFehlberg45::integrateSimple
static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Simple integration using the fifth-order scheme in the Runge-Kutta-Fehlberg method.
Definition: RungeKuttaFehlberg45.hpp:170
ls::primitives::RungeKuttaFehlberg45::integrateEmbedded
static TType integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Integrate using the Runge-Kutta-Fehlberg method (rk45) with truncation error output.
Definition: RungeKuttaFehlberg45.hpp:180