fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 01:50:34 +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
Download Patch File Download Diff File Expand all files Collapse all files
test/integration.cpp
+8 -5
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;
}