diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-06-17 14:41:35 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-06-17 14:41:35 +0400 |
commit | a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8 (patch) | |
tree | 695834036bb0d4435b5fa8b9eab822104e44efe7 /src/modules/position_estimator_inav/inertial_filter.c | |
parent | 650de90a904368eb93689bbb8a3be63888bf244e (diff) | |
download | px4-firmware-a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8.tar.gz px4-firmware-a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8.tar.bz2 px4-firmware-a83aca753c2a5c8680c8a3b7258a1b2c25fbbce8.zip |
position_estimator_inav rewrite, publishes vehicle_global_position now
Diffstat (limited to 'src/modules/position_estimator_inav/inertial_filter.c')
-rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 10 |
1 files changed, 5 insertions, 5 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c index b70d3504e..8cdde5e1a 100644 --- a/src/modules/position_estimator_inav/inertial_filter.c +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -13,16 +13,16 @@ void inertial_filter_predict(float dt, float x[3]) x[1] += x[2] * dt; } -void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +void inertial_filter_correct(float edt, float x[3], int i, float w) { - float e = z - x[i]; - x[i] += e * w * dt; + float ewdt = w * edt; + x[i] += ewdt; if (i == 0) { - x[1] += e * w * w * dt; + x[1] += w * ewdt; //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 } else if (i == 1) { - x[2] += e * w * w * dt; + x[2] += w * ewdt; } } |