陀螺仪+加速度计卡尔曼滤波.doc_第1页
陀螺仪+加速度计卡尔曼滤波.doc_第2页
陀螺仪+加速度计卡尔曼滤波.doc_第3页
全文预览已结束

下载本文档

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

文档简介

/float gyro_m：陀螺仪测得的量（角速度）/float incAngle:加计测得的角度值#define dt 0.0015/卡尔曼滤波采样频率 #define R_angle 0.69 /测量噪声的协方差（即是测量偏差）#define Q_angle 0.0001/过程噪声的协方差 #define Q_gyro 0.0003 /过程噪声的协方差 过程噪声协方差为一个一行两列矩阵float kalmanUpdate(const float gyro_m,const float incAngle) float K_0;/含有卡尔曼增益的另外一个函数，用于计算最优估计值 float K_1;/含有卡尔曼增益的函数，用于计算最优估计值的偏差 float Y_0; float Y_1; float Rate;/去除偏差后的角速度 float Pdot4;/过程协方差矩阵的微分矩阵 float angle_err;/角度偏量 float E;/计算的过程量 static float angle = 0; /下时刻最优估计值角度 static float q_bias = 0; /陀螺仪的偏差 static float P22 = 1, 0 , 0, 1 ;/过程协方差矩阵 Rate = gyro_m - q_bias; /计算过程协方差矩阵的微分矩阵 Pdot0 = Q_angle - P01 - P10;/？ Pdot1 = - P11; Pdot2 = - P11; Pdot3 = Q_gyro;/？ angle += Rate * dt; /角速度积分得出角度 P00 += Pdot0 * dt; /计算协方差矩阵 P01 += Pdot1 * dt; P10 += Pdot2 * dt; P11 += Pdot3 * dt; angle_err = incAngle - angle; /计算角度偏差 E = R_angle + P00; K_0 = P00 / E; /计算卡尔曼增益 K_1 = P10 / E; Y_0 = P00; Y_1 = P01; P00 -= K_0 * Y_0; /跟新协方差矩阵 P01 -= K_0 * Y_1; P10 -= K_1 * Y_0; P11 -= K_1 * Y_1; angle += K_0 * angle_err; /给出最优估计
人人文库> 全部分类> 生活休闲 > 科普知识

温馨提示

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

最新文档

评论

0/150

提交评论