RIT VEXU Core API
Loading...
Searching...
No Matches
numerical_integration.h
1#pragma once
2
3#include "math/eigen_interface.h"
4
5#include <functional>
6
33
34template <int X, int U>
35using WithInputDerivative =
36 std::function<Eigen::Vector<double, X>(const Eigen::Vector<double, X> &, const Eigen::Vector<double, U> &)>;
37
38template <int X>
39using WithoutInputDerivative = std::function<Eigen::Vector<double, X>(const Eigen::Vector<double, X> &)>;
40
41template <int Y>
42using TimeVariantDerivative = std::function<Eigen::Vector<double, Y>(const double &, const Eigen::Vector<double, Y> &)>;
43
57template <int X, int U>
58Eigen::Vector<double, X> euler_with_input(
59 const WithInputDerivative<X, U> &f, const Eigen::Vector<double, X> &x, const Eigen::Vector<double, U> &u,
60 const double &h
61) {
62 Eigen::Vector<double, X> k1 = f(x, u);
63
64 return x + h * k1;
65}
66
79template <int X>
80Eigen::Vector<double, X>
81euler_without_input(const WithoutInputDerivative<X> &f, const Eigen::Vector<double, X> &x, const double &h) {
82 Eigen::Vector<double, X> k1 = f(x);
83
84 return x + h * k1;
85}
86
100template <int Y>
101Eigen::Vector<double, Y> euler_time_variant(
102 const TimeVariantDerivative<Y> &f, const double &t, const Eigen::Vector<double, Y> &y, const double &h
103) {
104 Eigen::Vector<double, Y> k1 = f(t, y);
105
106 return y + h * k1;
107}
108
123template <int X, int U>
124Eigen::Vector<double, X> RK2_with_input(
125 const WithInputDerivative<X, U> &f, const Eigen::Vector<double, X> &x, const Eigen::Vector<double, U> &u,
126 const double &h
127) {
128 Eigen::Vector<double, X> k1 = f(x, u);
129 Eigen::Vector<double, X> k2 = f(x + h * 0.5 * k1, u);
130
131 return x + h * k2;
132}
133
147template <int X>
148Eigen::Vector<double, X>
149RK2_without_input(const WithoutInputDerivative<X> &f, const Eigen::Vector<double, X> &x, const double &h) {
150 Eigen::Vector<double, X> k1 = f(x);
151 Eigen::Vector<double, X> k2 = f(x + h * 0.5 * k1);
152
153 return x + h * k2;
154}
155
170template <int Y>
171Eigen::Vector<double, Y> RK2_time_variant(
172 const TimeVariantDerivative<Y> &f, const double &t, const Eigen::Vector<double, Y> &y, const double &h
173) {
174 Eigen::Vector<double, Y> k1 = f(t, y);
175 Eigen::Vector<double, Y> k2 = f(t + h * 0.5, y + h * 0.5 * k1);
176
177 return y + h * k2;
178}
179
196template <int X, int U>
197Eigen::Vector<double, X> RK4_with_input(
198 const WithInputDerivative<X, U> &f, const Eigen::Vector<double, X> &x, const Eigen::Vector<double, U> &u,
199 const double &h
200) {
201 Eigen::Vector<double, X> k1 = f(x, u);
202 Eigen::Vector<double, X> k2 = f(x + h * 0.5 * k1, u);
203 Eigen::Vector<double, X> k3 = f(x + h * 0.5 * k2, u);
204 Eigen::Vector<double, X> k4 = f(x + h * k3, u);
205
206 return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
207}
208
224template <int X>
225Eigen::Vector<double, X>
226RK4_without_input(const WithoutInputDerivative<X> &f, const Eigen::Vector<double, X> &x, const double &h) {
227 Eigen::Vector<double, X> k1 = f(x);
228 Eigen::Vector<double, X> k2 = f(x + h * 0.5 * k1);
229 Eigen::Vector<double, X> k3 = f(x + h * 0.5 * k2);
230 Eigen::Vector<double, X> k4 = f(x + h * k3);
231
232 return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
233}
234
251template <int Y>
252Eigen::Vector<double, Y> RK4_time_variant(
253 const TimeVariantDerivative<Y> &f, const double &t, const Eigen::Vector<double, Y> &y, const double &h
254) {
255 Eigen::Vector<double, Y> k1 = f(t, y);
256 Eigen::Vector<double, Y> k2 = f(t + h * 0.5, y + h * 0.5 * k1);
257 Eigen::Vector<double, Y> k3 = f(t + h * 0.5, y + h * 0.5 * k2);
258 Eigen::Vector<double, Y> k4 = f(t + h, y + h * k3);
259
260 return y + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
261}