Simple Kalman Filter

    本來將這篇放到 Arduino - 使用 Wii Nunchuck後面算是針對單個角度值做一維的卡爾曼濾波的簡單應用，不過感覺放在後面有點亂所以還是另外開一篇，下次在寫多維卡爾曼濾波融合加速度計與陀螺儀的應用

   Kalman FIilter的應用最常見的地方在慣性導航系統中，比如四軸飛行器或者火箭以及SEGWAY平衡的PID值預測，OpenCV中也有個Kalman Filter，用來做物體追蹤預測位置或者速度，Kalman FIilter可以從前一個預測結果計算出現在的最佳結果以及下個結果，而Kalman FIilter光是線性與非線性版本就有很多種，維數的少到多與難度從簡單到複雜成正比，簡單的Kalman FIilter可以用來校正加速度計的角度之類的值，或者將陀螺儀的角速度積分後的值與加速度計的角度做Kalman FIilter後取最佳的角度，而簡化後的Simple Kalman Filter只有幾個計算跑在Arduino上也很容易。


Simple Kalman Filter：

    q是預測誤差、r是感測器誤差、k是卡爾曼增益、x用來紀錄狀態值、p直接取q與r的平方和開根號，接著隨著時間更新時，現在的結果會是上一個結果加上上一個結果和現在量測結果的差再乘以增益的值（x += k * (value - x);），而下一個結果預測的偏差值就可以將p乘上1 - k後的值在加上q預測誤差值得到下一個要修正的結果，如此把Covariance遞迴下去就可以一直計算出現在的最佳結果，而且只需要從上一個保存的結果推算。


#include <math.h>

class KalmanFilter
{
   public:

      KalmanFilter(double q,double r);
      double Update(double);
      double GetK(){return k;}

   private:

      double k; //kalman gain
      double p; //estimation error cvariance
      double q; //process noise cvariance
      double r; //measurement noise covariance
      double x; //value
};

KalmanFilter::KalmanFilter(double q,double r):q(q),r(r),x(0.0)
{
  p = sqrt(q * q + r * r);
}

double KalmanFilter::Update(double value)
{
   p += q;
   k = p / (p + r);
   x += k * (value - x);
   p *= (1 - k);

   return x;
}
Used：
 
    假設預測誤差為2，感測器測量誤差為1.5（這兩個值看情況調整）。

KalmanFilter kalman(2.0,1.5);
    然後將每次測量的值用Update更新取得修正結果即可，會發現得到修正的結果會比沒修正的結果變動幅度較為平滑。



void loop()
{
  double Angle = Accelerometer();
  double k_value = kalman.Update(Angle);
}

直接用兩個OpenGL View Port其中各有一個方塊，並將角度值直接帶入World Matrix直接套用矩陣旋轉公式去作矩陣轉換，而兩方塊放在同個畫面可以方便看非使用卡爾曼濾波（螢幕左）以及使用卡爾曼濾波（螢幕右）兩個方塊振動幅度的差別。