Lodestar
An integrated real-time control package in C++
RungeKuttaFehlberg78.hpp
1 //
2 // Created by Hamza El-Kebir on 6/19/21.
3 //
4 
5 #ifndef LODESTAR_RUNGEKUTTAFEHLBERG78_HPP
6 #define LODESTAR_RUNGEKUTTAFEHLBERG78_HPP
7 
8 #include "ButcherTableau.hpp"
9 
10 namespace ls {
11  namespace primitives {
12  namespace detail {
13  class RungeKuttaFehlberg78Impl {
14  friend class RungeKuttaFehlberg78;
15 
16  protected:
25  RungeKuttaFehlberg78Impl()
26  {
27  btSimple.setNode<0>(2.0 / 27.0);
28  btSimple.setCoefficient<0, 0>(2.0 / 27.0);
29 
30  btSimple.setNode<1>(1.0 / 9.0);
31  btSimple.setCoefficient<1, 0>(1.0 / 36.0);
32  btSimple.setCoefficient<1, 1>(1.0 / 12.0);
33 
34  btSimple.setNode<2>(1.0 / 6.0);
35  btSimple.setCoefficient<2, 0>(1.0 / 24.0);
36  btSimple.setCoefficient<2, 1>(0.0);
37  btSimple.setCoefficient<2, 2>(1.0 / 8.0);
38 
39  btSimple.setNode<3>(5.0 / 12.0);
40  btSimple.setCoefficient<3, 0>(5.0 / 12.0);
41  btSimple.setCoefficient<3, 1>(0.0);
42  btSimple.setCoefficient<3, 2>(-25.0 / 16.0);
43  btSimple.setCoefficient<3, 3>(25.0 / 16.0);
44 
45  btSimple.setNode<4>(1.0 / 2.0);
46  btSimple.setCoefficient<4, 0>(1.0 / 20.0);
47  btSimple.setCoefficient<4, 1>(0.0);
48  btSimple.setCoefficient<4, 2>(0.0);
49  btSimple.setCoefficient<4, 3>(1.0 / 4.0);
50  btSimple.setCoefficient<4, 4>(1.0 / 5.0);
51 
52  btSimple.setNode<5>(5.0 / 6.0);
53  btSimple.setCoefficient<5, 0>(-25.0 / 108.0);
54  btSimple.setCoefficient<5, 1>(0.0);
55  btSimple.setCoefficient<5, 2>(0.0);
56  btSimple.setCoefficient<5, 3>(125.0 / 108.0);
57  btSimple.setCoefficient<5, 4>(-65.0 / 27.0);
58  btSimple.setCoefficient<5, 5>(-125.0 / 54.0);
59 
60  btSimple.setNode<6>(1.0 / 6.0);
61  btSimple.setCoefficient<6, 0>(31.0 / 300.0);
62  btSimple.setCoefficient<6, 1>(0.0);
63  btSimple.setCoefficient<6, 2>(0.0);
64  btSimple.setCoefficient<6, 3>(0.0);
65  btSimple.setCoefficient<6, 4>(61.0 / 225.0);
66  btSimple.setCoefficient<6, 5>(-2.0 / 9.0);
67  btSimple.setCoefficient<6, 6>(13.0 / 900.0);
68 
69  btSimple.setNode<7>(2.0 / 3.0);
70  btSimple.setCoefficient<7, 0>(2.0);
71  btSimple.setCoefficient<7, 1>(0.0);
72  btSimple.setCoefficient<7, 2>(0.0);
73  btSimple.setCoefficient<7, 3>(-53.0 / 6.0);
74  btSimple.setCoefficient<7, 4>(704.0 / 45.0);
75  btSimple.setCoefficient<7, 5>(-107.0 / 9.0);
76  btSimple.setCoefficient<7, 6>(67.0 / 90.0);
77  btSimple.setCoefficient<7, 7>(3.0);
78 
79  btSimple.setNode<8>(1.0 / 3.0);
80  btSimple.setCoefficient<8, 0>(-91.0 / 108.0);
81  btSimple.setCoefficient<8, 1>(0.0);
82  btSimple.setCoefficient<8, 2>(0.0);
83  btSimple.setCoefficient<8, 3>(23.0 / 108.0);
84  btSimple.setCoefficient<8, 4>(-976.0 / 135.0);
85  btSimple.setCoefficient<8, 5>(311.0 / 54.0);
86  btSimple.setCoefficient<8, 6>(-19.0 / 60.0);
87  btSimple.setCoefficient<8, 7>(17.0 / 6.0);
88  btSimple.setCoefficient<8, 8>(-1.0 / 12.0);
89 
90  btSimple.setNode<9>(1.0);
91  btSimple.setCoefficient<9, 0>(2383.0 / 4100.0);
92  btSimple.setCoefficient<9, 1>(0.0);
93  btSimple.setCoefficient<9, 2>(0.0);
94  btSimple.setCoefficient<9, 3>(-341.0 / 164.0);
95  btSimple.setCoefficient<9, 4>(4496.0 / 1025.0);
96  btSimple.setCoefficient<9, 5>(-301.0 / 82.0);
97  btSimple.setCoefficient<9, 6>(2133.0 / 4100.0);
98  btSimple.setCoefficient<9, 7>(45.0 / 82.0);
99  btSimple.setCoefficient<9, 8>(45.0 / 164.0);
100  btSimple.setCoefficient<9, 9>(18.0 / 41.0);
101 
102  btSimple.setNode<10>(0.0);
103  btSimple.setCoefficient<10, 0>(3.0 / 205.0);
104  btSimple.setCoefficient<10, 1>(0.0);
105  btSimple.setCoefficient<10, 2>(0.0);
106  btSimple.setCoefficient<10, 3>(0.0);
107  btSimple.setCoefficient<10, 4>(0.0);
108  btSimple.setCoefficient<10, 5>(-6.0 / 41.0);
109  btSimple.setCoefficient<10, 6>(-3.0 / 205.0);
110  btSimple.setCoefficient<10, 7>(-3.0 / 41.0);
111  btSimple.setCoefficient<10, 8>(3.0 / 41.0);
112  btSimple.setCoefficient<10, 9>(6.0 / 41.0);
113  btSimple.setCoefficient<10, 10>(0.0);
114 
115  btSimple.setNode<11>(1.0);
116  btSimple.setCoefficient<11, 0>(-1777.0 / 4100.0);
117  btSimple.setCoefficient<11, 1>(0.0);
118  btSimple.setCoefficient<11, 2>(0.0);
119  btSimple.setCoefficient<11, 3>(-341.0 / 164.0);
120  btSimple.setCoefficient<11, 4>(4496.0 / 1025.0);
121  btSimple.setCoefficient<11, 5>(-289.0 / 82.0);
122  btSimple.setCoefficient<11, 6>(2193.0 / 4100.0);
123  btSimple.setCoefficient<11, 7>(51.0 / 82.0);
124  btSimple.setCoefficient<11, 8>(33.0 / 164.0);
125  btSimple.setCoefficient<11, 9>(12.0 / 41.0);
126  btSimple.setCoefficient<11, 10>(0.0);
127  btSimple.setCoefficient<11, 11>(1.0);
128 
129  btSimple.setWeight<0>(0.0);
130  btSimple.setWeight<1>(0.0);
131  btSimple.setWeight<2>(0.0);
132  btSimple.setWeight<3>(0.0);
133  btSimple.setWeight<4>(0.0);
134  btSimple.setWeight<5>(34.0 / 105.0);
135  btSimple.setWeight<6>(9.0 / 35.0);
136  btSimple.setWeight<7>(9.0 / 35.0);
137  btSimple.setWeight<8>(9.0 / 280.0);
138  btSimple.setWeight<9>(9.0 / 280.0);
139  btSimple.setWeight<10>(0.0);
140  btSimple.setWeight<11>(41.0 / 840.0);
141  btSimple.setWeight<12>(41.0 / 840.0);
142 
143  // Extended
144 
145  btExtended.setNode<0>(2.0 / 27.0);
146  btExtended.setCoefficient<0, 0>(2.0 / 27.0);
147 
148  btExtended.setNode<1>(1.0 / 9.0);
149  btExtended.setCoefficient<1, 0>(1.0 / 36.0);
150  btExtended.setCoefficient<1, 1>(1.0 / 12.0);
151 
152  btExtended.setNode<2>(1.0 / 6.0);
153  btExtended.setCoefficient<2, 0>(1.0 / 24.0);
154  btExtended.setCoefficient<2, 1>(0.0);
155  btExtended.setCoefficient<2, 2>(1.0 / 8.0);
156 
157  btExtended.setNode<3>(5.0 / 12.0);
158  btExtended.setCoefficient<3, 0>(5.0 / 12.0);
159  btExtended.setCoefficient<3, 1>(0.0);
160  btExtended.setCoefficient<3, 2>(-25.0 / 16.0);
161  btExtended.setCoefficient<3, 3>(25.0 / 16.0);
162 
163  btExtended.setNode<4>(1.0 / 2.0);
164  btExtended.setCoefficient<4, 0>(1.0 / 20.0);
165  btExtended.setCoefficient<4, 1>(0.0);
166  btExtended.setCoefficient<4, 2>(0.0);
167  btExtended.setCoefficient<4, 3>(1.0 / 4.0);
168  btExtended.setCoefficient<4, 4>(1.0 / 5.0);
169 
170  btExtended.setNode<5>(5.0 / 6.0);
171  btExtended.setCoefficient<5, 0>(-25.0 / 108.0);
172  btExtended.setCoefficient<5, 1>(0.0);
173  btExtended.setCoefficient<5, 2>(0.0);
174  btExtended.setCoefficient<5, 3>(125.0 / 108.0);
175  btExtended.setCoefficient<5, 4>(-65.0 / 27.0);
176  btExtended.setCoefficient<5, 5>(-125.0 / 54.0);
177 
178  btExtended.setNode<6>(1.0 / 6.0);
179  btExtended.setCoefficient<6, 0>(31.0 / 300.0);
180  btExtended.setCoefficient<6, 1>(0.0);
181  btExtended.setCoefficient<6, 2>(0.0);
182  btExtended.setCoefficient<6, 3>(0.0);
183  btExtended.setCoefficient<6, 4>(61.0 / 225.0);
184  btExtended.setCoefficient<6, 5>(-2.0 / 9.0);
185  btExtended.setCoefficient<6, 6>(13.0 / 900.0);
186 
187  btExtended.setNode<7>(2.0 / 3.0);
188  btExtended.setCoefficient<7, 0>(2.0);
189  btExtended.setCoefficient<7, 1>(0.0);
190  btExtended.setCoefficient<7, 2>(0.0);
191  btExtended.setCoefficient<7, 3>(-53.0 / 6.0);
192  btExtended.setCoefficient<7, 4>(704.0 / 45.0);
193  btExtended.setCoefficient<7, 5>(-107.0 / 9.0);
194  btExtended.setCoefficient<7, 6>(67.0 / 90.0);
195  btExtended.setCoefficient<7, 7>(3.0);
196 
197  btExtended.setNode<8>(1.0 / 3.0);
198  btExtended.setCoefficient<8, 0>(-91.0 / 108.0);
199  btExtended.setCoefficient<8, 1>(0.0);
200  btExtended.setCoefficient<8, 2>(0.0);
201  btExtended.setCoefficient<8, 3>(23.0 / 108.0);
202  btExtended.setCoefficient<8, 4>(-976.0 / 135.0);
203  btExtended.setCoefficient<8, 5>(311.0 / 54.0);
204  btExtended.setCoefficient<8, 6>(-19.0 / 60.0);
205  btExtended.setCoefficient<8, 7>(17.0 / 6.0);
206  btExtended.setCoefficient<8, 8>(-1.0 / 12.0);
207 
208  btExtended.setNode<9>(1.0);
209  btExtended.setCoefficient<9, 0>(2383.0 / 4100.0);
210  btExtended.setCoefficient<9, 1>(0.0);
211  btExtended.setCoefficient<9, 2>(0.0);
212  btExtended.setCoefficient<9, 3>(-341.0 / 164.0);
213  btExtended.setCoefficient<9, 4>(4496.0 / 1025.0);
214  btExtended.setCoefficient<9, 5>(-301.0 / 82.0);
215  btExtended.setCoefficient<9, 6>(2133.0 / 4100.0);
216  btExtended.setCoefficient<9, 7>(45.0 / 82.0);
217  btExtended.setCoefficient<9, 8>(45.0 / 164.0);
218  btExtended.setCoefficient<9, 9>(18.0 / 41.0);
219 
220  btExtended.setNode<10>(0.0);
221  btExtended.setCoefficient<10, 0>(3.0 / 205.0);
222  btExtended.setCoefficient<10, 1>(0.0);
223  btExtended.setCoefficient<10, 2>(0.0);
224  btExtended.setCoefficient<10, 3>(0.0);
225  btExtended.setCoefficient<10, 4>(0.0);
226  btExtended.setCoefficient<10, 5>(-6.0 / 41.0);
227  btExtended.setCoefficient<10, 6>(-3.0 / 205.0);
228  btExtended.setCoefficient<10, 7>(-3.0 / 41.0);
229  btExtended.setCoefficient<10, 8>(3.0 / 41.0);
230  btExtended.setCoefficient<10, 9>(6.0 / 41.0);
231  btExtended.setCoefficient<10, 10>(0.0);
232 
233  btExtended.setNode<11>(1.0);
234  btExtended.setCoefficient<11, 0>(-1777.0 / 4100.0);
235  btExtended.setCoefficient<11, 1>(0.0);
236  btExtended.setCoefficient<11, 2>(0.0);
237  btExtended.setCoefficient<11, 3>(-341.0 / 164.0);
238  btExtended.setCoefficient<11, 4>(4496.0 / 1025.0);
239  btExtended.setCoefficient<11, 5>(-289.0 / 82.0);
240  btExtended.setCoefficient<11, 6>(2193.0 / 4100.0);
241  btExtended.setCoefficient<11, 7>(51.0 / 82.0);
242  btExtended.setCoefficient<11, 8>(33.0 / 164.0);
243  btExtended.setCoefficient<11, 9>(12.0 / 41.0);
244  btExtended.setCoefficient<11, 10>(0.0);
245  btExtended.setCoefficient<11, 11>(1.0);
246 
247  btExtended.setWeight<0, true>(0.0);
248  btExtended.setWeight<1, true>(0.0);
249  btExtended.setWeight<2, true>(0.0);
250  btExtended.setWeight<3, true>(0.0);
251  btExtended.setWeight<4, true>(0.0);
252  btExtended.setWeight<5, true>(34.0 / 105.0);
253  btExtended.setWeight<6, true>(9.0 / 35.0);
254  btExtended.setWeight<7, true>(9.0 / 35.0);
255  btExtended.setWeight<8, true>(9.0 / 280.0);
256  btExtended.setWeight<9, true>(9.0 / 280.0);
257  btExtended.setWeight<10, true>(0.0);
258  btExtended.setWeight<11, true>(41.0 / 840.0);
259  btExtended.setWeight<12, true>(41.0 / 840.0);
260 
261  btExtended.setWeight<0, false>(41.0 / 840.0);
262  btExtended.setWeight<1, false>(0.0);
263  btExtended.setWeight<2, false>(0.0);
264  btExtended.setWeight<3, false>(0.0);
265  btExtended.setWeight<4, false>(0.0);
266  btExtended.setWeight<5, false>(0.0);
267  btExtended.setWeight<6, false>(34.0 / 105.0);
268  btExtended.setWeight<7, false>(9.0 / 35.0);
269  btExtended.setWeight<8, false>(9.0 / 35.0);
270  btExtended.setWeight<9, false>(9.0 / 280.0);
271  btExtended.setWeight<10, false>(9.0 / 280.0);
272  btExtended.setWeight<11, false>(41.0 / 480.0);
273  btExtended.setWeight<12, false>(0.0);
274  }
275 
276  ls::primitives::ButcherTableau<13, false> btSimple;
277  ls::primitives::ButcherTableau<13, true> btExtended;
278  };
279 
280  class RungeKuttaFehlberg78 {
281  public:
282  static ls::primitives::ButcherTableau<13, true> &getButcherTableau()
283  {
284  static RungeKuttaFehlberg78Impl *boshImpl = new RungeKuttaFehlberg78Impl();
285 
286  return boshImpl->btExtended;
287  }
288 
289  static ls::primitives::ButcherTableau<13, false> &getButcherTableauSimple()
290  {
291  static RungeKuttaFehlberg78Impl *boshImpl = new RungeKuttaFehlberg78Impl();
292 
293  return boshImpl->btSimple;
294  }
295 
296  private:
297  RungeKuttaFehlberg78() = default;
298 
299  // Delete copy/move so extra instances can't be created/moved.
300  RungeKuttaFehlberg78(const RungeKuttaFehlberg78 &) = delete;
301 
302  RungeKuttaFehlberg78 &operator=(const RungeKuttaFehlberg78 &) = delete;
303 
304  RungeKuttaFehlberg78(RungeKuttaFehlberg78 &&) = delete;
305 
306  RungeKuttaFehlberg78 &operator=(RungeKuttaFehlberg78 &&) = delete;
307  };
308  }
309 
310  template<typename TType, typename TScalarType = double>
311  class RungeKuttaFehlberg78 {
312  public:
313  typedef std::function<TType(TScalarType, TType)> TDFunction;
314 
315  static const short int kLowerErrorOrder = 7;
316  static const short int kLigherErrorOrder = 8;
317  static const short int kStages = 13;
318 
328  static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N = 1);
329 
341  static TType integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N = 1);
342 
343  // TODO: Add integrate function with maximum allowable error (adaptive step size).
344  };
345 
346  template<typename TType, typename TScalarType>
347  void RungeKuttaFehlberg78<TType, TScalarType>::integrateSimple(const TDFunction &f, TScalarType &t, TType &y,
348  const TScalarType h, size_t N)
349  {
350  for (int i = 0; i < N; i++) {
351  y = detail::RungeKuttaFehlberg78::getButcherTableauSimple().template execute(f, y, t, h);
352  t += h;
353  }
354  }
355 
356  template<typename TType, typename TScalarType>
357  TType RungeKuttaFehlberg78<TType, TScalarType>::integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y,
358  const TScalarType h, size_t N)
359  {
360  std::pair<TType, TType> ye;
361  for (int i = 0; i < N; i++) {
362  ye = detail::RungeKuttaFehlberg78::getButcherTableau().template execute<TType, 0, true>(f, y, t, h);
363  y = ye.first;
364  t += h;
365  }
366 
367  return ye.second;
368  }
369  }
370 }
371 
372 #endif //LODESTAR_RUNGEKUTTAFEHLBERG78_HPP
ls::primitives::detail::RungeKuttaFehlberg78Impl::RungeKuttaFehlberg78Impl
RungeKuttaFehlberg78Impl()
Constructor initializes Butcher tables.
Definition: RungeKuttaFehlberg78.hpp:25
ls::primitives::RungeKuttaFehlberg78::integrateEmbedded
static TType integrateEmbedded(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Integrate using the Runge-Kutta-Fehlberg method (rk78) with truncation error output.
Definition: RungeKuttaFehlberg78.hpp:357
ls::primitives::ButcherTableau< 13, false >
ls::primitives::RungeKuttaFehlberg78
Definition: RungeKuttaFehlberg78.hpp:311
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::primitives::RungeKuttaFehlberg78::integrateSimple
static void integrateSimple(const TDFunction &f, TScalarType &t, TType &y, TScalarType h, size_t N=1)
Simple integration using the eighth-order scheme in the Runge-Kutta-Fehlberg method.
Definition: RungeKuttaFehlberg78.hpp:347
ls::primitives::detail::RungeKuttaFehlberg78Impl
Definition: RungeKuttaFehlberg78.hpp:13
ls::primitives::detail::RungeKuttaFehlberg78
Definition: RungeKuttaFehlberg78.hpp:280