经典卡尔曼滤波程序设计

卡尔曼滤波：以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。下面程序中angle_m为测量角度，gyro_m为测量角速度，gyro_m*dt为控制量。

以下程序是按卡尔曼滤波的五个公式来编写的。

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

P(k|k-1)=A P(k-1|k-1) A’+Q (2)

X(k|k)= X(k|k-1)+Kg(k) (Z(k)-H X(k|k-1)) (3)

Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) (4)

P(k|k)=（I-Kg(k) H）P(k|k-1) (5)

对于单输入单输出系统，A、B、H、I不为矩阵且值都为1。

卡尔曼滤波参数的调整：其参数有三个，p0是初始化最优角度估计的协方差（初始化最优角度估计可设为零），它是一个初值。Q是预测值的协方差，R是测量值的协方差。对Q和R的设定只需记住，Q/(Q+R)的值就是卡尔曼增益的收敛值，比如其值为，那么卡尔曼增益会向收敛（对于的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预测值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值）。p0/(Q+R)反映收敛的快慢程度，该值设定越小，收敛越快，该值越大，收敛越慢（这里的p0是指初始最优角度值的协方差），因为卡尔曼增益收敛总的来说是很快的，所以该值设定大一点或小一点都没什么关系。

注：以下程序只用于说明算法，存在语法错误，初始的参数也是随意给定的。

x=0;/* 最优角度初值*/

p=1;/* 最优角度对应协方差初值*/

dt=;

Q=；

R=；

voidKalman_Filter(floatangle_m,floatgyro_m) //gyro_m:gyro_measure

{

x=x+ gyro_m*dt; 等号右边的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表示这一次最优角度值的协方差

}

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