diff options
Diffstat (limited to 'src/modules/position_estimator_inav/inertial_filter.c')
-rw-r--r-- | src/modules/position_estimator_inav/inertial_filter.c | 28 |
1 files changed, 28 insertions, 0 deletions
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c new file mode 100644 index 000000000..b70d3504e --- /dev/null +++ b/src/modules/position_estimator_inav/inertial_filter.c @@ -0,0 +1,28 @@ +/* + * inertial_filter.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> + */ + +#include "inertial_filter.h" + +void inertial_filter_predict(float dt, float x[3]) +{ + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void inertial_filter_correct(float dt, float x[3], int i, float z, float w) +{ + float e = z - x[i]; + x[i] += e * w * dt; + + if (i == 0) { + x[1] += e * w * w * dt; + //x[2] += e * w * w * w * dt / 3.0; // ~ 0.0 + + } else if (i == 1) { + x[2] += e * w * w * dt; + } +} |