diff options
Diffstat (limited to 'apps/position_estimator_inav/kalman_filter_inertial.c')
-rw-r--r-- | apps/position_estimator_inav/kalman_filter_inertial.c | 21 |
1 files changed, 21 insertions, 0 deletions
diff --git a/apps/position_estimator_inav/kalman_filter_inertial.c b/apps/position_estimator_inav/kalman_filter_inertial.c new file mode 100644 index 000000000..7de06cb44 --- /dev/null +++ b/apps/position_estimator_inav/kalman_filter_inertial.c @@ -0,0 +1,21 @@ +#include "kalman_filter_inertial.h" + +void kalman_filter_inertial_predict(float dt, float x[3]) { + x[0] += x[1] * dt + x[2] * dt * dt / 2.0f; + x[1] += x[2] * dt; +} + +void kalman_filter_inertial_update(float x[3], float z[2], float k[3][2], bool use[2]) { + float y[2]; + // y = z - x + for (int i = 0; i < 2; i++) { + y[i] = z[i] - x[i]; + } + // x = x + K * y + for (int i = 0; i < 3; i++) { // Row + for (int j = 0; j < 2; j++) { // Column + if (use[j]) + x[i] += k[i][j] * y[j]; + } + } +} |