autogenu-jupyter
An automatic code generator and the continuation/GMRES (C/GMRES) based numerical solvers for nonlinear MPC
Loading...
Searching...
No Matches
integrator.hpp
Go to the documentation of this file.
1#ifndef CGMRES__INTEGRATOR_HPP_
2#define CGMRES__INTEGRATOR_HPP_
3
4#include "cgmres/types.hpp"
5
6namespace cgmres {
7
17template <typename OCP, typename StateVectorType, typename ControlInputVectorType>
18VectorX ForwardEuler(const OCP& ocp, const Scalar t, const Scalar dt,
21 VectorX x1;
22 x1.setZero(x.size());
23 ocp.eval_f(t, x, u, x1);
24 x1.array() *= dt;
25 x1.noalias() += x;
26 return x1;
27}
28
29
39template <typename OCP, typename StateVectorType, typename ControlInputVectorType>
40VectorX RK4(const OCP& ocp, const Scalar t, const Scalar dt,
43 VectorX k1, k2, k3, k4, x1;
44 k1.setZero(x.size());
45 k2.setZero(x.size());
46 k3.setZero(x.size());
47 k4.setZero(x.size());
48 x1.setZero(x.size());
49 ocp.eval_f(t, x, u, k1);
50 x1 = x + 0.5 * dt * k1;
51 ocp.eval_f(t+0.5*dt, x1, u, k2);
52 x1 = x + dt * 0.5 * (std::sqrt(2.0)-1.0) * k1 + dt*(1.0-(1.0/std::sqrt(2.0))) * k2;
53 ocp.eval_f(t+0.5*dt, x1, u, k3);
54 x1 = x - dt * 0.5 * std::sqrt(2.0) * k2 + dt * (1.0+(1.0/std::sqrt(2.0))) * k3;
55 ocp.eval_f(t+dt, x1, u, k4);
56 x1 = x + (dt/6.0) * (k1+(2.0-std::sqrt(2.0))*k2 + (2.0+std::sqrt(2.0))*k3+k4);
57 return x1;
58}
59
60} // namespace cgmres
61
62#endif // CGMRES__INTEGRATOR_HPP_
Definition: continuation_gmres.hpp:11
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorX
Alias of Eigen::VectorXd (dynamic-size vector).
Definition: types.hpp:39
VectorX RK4(const OCP &ocp, const Scalar t, const Scalar dt, const MatrixBase< StateVectorType > &x, const MatrixBase< ControlInputVectorType > &u)
Computes the next state by the 4th-order Runge-Kutta method.
Definition: integrator.hpp:40
VectorX ForwardEuler(const OCP &ocp, const Scalar t, const Scalar dt, const MatrixBase< StateVectorType > &x, const MatrixBase< ControlInputVectorType > &u)
Computes the next state by the forward Euler.
Definition: integrator.hpp:18
double Scalar
Alias of double.
Definition: types.hpp:11
Eigen::MatrixBase< MatrixType > MatrixBase
Alias of Eigen::MatrixBase.
Definition: types.hpp:29