fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-16 20:37:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Work on rk4 interface.

Browse Source
This commit is contained in:
jgoppert
2015-11-12 15:13:17 -05:00
parent 7656385ea1
commit aa3a086cda
1 changed files with 7 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
matrix/integration.hpp
+7 -6
View File
@@ -4,10 +4,11 @@
namespace matrix {
template<typename Type, size_t M>
template<typename Type, size_t M, size_t N>
int integrate_rk4(
Vector<Type, M> (*f)(Type, Vector<Type, M>),
Vector<Type, M> (*f)(Type, const Vector<Type, M> &x, const Vector<Type, N> & u),
const Vector<Type, M> & y0,
const Vector<Type, N> & u,
Type t0,
Type h,
Vector<Type, M> & y1
@@ -15,10 +16,10 @@ int integrate_rk4(
{
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
Vector<Type, M> k1, k2, k3, k4;
k1 = f(t0, y0);
k2 = f(t0 + h/2, y0 + k1*h/2);
k3 = f(t0 + h/2, y0 + k2*h/2);
k4 = f(t0 + h, y0 + k3*h);
k1 = f(t0, y0, u);
k2 = f(t0 + h/2, y0 + k1*h/2, u);
k3 = f(t0 + h/2, y0 + k2*h/2, u);
k4 = f(t0 + h, y0 + k3*h, u);
y1 = y0 + (k1 + k2*2 + k3*2 + k4)*(h/6);
return 0;
}