freeExec

SimpleKalman

Dec 2nd, 2021
702
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1.     public class SimpleKalman
  2.     {
  3.         private readonly float _q = 0.000001f;
  4.         private readonly float _r = 0.01f;
  5.         private float _p = 1, _k;
  6.  
  7.         public SimpleKalman()
  8.         {
  9.         }
  10.  
  11.         public SimpleKalman(float q, float r)
  12.         {
  13.             _q = q;
  14.             _r = r;
  15.         }
  16.  
  17.         private void MeasurementUpdate()
  18.         {
  19.             _k = (_p + _q)/(_p + _q + _r);
  20.             _p = _r*(_p + _q)/(_r + _p + _q);
  21.         }
  22.  
  23.         public float Last { get; private set; }
  24.  
  25.         public float Update(float measurement)
  26.         {
  27.             MeasurementUpdate();
  28.  
  29.             var result = Last + (measurement - Last)*_k;
  30.             Last = result;
  31.  
  32.             return result;
  33.         }
  34.     }
RAW Paste Data