经典卡尔曼滤波程序设计.doc

《经典卡尔曼滤波程序设计.doc》由会员上传分享，免费在线阅读，更多相关内容在行业资料-天天文库。

1、卡尔曼滤波：以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。下面程序中angle_m为测量角度，gyro_m为测量角速度，gyro_m*dt为控制量。以下程序是按卡尔曼滤波的五个公式来编写的。X(k

2、k-1)=AX(k-1

3、k-1)+BU(k)………..(1)P(k

4、k-1)=AP(k-1

5、k-1)A’+Q………(2)X(k

6、k)=X(k

7、k-1)+Kg(k)(Z(k)-HX(k

8、k-1))………(3)Kg(k)=P(k

9、k-1)H’/(HP(k

10、k-1)H’+R)………(4)P(k

11、k)=（I-Kg(k)H）P(k

12、k-1)………(

13、5)对于单输入单输出系统，A、B、H、I不为矩阵且值都为1。卡尔曼滤波参数的调整：其参数有三个，p0是初始化最优角度估计的协方差（初始化最优角度估计可设为零），它是一个初值。Q是预测值的协方差，R是测量值的协方差。对Q和R的设定只需记住，Q/(Q+R)的值就是卡尔曼增益的收敛值，比如其值为0.2，那么卡尔曼增益会向0.2收敛（对于0.2的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+0.2*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预测值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化

14、角度越接近测量值）。p0/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢（这里的p0是指初始最优角度值的协方差），因为卡尔曼增益收敛总的来说是很快的，所以该值设定大一点或小一点都没什么关系。注：以下程序只用于说明算法，存在语法错误，初始的参数也是随意给定的。x=0;/*最优角度初值*/p=1;/*最优角度对应协方差初值*/dt=0.02;Q=0.0025；R=0.25；void Kalman_Filter(float angle_m,float gyro_m)//gyro_m:gyro_measure{x=x+gyro_m*dt;等号

15、右边的x表示上一次最优角度值，等号左边的x表示这一次的角度的预测值p=p+Q;等号右边的p表示上一次最优角度值的协方差，等号左边的p表示这一次的角度预测值的协方差k=p/(p+R);k值为卡尔曼增益（k值每次计算都不一样，它会越来越趋近于Q/(Q+R)这个收敛值）x=x+k*(angle_m-x);等号左边的x表示根据预测值和测量值计算出来的这一次的最优角度值（从这里可以看出，k越大，等号左边的最优值x与等号右边的测量值angle_m越接近；k越小，等号左边的最优值x与等号右边的预测值x越接近；）p=(1-k)*p;等号左边的p表示这一次最优角度值的协方差}从

16、上面的程序可以看出，卡尔曼滤波是一个递推过程，初始的最优角度值可设为x=0，初始最优角度值的协方差p一定不能设为零，dt是采样周期，Q与R可共同决定卡尔曼增益收敛的大小。