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

Improved rk4 integration to allow longer time interval than 1 step.

Browse Source
This commit is contained in:
James Goppert 2016-02-16 10:53:12 -05:00
parent ce30542dba
commit 1ae114b3d2
2 changed files with 29 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
matrix/integration.hpp
View File

@ -10,18 +10,33 @@ int integrate_rk4(
const Matrix<Type, M, 1> & y0,
const Matrix<Type, N, 1> & u,
Type t0,
Type h,
Type tf,
Type h0,
Matrix<Type, M, 1> & y1
)
{
// https://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods
Type t1 = t0;
y1 = y0;
Type h = h0;
Vector<Type, M> k1, k2, k3, k4;
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);
if (tf < t0) return -1; // make sure t1 > t0
while (t1 < tf) {
if (t1 + h0 < tf) {
h = h0;
} else {
h = tf - t1;
}
k1 = f(t1, y1, u);
k2 = f(t1 + h/2, y1 + k1*h/2, u);
k3 = f(t1 + h/2, y1 + k2*h/2, u);
k4 = f(t1 + h, y1 + k3*h, u);
y1 += (k1 + k2*2 + k3*2 + k4)*(h/6);
t1 += h;
}
return 0;
}
} // namespace matrix
// vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 :

13
test/integration.cpp
View File

@ -8,17 +8,20 @@ using namespace matrix;
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & y, const Matrix<float, 3, 1> & u);
Vector<float, 6> f(float t, const Matrix<float, 6, 1> & y, const Matrix<float, 3, 1> & u) {
return ones<float, 6, 1>();
float v = -sinf(t);
return v*ones<float, 6, 1>();
}
int main()
{
Vector<float, 6> y = ones<float, 6, 1>();
Vector<float, 3> u = ones<float, 3, 1>();
float t = 1;
float h = 0.1f;
integrate_rk4(f, y, u, t, h, y);
TEST(isEqual(y, (ones<float, 6, 1>()*1.1f)));
float t0 = 0;
float tf = 2;
float h = 0.001f;
integrate_rk4(f, y, u, t0, tf, h, y);
float v = 1 + cosf(tf) - cosf(t0);
TEST(isEqual(y, (ones<float, 6, 1>()*v)));
return 0;
}