卡爾曼濾波筆記

一、一維卡爾曼濾波公式

double KalmanFilterLossPr(double x) const {
#define KALMAN_Q  實測調整
#define KALMAN_R  實測調整

    double R = KALMAN_R;
    double Q = KALMAN_Q;

    static double x_last;//任意配置
    double x_mid = x_last;
    double x_now;

    static double p_last;//不能爲0
    double p_mid ;
    double p_now;

    double kg;

    x_mid = x_last;                      
    p_mid = p_last;                    

    kg = p_mid / (p_mid + R);                 
    x_now = x_mid + kg * (x - x_mid);   
    p_now = (1 - kg) * p_mid + Q;                 
    p_last = p_now;                     
    x_last = x_now;      

    return x_now;

}

二、參數說明：

Q：過程噪聲，越小系統越容易收斂，我們對模型預測的值信任度越高；但是太小則容易發散，如果Q爲零，那麼我們只相信預測值；Q值越大我們對於預測的信任度就越低，而對測量值的信任度就變高；如果Q值無窮大，那麼我們只信任測量值；

R：測量噪聲，太小太大都不一定合適。R太大，卡爾曼濾波響應會變慢，因爲它對新測量的值的信任度降低；越小系統收斂越快，但過小則容易出現震盪；

P：誤差協方差初始值，表示我們對當前預測狀態的信任度。它越小說明我們越相信當前預測狀態；它的值決定了初始收斂速度，一般開始設一個較小的值以便於獲取較快的收斂速度。隨着卡爾曼濾波的迭代，P的值會不斷的改變，當系統進入穩態之後P值會收斂成一個最小的估計方差矩陣，這個時候的卡爾曼增益也是最優的，所以這個值只是影響初始收斂速度。

三、參考

《Kalman Filter For Dummies》

《How a Kalman filter works, in pictures》

《卡爾曼濾波的簡單實現(Matlab & OC)》