blob: 64031ee7bb1129e9d814b2dc82f107bb1d2f8b01 (
plain) (
blame)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
|
/*
* kalman_filter_inertial.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#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 - H x
y[0] = z[0] - x[0];
y[1] = z[1] - x[2];
// 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];
}
}
}
|