卡尔曼滤波算法总结_第1页
卡尔曼滤波算法总结_第2页
卡尔曼滤波算法总结_第3页
免费预览已结束,剩余1页可下载查看

下载本文档

版权说明:本文档由用户提供并上传,收益归属内容提供方,若内容存在侵权,请进行举报或认领

文档简介

1、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 * PP10;E = R_angle + C_0 * PCt_0

2、;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;首先是卡尔曼滤波的 5 个方程:1)先验估计2)协方差矩阵的预测X(k |k 1) AX(k 1|k 1) Bu(k)P(k |k 1) AP(k 1|k 1)A' Q3)计算卡尔

3、曼增益4)进行修正5)更新协方差阵Kg(k) P(k |k 1)H '/ HP(k|k 1)H ' R)X(k |k) X(k|k 1) Kg(k)(Z(k) HX (k|k 1)P(k|k) (I Kg(k)H)P(k |k 1)5个式子比较抽象,现在直接用实例来说:一、卡尔曼滤波第一个式子对于角度来说, 我们认为此时的角度可以近似认为是上一时刻的角度值加上 上一时刻陀螺仪测得的角加速度值乘以时间,因为 d dt ,角度微分等于时 间的微分乘以角速度。但是陀螺仪有个静态漂移(而且还是变化的) ,静态漂移 就是静止了没有角速度然后陀螺仪也会输出一个值,这个值肯定是没有意义的,

4、计算时要把它减去。由此我们得到了当前角度的预测值 AngleAngle=Angle+(Gyro - Q_bias) * dt;其中等号左边 Angle 为此时的角度,等号右边 Angle 为上一时刻的角度, Gyro 为陀螺仪测的角速度的值, dt 是两次滤波之间的时间间隔,我们的运行周期是 4ms或者 6ms。同时 Q_bias 也是一个变化的量。 但是就预测来说认为现在的漂移跟上一时刻是相同的,即Q_bias=Q_bias 将上面两个式子写成矩阵的形式Angledt AngleQ _ bias1 Q_biasdtGyro0得到上式,这个式子对应于卡尔曼滤波的第一个式子X(k |k 1) A

5、X(k 1|k 1) Bu(k)X k|k 1 为 2 维列向量AngleQ _ bias,A为 2 维方阵dtX k-1|k 1 为 2 维列向量 AngleQ_biasB 为 2 维列向量dt0u k 为 Gyro卡尔曼滤波第二个式子接着是预测方差阵的预测值,这里首先要给出两个值,一个是漂移的噪声,一个是角度值的噪声, (所谓噪声就是数据的方差值)P(k|k 1) AP(k 1| k 1)A' Q这里的 Q为向量Angle 的协方差矩阵, 即Q _ biascov(Angle,Angle) cov(Q_bias,Angle)cov(Angle,Q_bias)cov(Q_bias,Q

6、_bias)因为漂移噪声和角度噪声是相互独立的,则 cov(Angle,Q_bias) 0又由性质可知 cov( x, x) D(x) 即方差,所以得到的矩阵如下D(Angle) 00 D(Q_bias) ,这里的两个方差值是开始就给出的常数 程序中的定义如下float Q_angle=;float Q_gyro=;接着是这一部分 A P(k-1|k- 1) A ', 其中的( P(k-1 )|P(k-1) )为上一时刻的 预测方差阵 卡尔曼滤波的目标就是要让这个预测方差阵最小。其中 P(k-1|k-1)设为,第1 式已知 A 为0dt1则计算 A P(k-1|k- 1) A '

7、;+Q(就是个矩阵乘法和加法,算算吧)结果如下D(Angle) b d dt d2 a c dt b dt d.(dt)2 c d dt2d.(dt) 很小为了计算简便忽略不计。 于是得到a c dt b dt D(Angle) b d dtc d dt da,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 += Pdot2 * d

8、t;PP11 += Pdot3 * dt;三,这里是卡尔曼滤波的第三个式子Kg(k)= P(k|k- 1) H' / (H P(k|k -1) H' + R)k0(3) 0 H 1k1P00P10K0K1注意这个是加速度计算出来的角度Angle_err = Accel - Angle;对应程序如下Angle += K_0 * 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=; float Q_gyro=

温馨提示

  • 1. 本站所有资源如无特殊说明,都需要本地电脑安装OFFICE2007和PDF阅读器。图纸软件为CAD,CAXA,PROE,UG,SolidWorks等.压缩文件请下载最新的WinRAR软件解压。
  • 2. 本站的文档不包含任何第三方提供的附件图纸等,如果需要附件,请联系上传者。文件的所有权益归上传用户所有。
  • 3. 本站RAR压缩包中若带图纸,网页内容里面会有图纸预览,若没有图纸预览就没有图纸。
  • 4. 未经权益所有人同意不得将文件中的内容挪作商业或盈利用途。
  • 5. 人人文库网仅提供信息存储空间,仅对用户上传内容的表现方式做保护处理,对用户上传分享的文档内容本身不做任何修改或编辑,并不能对任何下载内容负责。
  • 6. 下载文件中如有侵权或不适当内容,请与我们联系,我们立即纠正。
  • 7. 本站不保证下载资源的准确性、安全性和完整性, 同时也不承担用户因使用这些下载资源对自己和他人造成任何形式的伤害或损失。

评论

0/150

提交评论