Lodestar
An integrated real-time control package in C++
BackwardDifference.hpp
1 //
2 // Created by Hamza El-Kebir on 6/18/21.
3 //
4 
5 #ifndef LODESTAR_BACKWARDDIFFERENCE_HPP
6 #define LODESTAR_BACKWARDDIFFERENCE_HPP
7 
8 #include <Eigen/Dense>
9 
10 namespace ls {
11  namespace primitives {
12  template<typename TType, size_t TSamples, size_t TOrder, typename TScalarType = double>
13  class BackwardDifference {
14  static_assert(TSamples > 1, "Number of samples must be greater than one.");
15 
16  static_assert(TOrder > 1, "Differentiation order must be greater than one.");
17 
18  static_assert(TSamples > TOrder, "Number of samples must be greater than order.");
19 
20  static_assert(
21  // First order
22  (TSamples == 2 && TOrder == 1) ||
23  (TSamples == 3 && TOrder == 1) ||
24  (TSamples == 4 && TOrder == 1) ||
25  (TSamples == 5 && TOrder == 1) ||
26  (TSamples == 6 && TOrder == 1) ||
27  (TSamples == 7 && TOrder == 1) ||
28  // Second order
29  (TSamples == 3 && TOrder == 2) ||
30  (TSamples == 4 && TOrder == 2) ||
31  (TSamples == 5 && TOrder == 2) ||
32  (TSamples == 6 && TOrder == 2) ||
33  (TSamples == 7 && TOrder == 2) ||
34  // Third order
35  (TSamples == 4 && TOrder == 3) ||
36  (TSamples == 5 && TOrder == 3) ||
37  (TSamples == 6 && TOrder == 3) ||
38  (TSamples == 7 && TOrder == 3), "Combination of samples and order is not supported.");
39  };
40 
41 // ------- First derivative ---------
42 
49  template<typename TType, typename TScalarType>
50  class BackwardDifference<TType, 2, 1, TScalarType> {
51  protected:
52  static TType compute(const TType &xNeg1, const TType &x0)
53  {
54  return x0 - xNeg1;
55  }
56 
57  public:
58  static TType compute(const TType &xNeg1, const TType &x0, TScalarType h)
59  {
60  return compute(xNeg1, x0) / h;
61  }
62 
63  static const int kOrder = 1;
64  static const int kErrorOrder = 1;
65  };
66 
73  template<typename TType, typename TScalarType>
74  class BackwardDifference<TType, 3, 1, TScalarType> {
75  protected:
76  static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
77  {
78  return (3.0 / 2.0) * x0 - 2.0 * xNeg1 + (1.0 / 2.0) * xNeg2;
79  }
80 
81  public:
82  static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
83  {
84  return compute(xNeg2, xNeg1, x0) / h;
85  }
86 
87  static const int kOrder = 1;
88  static const int kErrorOrder = 2;
89  };
90 
97  template<typename TType, typename TScalarType>
98  class BackwardDifference<TType, 4, 1, TScalarType> {
99  protected:
100  static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
101  {
102  return (11.0 / 6.0) * x0 - 3.0 * xNeg1 + (3.0 / 2.0) * xNeg2 - (1.0 / 3.0) * xNeg3;
103  }
104 
105  public:
106  static TType
107  compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
108  {
109  return compute(xNeg3, xNeg2, xNeg1, x0) / h;
110  }
111 
112  static const int kOrder = 1;
113  static const int kErrorOrder = 3;
114  };
115 
122  template<typename TType, typename TScalarType>
123  class BackwardDifference<TType, 5, 1, TScalarType> {
124  protected:
125  static TType
126  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
127  {
128  return (25.0 / 12.0) * x0 - 4.0 * xNeg1 + 3.0 * xNeg2 - (4.0 / 3.0) * xNeg3 + (1.0 / 4.0) * xNeg4;
129  }
130 
131  public:
132  static TType
133  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
134  TScalarType h)
135  {
136  return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
137  }
138 
139  static const int kOrder = 1;
140  static const int kErrorOrder = 4;
141  };
142 
149  template<typename TType, typename TScalarType>
150  class BackwardDifference<TType, 6, 1, TScalarType> {
151  protected:
152  static TType
153  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
154  const TType &x0)
155  {
156  return (137.0 / 60.0) * x0 - 5.0 * xNeg1 + 5.0 * xNeg2 - (10.0 / 3.0) * xNeg3 + (5.0 / 4.0) * xNeg4 -
157  (1.0 / 6.0) * xNeg5;
158  }
159 
160  public:
161  static TType
162  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
163  const TType &x0, TScalarType h)
164  {
165  return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
166  }
167 
168  static const int kOrder = 1;
169  static const int kErrorOrder = 5;
170  };
171 
178  template<typename TType, typename TScalarType>
179  class BackwardDifference<TType, 7, 1, TScalarType> {
180  protected:
181  static TType
182  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
183  const TType &xNeg1, const TType &x0)
184  {
185  return (49.0 / 20.0) * x0 - 6.0 * xNeg1 + (15.0 / 2.0) * xNeg2 - (20.0 / 3.0) * xNeg3 +
186  (15.0 / 4.0) * xNeg4 - (6.0 / 5.0) * xNeg5 + (1.0 / 6.0) * xNeg6;
187  }
188 
189  public:
190  static TType
191  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
192  const TType &xNeg1, const TType &x0, TScalarType h)
193  {
194  return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / h;
195  }
196 
197  static const int kOrder = 1;
198  static const int kErrorOrder = 6;
199  };
200 
201 // ------- Second derivative ---------
202 
209  template<typename TType, typename TScalarType>
210  class BackwardDifference<TType, 3, 2, TScalarType> {
211  protected:
212  static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0)
213  {
214  return x0 - 2 * xNeg1 + xNeg2;
215  }
216 
217  public:
218  static TType compute(const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
219  {
220  return compute(xNeg2, xNeg1, x0) / (h * h);
221  }
222 
223  static const int kOrder = 2;
224  static const int kErrorOrder = 1;
225  };
226 
233  template<typename TType, typename TScalarType>
234  class BackwardDifference<TType, 4, 2, TScalarType> {
235  protected:
236  static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
237  {
238  return 2 * x0 - 5 * xNeg1 + 4 * xNeg2 - 1 * xNeg3;
239  }
240 
241  public:
242  static TType
243  compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
244  {
245  return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h);
246  }
247 
248  static const int kOrder = 2;
249  static const int kErrorOrder = 2;
250  };
251 
258  template<typename TType, typename TScalarType>
259  class BackwardDifference<TType, 5, 2, TScalarType> {
260  protected:
261  static TType
262  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
263  {
264  return (35.0 / 12.0) * x0 - (26.0 / 3.0) * xNeg1 + (19.0 / 2.0) * xNeg2 - (14.0 / 3.0) * xNeg3 +
265  (11.0 / 12.0) * xNeg4;
266  }
267 
268  public:
269  static TType
270  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
271  TScalarType h)
272  {
273  return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
274  }
275 
276  static const int kOrder = 2;
277  static const int kErrorOrder = 3;
278  };
279 
286  template<typename TType, typename TScalarType>
287  class BackwardDifference<TType, 6, 2, TScalarType> {
288  protected:
289  static TType
290  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
291  const TType &x0)
292  {
293  return (15.0 / 4.0) * x0 - (77.0 / 6.0) * xNeg1 + (107.0 / 6.0) * xNeg2 - 13.0 * xNeg3 +
294  (61.0 / 12.0) * xNeg4 - (5.0 / 6.0) * xNeg5;
295  }
296 
297  public:
298  static TType
299  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
300  const TType &x0, TScalarType h)
301  {
302  return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
303  }
304 
305  static const int kOrder = 2;
306  static const int kErrorOrder = 4;
307  };
308 
315  template<typename TType, typename TScalarType>
316  class BackwardDifference<TType, 7, 2, TScalarType> {
317  protected:
318  static TType
319  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
320  const TType &xNeg1, const TType &x0)
321  {
322  return (203.0 / 45.0) * x0 - (87.0 / 5.0) * xNeg1 + (117.0 / 4.0) * xNeg2 - (254.0 / 9.0) * xNeg3 +
323  (33.0 / 2.0) * xNeg4 - (27.0 / 5.0) * xNeg5 + (137.0 / 180.0) * xNeg6;
324  }
325 
326  public:
327  static TType
328  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
329  const TType &xNeg1, const TType &x0, TScalarType h)
330  {
331  return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h);
332  }
333 
334  static const int kOrder = 2;
335  static const int kErrorOrder = 5;
336  };
337 
338 // ------- Third derivative ---------
339 
346  template<typename TType, typename TScalarType>
347  class BackwardDifference<TType, 4, 3, TScalarType> {
348  protected:
349  static TType compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
350  {
351  return 1.0 * x0 - 3.0 * xNeg1 + 3.0 * xNeg2 - 1.0 * xNeg3;
352  }
353 
354  public:
355  static TType
356  compute(const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0, TScalarType h)
357  {
358  return compute(xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
359  }
360 
361  static const int kOrder = 3;
362  static const int kErrorOrder = 1;
363  };
364 
371  template<typename TType, typename TScalarType>
372  class BackwardDifference<TType, 5, 3, TScalarType> {
373  protected:
374  static TType
375  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0)
376  {
377  return (5.0 / 2.0) * x0 - 9.0 * xNeg1 + 12.0 * xNeg2 - 7.0 * xNeg3 + (3.0 / 2.0) * xNeg4;
378  }
379 
380  public:
381  static TType
382  compute(const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1, const TType &x0,
383  TScalarType h)
384  {
385  return compute(xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
386  }
387 
388  static const int kOrder = 3;
389  static const int kErrorOrder = 2;
390  };
391 
398  template<typename TType, typename TScalarType>
399  class BackwardDifference<TType, 6, 3, TScalarType> {
400  protected:
401  static TType
402  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
403  const TType &x0)
404  {
405  return (17.0 / 4.0) * x0 - (71.0 / 4.0) * xNeg1 + (59.0 / 2.0) * xNeg2 - (49.0 / 2.0) * xNeg3 +
406  (41.0 / 4.0) * xNeg4 - (7.0 / 4.0) * xNeg5;
407  }
408 
409  public:
410  static TType
411  compute(const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2, const TType &xNeg1,
412  const TType &x0, TScalarType h)
413  {
414  return compute(xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
415  }
416 
417  static const int kOrder = 3;
418  static const int kErrorOrder = 3;
419  };
420 
427  template<typename TType, typename TScalarType>
428  class BackwardDifference<TType, 7, 3, TScalarType> {
429  protected:
430  static TType
431  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
432  const TType &xNeg1, const TType &x0)
433  {
434  return (49.0 / 8.0) * x0 - 29.0 * xNeg1 + (461.0 / 8.0) * xNeg2 - 62.0 * xNeg3 + (307.0 / 8.0) * xNeg4 -
435  13.0 * xNeg5 + (15.0 / 8.0) * xNeg6;
436  }
437 
438  public:
439  static TType
440  compute(const TType &xNeg6, const TType &xNeg5, const TType &xNeg4, const TType &xNeg3, const TType &xNeg2,
441  const TType &xNeg1, const TType &x0, TScalarType h)
442  {
443  return compute(xNeg6, xNeg5, xNeg4, xNeg3, xNeg2, xNeg1, x0) / (h * h * h);
444  }
445 
446  static const int kOrder = 3;
447  static const int kErrorOrder = 4;
448  };
449 
450  }
451 }
452 
453 #endif //LODESTAR_BACKWARDDIFFERENCE_HPP
ls
Main Lodestar code.
Definition: BilinearTransformation.hpp:12
ls::primitives::BackwardDifference
Definition: BackwardDifference.hpp:13