M
Size: a a a
M
КГ
M
M
f
КГ
КГ
КГ
КГ
КГ
f
public class SimpleKalman
{
private readonly float _q = 0.000001f;
private readonly float _r = 0.01f;
private float _p = 1, _k;
public SimpleKalman()
{
}
public SimpleKalman(float q, float r)
{
_q = q;
_r = r;
}
private void MeasurementUpdate()
{
_k = (_p + _q)/(_p + _q + _r);
_p = _r*(_p + _q)/(_r + _p + _q);
}
public float Last { get; private set; }
public float Update(float measurement)
{
MeasurementUpdate();
var result = Last + (measurement - Last)*_k;
Last = result;
return result;
}
}
КГ
КГ
f
КГ
КГ
f
КГ
f
КГ