2022年2022年卡尔曼滤波算法总结 .pdf

《2022年2022年卡尔曼滤波算法总结 .pdf》由会员分享，可在线阅读，更多相关《2022年2022年卡尔曼滤波算法总结 .pdf（5页珍藏版）》请在淘文阁 - 分享文档赚钱的网站上搜索。

1、2015.12.12 void Kalman_Filter(float Gyro,float Accel) Angle+=(Gyro - Q_bias) * dt; Pdot0=Q_angle - PP01 - PP10; Pdot1= - PP11; Pdot2= - PP11; Pdot3=Q_gyro; PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 += Pdot2 * dt; PP11 += Pdot3 * dt; Angle_err = Accel - Angle; PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP1

2、0; E = R_angle + C_0 * PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = C_0 * PP01; PP00 -= K_0 * t_0; PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= K_1 * t_1; Angle += K_0 * Angle_err; Q_bias += K_1 * Angle_err; Gyro_x = Gyro - Q_bias; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - -

3、 - - 名师精心整理 - - - - - - - 第 1 页，共 5 页 - - - - - - - - - 首先是卡尔曼滤波的5 个方程：( |1)(1|1)( )X k kAX kkBu k（1）先验估计(|1)(1|1) P k kAP kkAQ（2）协方差矩阵的预测( )( |1)/( |1)Kg kP k kHHP k kHR（3）计算卡尔曼增益( | )( |1)( )( )(|1)X k kX k kKg kZ kHX k k（4）进行修正5 个式子比较抽象，现在直接用实例来说：一、卡尔曼滤波第一个式子对于角度来说， 我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻

4、陀螺仪测得的角加速度值乘以时间，因为ddt，角度微分等于时间的微分乘以角速度。但是陀螺仪有个静态漂移（而且还是变化的），静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值，这个值肯定是没有意义的，计算时要把它减去。由此我们得到了当前角度的预测值AngleAngle=Angle+(Gyro - Q_bias) * dt; 其中等号左边Angle 为此时的角度，等号右边Angle 为上一时刻的角度， Gyro 为陀螺仪测的角速度的值，dt 是两次滤波之间的时间间隔，我们的运行周期是4ms或者 6ms。同时 Q_bias也是一个变化的量。但是就预测来说认为现在的漂移跟上一时刻是相同的，即Q_bia

5、s=Q_bias 将上面两个式子写成矩阵的形式1_01_0Angledt AngledtQbiasQbiaosGyr得到上式，这个式子对应于卡尔曼滤波的第一个式子(|1)(1|1)( )X k kAX kkBu k|1X k k为 2 维列向量_AngleQbias，A 为 2 维方阵101dt，|-11X kk为 2 维列向量_AngleQbias，B 为 2 维列向量0dt，u k 为Gyro(| )(|1)P k kIKg kHP k k（ ） ）（5）更新协方差阵名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整

6、理 - - - - - - - 第 2 页，共 5 页 - - - - - - - - - 二、卡尔曼滤波第二个式子接着是预测方差阵的预测值，这里首先要给出两个值，一个是漂移的噪声，一个是角度值的噪声， （所谓噪声就是数据的方差值）(|1)(1|1)P k kAP kkAQ这里的 Q 为向量_AngleQbias的协方差矩阵，即cov(Angle,Angle) cov(Q_bias,Angle)cov(Angle,Q_bias)cov(Q_bias,Q_bias)，因为漂移噪声和角度噪声是相互独立的，则cov(Angle,Q_bias)0。又由性质可知cov( , )( )x xD x即方差，

7、所以得到的矩阵如下D(Angle)00D(Q_bias)，这里的两个方差值是开始就给出的常数程序中的定义如下float Q_angle=0.001; float Q_gyro=0.003; 接着是这一部分 A P(k-1|k-1) A,其中的（ P（k-1）|P(k-1)）为上一时刻的预测方差阵卡尔曼滤波的目标就是要让这个预测方差阵最小。其中 P(k-1|k-1)设为abcd,第一式已知 A 为101dt则计算 A P(k-1|k-1) A +Q（就是个矩阵乘法和加法，算算吧）结果如下2.(dt)(Angle)acdtbdtdDbddtcddtd2.(dt)d很小为了计算简便忽略不计。于是得到

8、(Angle)acdtbdtDbddtcddtd名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 3 页，共 5 页 - - - - - - - - - a,b,c,d分别和矩阵的 P00,P01,P10,P11 计算过程转化为如下程序，代换即可Pdot0=Q_angle - PP01 - PP10; Pdot1= - PP11; Pdot2= - PP11;/ Pdot3=Q_gyro; PP00 += Pdot0 * dt; PP01 += Pdot1 * dt; PP10 +=

9、Pdot2 * dt; PP11 += Pdot3 * dt; 三，这里是卡尔曼滤波的第三个式子Kg(k)= P(k|k-1) H / (H P(k|k-1) H + R) (3)/计算卡尔曼增益即计算卡尔曼增益， 这是个二维向量设为01kk， 这里的H=10为由此 kg= P(K|K-1)+R，这里又有一个常数R，程序中的定义如下float R_angle=0.5; 这个指的是角度测量噪声值，则式子的分母=P00+R_angle 即程序中的PCt_0 = C_0 * PP00; PCt_1 = C_0 * PP10; E = R_angle + C_0 * PCt_0; 分子0010PP于是

10、求出01KKK_0 = PCt_0 / E; 名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 4 页，共 5 页 - - - - - - - - - K_1 = PCt_1 / E; 四，用误差还有卡尔曼增益来修正X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1) (4)通过卡尔曼增益进行修正这个矩阵带进去就行了Z（k)=Accel.注意这个是加速度计算出来的角度Angle_err = Accel - Angle; 对应程序如下Angle += K_0

11、 * Angle_err; Q_bias += K_1 * Angle_err; 同时为了 PID 控制还有下次的使用把角速度算出来了Gyro_x = Gyro - Q_bias; 五，最后一步对矩阵P 进行更新，因为下一次滤波时要用到PP00 -= K_0 * t_0; PP01 -= K_0 * t_1; PP10 -= K_1 * t_0; PP11 -= K_1 * t_1; P(k|k)=（I-Kg(k) H ）P(k|k-1) (5)/ 跟预测方差阵这个很简单，矩阵带进去算就行了六，总结卡尔曼滤波一共只需要给很少的初始值量，float Q_angle=0.001; float Q_gyro=0.003; 还有 float R_angle=0.5; 以及系统的初始量angle还有 Q_bias 还有预测误差矩阵P，程序里给的是 0（数组）理论上由于卡尔曼滤波是迭代的算法，当时间充分长以后。 滤波估值将与初始值的选取无关。但是实际上并不是如此，比如测量方差值一直在变化。名师资料总结 - - -精品资料欢迎下载 - - - - - - - - - - - - - - - - - - 名师精心整理 - - - - - - - 第 5 页，共 5 页 - - - - - - - - -