blob: 13328edb4baa2b2dec3140a24e0d915b822a1098 (
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
28
29
30
31
|
/*
* 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 e, float dt, float x[3], int i, float w)
{
float ewdt = w * dt;
if (ewdt > 1.0f)
ewdt = 1.0f; // prevent over-correcting
ewdt *= e;
x[i] += ewdt;
if (i == 0) {
x[1] += w * ewdt;
x[2] += w * w * ewdt / 3.0;
} else if (i == 1) {
x[2] += w * ewdt;
}
}
|