- 1、本文档共6页,可阅读全部内容。
- 2、有哪些信誉好的足球投注网站(book118)网站文档一经付费(服务费),不意味着购买了该文档的版权,仅供个人/单位学习、研究之用,不得用于商业用途,未经授权,严禁复制、发行、汇编、翻译或者网络传播等,侵权必究。
- 3、本站所有内容均由合作方或网友上传,本站不对文档的完整性、权威性及其观点立场正确性做任何保证或承诺!文档内容仅供研究参考,付费前请自行鉴别。如您付费,意味着您自己接受本站规则且自行承担风险,本站不退款、不进行额外附加服务;查看《如何避免下载的几个坑》。如果您已付费下载过本站文档,您可以点击 这里二次下载。
- 4、如文档侵犯商业秘密、侵犯著作权、侵犯人身权等,请点击“版权申诉”(推荐),也可以打举报电话:400-050-0827(电话支持时间:9:00-18:30)。
查看更多
卡尔曼滤波算法的程序实现和推导过程汇编
卡尔曼滤波算法的程序实现和推导过程
---蒋海林(QQ:280586940)匈牙利裔美国数学鲁道夫·卡尔曼(Rudolf Emil Kalman)///陀螺仪积分角度(先验估计)
Angle_Final = Angle_Final + (Gyro - Q_bias) * dt;
///先验估计误差协方差的微分
Pdot[0] = Q_angle - PP[0][1] - PP[1][0];
Pdot[1] = - PP[1][1];
Pdot[2] = - PP[1][1];
Pdot[3] = Q_gyro;
///先验估计误差协方差的积分
PP[0][0] += Pdot[0] * dt;
PP[0][1] += Pdot[1] * dt;
PP[1][0] += Pdot[2] * dt;
PP[1][1] += Pdot[3] * dt;
///计算角度偏差
Angle_err = Accel - Angle_Final;
///卡尔曼增益计算
PCt_0 = C_0 * PP[0][0];
PCt_1 = C_0 * PP[1][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 * PP[0][1];
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
Angle_Final += K_0 * Angle_err; ///后验估计最优角度值
Q_bias += K_1 * Angle_err; ///更新最优估计值的偏差
Gyro_Final = Gyro - Q_bias; ///更新最优角速度值
}
我们先把卡尔曼滤波的5个方程贴上来:
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)//协方差矩阵的预测
Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R) ……… (3)//计算卡尔曼增益
X(k|k)= X(k|k-1)+Kg(k) (Z(k) - H X(k|k-1)) ……… (4)通过卡尔曼增益进行修正
P(k|k)=(I-Kg(k) H)P(k|k-1) ……… (5)//更新协方差阵
这5个方程比较抽象,下面我们就来把这5个方程和上面的程序对应起来。
—,对于角度来说,我们认为此时的角度可以近似认为是上一时刻的角度值加上上一时刻陀螺仪测得的角速度乘以时间,因为,角度微分等于时间的微分乘以角速度。
但是陀螺仪有个静态漂移(而且还是变化的),静态漂移就是静止了没有角速度然后陀螺仪也会输出一个值,这个值肯定是没有意义的,计算时要把它减去。由此我们得到了当前角度的预测值Angle_Final:
Angle_Final = Angle_Final + (Gyro - Q_bias) * dt;
其中等号左边Angle_Final为此时的角度,等号右边Angle_Final为上一时刻的角度,Gyro为陀螺仪测的角速度的值,dt是两次滤波之间的时间间隔。
#define dt 0.01
这是程序中的定义,同时零漂Q_bias也是一个变化的量。
但是就预测来说认为现在的漂移跟上一时刻是相同的即
Q_bias=Q_bias
将两个式子写成矩阵的形式
这个式子对应于卡尔曼滤波的第一个式子
X(k|k-1)=A X(k-1|k-1)+B U(k) ………(1)//先验估计
X(k|k-1)为2维列向量,A为2维方阵,X(k-1|k-1)为2维列向量,B 为2维列向量,U(k) 为Gyro
二,这里是卡尔曼滤波的第二个式子
接着是预测方差阵的预测值,这里首先要给出两个值,一个是漂移的噪声,一个是角度值的噪声,(所谓噪声就是数据的方差值)
P(k|k-1)=A P(k-1|k-1) A’+Q
这里的Q为向量的协方差矩阵,即因为漂移噪声和角度噪声是相互独立的,则=0;=0;
又由性质可知cov(x,x)=D(x)即方差,所以得到的矩阵如下:
这里的两个方差值是开始就给定的常数,程序中定义如下:
#define Q_angle 0.001 ////角度过程噪声的协方差
#define Q_gyro 0.
您可能关注的文档
最近下载
- 数字化转型背景下高职课堂改革路径研究.docx VIP
- 实验室安全操作培训内容.docx VIP
- DBJ61T72-2012城市道路交通管理设施设置技术规范—第5部分:交通监控.pdf VIP
- 搅拌器设计选型.pdf VIP
- 2024年医疗机构病历管理规定.docx VIP
- 医院总值班应急管理职责及流程规章制度.docx VIP
- 2023年贵州省毕节七星关区人民政府办公室考调下属事业单位人员9人考前自测高频考点模拟试题(共500题)含答案详解.docx VIP
- 湖南科技大学开题报告PPT模板.pptx VIP
- 四年级上习题四年级上英语单词和句子.doc VIP
- 全国各地省市县区域列表(2)_4079474.xls VIP
文档评论(0)