卡尔曼滤波笔记

一、一维卡尔曼滤波公式

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)》