//gx...分别为重力加速度在三个轴向的分力 由加速度计测得

//ax...分别为角速度在三个轴向的角速度 由陀螺仪测得

//最后得到最终滤波完毕的x、y、z方向的角度值(°)

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)
{
float norm;
// float hx, hy, hz, bx, bz;
float vx, vy, vz;// wx, wy, wz;
float ex, ey, ez; // 先把这些用得到的值算好
float q0q0 = q0*q0;
float q0q1 = q0*q1;
float q0q2 = q0*q2;
// float q0q3 = q0*q3;
float q1q1 = q1*q1;
// float q1q2 = q1*q2;
float q1q3 = q1*q3;
float q2q2 = q2*q2;
float q2q3 = q2*q3;
float q3q3 = q3*q3; if(ax*ay*az==0)
return; norm = sqrt(ax*ax + ay*ay + az*az); //acc数据归一化
ax = ax /norm;
ay = ay / norm;
az = az / norm; // estimated direction of gravity and flux (v and w) 估计重力方向和流量/变迁
vx = 2*(q1q3 - q0q2); //四元素中xyz的表示
vy = 2*(q0q1 + q2q3);
vz = q0q0 - q1q1 - q2q2 + q3q3 ; // error is sum of cross product between reference direction of fields and direction measured by sensors
ex = (ay*vz - az*vy) ; //向量外积在相减得到差分就是误差
ey = (az*vx - ax*vz) ;
ez = (ax*vy - ay*vx) ; exInt = exInt + ex * Ki; //对误差进行积分
eyInt = eyInt + ey * Ki;
ezInt = ezInt + ez * Ki; // adjusted gyroscope measurements
gx = gx + Kp*ex + exInt; //将误差PI后补偿到陀螺仪,即补偿零点漂移
gy = gy + Kp*ey + eyInt;
gz = gz + Kp*ez + ezInt; //这里的gz由于没有观测者进行矫正会产生漂移,表现出来的就是积分自增或自减 // integrate quaternion rate and normalise //四元素的微分方程
q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;
q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;
q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;
q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; // normalise quaternion
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 = q0 / norm;
q1 = q1 / norm;
q2 = q2 / norm;
q3 = q3 / norm; Q_ANGLE.Yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* 57.3; // yaw
Q_ANGLE.Y = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3; // pitch
Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
}

mahony互补滤波器C编程的更多相关文章

  1. 杂项-编程:AOP(面向切面编程)

    ylbtech-杂项-编程:AOP(面向切面编程) 在软件业,AOP为Aspect Oriented Programming的缩写,意为:面向切面编程,通过预编译方式和运行期动态代理实现程序功能的统一 ...

  2. 面向切面编程AOP——加锁、cache、logging、trace、同步等这些较通用的操作,如果都写一个类,则每个用到这些功能的类使用多继承非常难看,AOP就是解决这个问题的,python AOP就是装饰器

    面向切面编程(AOP)是一种编程思想,与OOP并不矛盾,只是它们的关注点相同.面向对象的目的在于抽象和管理,而面向切面的目的在于解耦和复用. 举两个大家都接触过的AOP的例子: 1)java中myba ...

  3. 【腾讯Bugly干货分享】WebVR如此近-three.js的WebVR示例解析

    本文来自于腾讯bugly开发者社区,非经作者同意,请勿转载,原文地址:http://dev.qq.com/topic/57c7ff1689a6c9121b1adb16 作者:苏晏烨 关于WebVR 最 ...

  4. 安卓版App开发心得

    从2016年4月到6月主要做的工作是网站的开发,而6月到现在2016年8月初,主要做的工作是Android和IOS两种App的开发,又以Android为主. 将这段时间的Android开发心得记录如下 ...

  5. DCM TUTORIAL – AN INTRODUCTION TO ORIENTATION KINEMATICS (REV 0.1)

    原英文地址:dcm_tutorial 感觉这篇文章还是很有学习价值的,所以就抽出了一些时间对本文进行的翻译.下面这个好多人用的算法就是一种DCM 滤波器. //==================== ...

  6. Three.js与webVR

    WebVR如此近 - three.js的WebVR示例程序解析 关于WebVR 最近VR的发展十分吸引人们的眼球,很多同学应该也心痒痒的想体验VR设备,然而现在的专业硬件价格还比较高,入手一个估计就要 ...

  7. Human Motion Analysis with Wearable Inertial Sensors——阅读3

    Human Motion Analysis with Wearable Inertial Sensors——阅读3 四元数方向滤波器 之前的研究开发了一种自适应增益互补滤波器,并结合高斯 - 牛顿优化 ...

  8. 注解:大话AOP与Android的爱恨情仇

    转载:大话AOP与Android的爱恨情仇 1. AOP与OOP的区别 平时我接触多的就是OOP(Object Oriented Programming面向对象).AOP(Aspect Oriente ...

  9. 基于GUI的小学生四则运算系统

    前言:首先在此感谢我的结对搭档,没有她的帮助和引导绝不会有现在的项目.很高兴和她一起结对完成这个项目.搭档真的是棒棒哒! 一.Coding.Net项目地址: https://git.coding.ne ...

随机推荐

  1. 如何降低90%Java垃圾回收时间?以阿里HBase的GC优化实践为例

    过去的一年里,我们准备在Ali-HBase上突破这个被普遍认知的痛点,为此进行了深度分析及全面创新的工作,获得了一些比较好的效果.以蚂蚁风控场景为例,HBase的线上young GC时间从120ms减 ...

  2. 智能合约语言 Solidity 教程系列8 - Solidity API

    这是Solidity教程系列文章第8篇介绍Solidity API,它们主要表现为内置的特殊的变量及函数,存在于全局命名空间里. 写在前面 Solidity 是以太坊智能合约编程语言,阅读本文前,你应 ...

  3. js,获取和设置cookie、 localStorage

    cookie 跟域名有关系的浏览器缓存 获取cookie document.cookie; 获取到的如果是多条cookie信息,是以分号和一个空格("; ")隔开:a=1; b=2 ...

  4. 三大家族,offset,scroll,client

    1.client 1.1主要成员 1.clientWidth 获取网页可视区域宽度(两种用法)    clientHeight 获取网页可视区域高度 (两张用法) 盒子调用: 指盒子本省 浏览器调用: ...

  5. CYQ.data 框架结构

    -------------------------V5.0开始(刚开始过滤版本:有些更新功能迁到V4,所以记录在V4那)-----------------------------7:Insert方法增 ...

  6. [转]Thunderbird 使用 Exchange 邮箱

    [转]Thunderbird 使用 Exchange 邮箱 http://my.oschina.net/MaTech/blog/295238#OSC_h3_1 公司最近邮箱只支持Exchange模式, ...

  7. 数据操纵:SELECT, INSERT, UPDATE, DELETE

    SELECT 句法 SELECT [STRAIGHT_JOIN] [SQL_SMALL_RESULT] [SQL_BIG_RESULT] [SQL_BUFFER_RESULT] [SQL_CACHE ...

  8. 记录使用CI框架开发项目时遇到的问题

    关于CI框架在视图文件中怎样引入静态资源文件(js,css,images)的问题: 第一步:在application/config/config.php文件中配置  $config['base_url ...

  9. S/4 HANA中的ACDOCT和FAGLFLEXT

    最近的几个需求让我对ACDOCT和FAGLFLEXT这两个财务相关表(准确地说是视图)产生了一些了解,同时也发现某些开发同行和业务顾问并没有认识到这些东西.因此打算从技术角度来说明一下这两个视图在S4 ...

  10. spy++捕获窗口消息

    打开spy++,窗口截图如下,点击窗口搜索按钮(红框标识) ,如果找不到对应的窗口,鼠标右键刷新即可. 鼠标左键点击窗口搜索图标,按住不放,拖到需要抓取消息的窗口上: spy++会自动在列表中高亮定位 ...