kalman卡尔曼滤波

卡尔曼滤波：以陀螺仪测量的角速度作为预测值的控制量，加速度传感器测量的角度作为观测值。下面程序中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)的值就是卡尔曼增益的收敛值，比如其值为0.2，那么卡尔曼增益会向0.2收敛（对于0.2的含义解释一下，比如预测角度值是5度，角度测量值是10度，那么最优化角度为：5+0.2*（10-5）=6。从这里可以看出，卡尔曼增益越小，说明预测值越可靠，最优化角度越接近预测值；相反的，卡尔曼增益越大，说明测量值越可靠，最优化角度越接近测量值）。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; 等号右边的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可共同决定卡尔曼增益收敛的大小。

本文来源：https://www.bwwdw.com/article/4wj5.html