EEPW论坛

一，定义介绍

卡尔曼滤波是一种利用线性的系统方程，通过系统的输入输出来观测数据，对系统状态进行最优估计的一种算法。但是由于观测系统中存在着噪声与干扰，所以也可以看作是滤波求最优解的过程。

卡尔曼滤波在理想状态下，我们可以认为

滤波就是加权：信号*1+噪声*0

卡尔曼滤波：估计值*1+观测值*0

二，公式介绍

卡尔曼滤波算法公式

状态方程：

X(k)=A X(k-1)+B U(k)+W(k)

再加上系统传感器的观测测值：

Y(k)=H X(k)+V(k)

其实现过程为：

使用上一次的最优结果预测当前的值，同时使用观测值（传感器测量值）修正当前值的到最大最优结果。

X（k）k时刻系统状态

Z(k)  k时刻的测量值

U（k）k时刻系统的控制量

H  测量系统参数

A/F  状态转移矩阵

W（k)过程噪声方差Q

B控制矩阵

V（k)测量方差R

三，代码实现如下

/*--------------------------------------------------------------------*/
/*        
        Q:过程噪声，Q增大，动态响应变快，收敛稳定性变坏
        R:测量噪声，R增大，动态响应变慢，收敛稳定性变好        
*/
double KalmanFilter(const double ResrcData,
                                        double ProcessNiose_Q,double MeasureNoise_R,double InitialPrediction)
{
        double R = MeasureNoise_R;
        double Q = ProcessNiose_Q;
        static        double x_last;
        double x_mid = x_last;
        double x_now;
        static        double p_last;
        double p_mid ;
        double p_now;
        double kg;        
        x_mid=x_last; //x_last=x(k-1|k-1),x_mid=x(k|k-1)
        p_mid=p_last+Q; //p_mid=p(k|k-1),p_last=p(k-1|k-1),Q=噪声
        kg=p_mid/(p_mid+R); //kg为kalman filter，R为噪声
        x_now=x_mid+kg*(ResrcData-x_mid);//估计出的最优值
                
        p_now=(1-kg)*p_mid;//最优值对应的covariance        
        p_last = p_now; //更新covariance值
        x_last = x_now; //更新系统状态值
        return x_now;                
}