次のスムージング機能を使用して、GPSからの速度の読み取りをスムージングしています。
void smoothing_init()
{
k = 0;
kalman_init(0.0625, 32, 1.3833094, 0);
}
void kalman_init(double _q, double _r, double _p, double intial_value)
{
q = _q;
r = _r;
p = _p;
x = intial_value;
}
double smoothing_add_sample(double measurement)
{
p = p + q;
k = p / (p + r);
x = x + k * (measurement - x);
p = (1 - k) * p;
return x;
}
ただし、これにより、平滑化値700(通常の範囲は0〜150)が得られ、その後下がることがあります。ルーチンを0で初期化したが、すぐに0を超える読み取り値(たとえば、40、50)を受け取ったときに発生すると思います。
これらの関数を微調整して、このようなスパイクを自然に防止しながら、データをスムーズにできるようにするにはどうすればよいですか。