mahony互补滤波器C编程
//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编程的更多相关文章
- 杂项-编程:AOP(面向切面编程)
ylbtech-杂项-编程:AOP(面向切面编程) 在软件业,AOP为Aspect Oriented Programming的缩写,意为:面向切面编程,通过预编译方式和运行期动态代理实现程序功能的统一 ...
- 面向切面编程AOP——加锁、cache、logging、trace、同步等这些较通用的操作,如果都写一个类,则每个用到这些功能的类使用多继承非常难看,AOP就是解决这个问题的,python AOP就是装饰器
面向切面编程(AOP)是一种编程思想,与OOP并不矛盾,只是它们的关注点相同.面向对象的目的在于抽象和管理,而面向切面的目的在于解耦和复用. 举两个大家都接触过的AOP的例子: 1)java中myba ...
- 【腾讯Bugly干货分享】WebVR如此近-three.js的WebVR示例解析
本文来自于腾讯bugly开发者社区,非经作者同意,请勿转载,原文地址:http://dev.qq.com/topic/57c7ff1689a6c9121b1adb16 作者:苏晏烨 关于WebVR 最 ...
- 安卓版App开发心得
从2016年4月到6月主要做的工作是网站的开发,而6月到现在2016年8月初,主要做的工作是Android和IOS两种App的开发,又以Android为主. 将这段时间的Android开发心得记录如下 ...
- DCM TUTORIAL – AN INTRODUCTION TO ORIENTATION KINEMATICS (REV 0.1)
原英文地址:dcm_tutorial 感觉这篇文章还是很有学习价值的,所以就抽出了一些时间对本文进行的翻译.下面这个好多人用的算法就是一种DCM 滤波器. //==================== ...
- Three.js与webVR
WebVR如此近 - three.js的WebVR示例程序解析 关于WebVR 最近VR的发展十分吸引人们的眼球,很多同学应该也心痒痒的想体验VR设备,然而现在的专业硬件价格还比较高,入手一个估计就要 ...
- Human Motion Analysis with Wearable Inertial Sensors——阅读3
Human Motion Analysis with Wearable Inertial Sensors——阅读3 四元数方向滤波器 之前的研究开发了一种自适应增益互补滤波器,并结合高斯 - 牛顿优化 ...
- 注解:大话AOP与Android的爱恨情仇
转载:大话AOP与Android的爱恨情仇 1. AOP与OOP的区别 平时我接触多的就是OOP(Object Oriented Programming面向对象).AOP(Aspect Oriente ...
- 基于GUI的小学生四则运算系统
前言:首先在此感谢我的结对搭档,没有她的帮助和引导绝不会有现在的项目.很高兴和她一起结对完成这个项目.搭档真的是棒棒哒! 一.Coding.Net项目地址: https://git.coding.ne ...
随机推荐
- Python基础__函数
本节将进入函数的介绍,函数是Python基础中最精彩的部分之一,接下来将对函数做详细介绍.函数 函数就是对代码进行一个封装.把实现某一功能的代码进行封装到一起.下次需要使用时不需要进行编写代码直接调用 ...
- linux命令类型及执行顺序
一 为什么要使用命令行 当初级Linux用户面对缺乏图形界面的Linux时很多人都会抱怨:为何要死守命令行?为什么不采用人机互交好.更简单的图形界面呢?事实上,图形界面在某些任务方面确实高效而且简 ...
- 第十一篇:Map/Reduce 工作机制分析 - 错误处理机制
前言 对于Hadoop集群来说,节点损坏是非常常见的现象. 而Hadoop一个很大的特点就是某个节点的损坏,不会影响到整个分布式任务的运行. 下面就来分析Hadoop平台是如何做到的. 硬件故障 硬件 ...
- #YCB#待做题目与填坑资料
各种填坑资料(qwq) 主席树(by YL)戳 树套树(by ZSY)戳 不要问我这些题咋来的(查大佬的水表呗) 题目列表: [HDU5977]Garden of Eden [BZOJ2752][HA ...
- [HNOI2011]XOR和路径
题面在这里 题意:给定一个无向图,从1号节点出发,每次等概率选择连接该节点的一条边走到另一个节点,到达n号节点时,将走过的路径上的所有边权异或起来,求这个异或和的期望 sol 一道期望大火题(表示看了 ...
- UML那些事
什么是UML?它的全名:Unified Modeling Language,统一建模语言.最近我用到了uml,顺便重温了下这些知识.知乎上有一个讨论话题:uml还有用吗?这个讨论挺有意思的,看完后,受 ...
- mysql初步学习
1.insert_select 的使用:从一个表复制数据给另一个表 INSERT INTO students(name,sex,LikeBooksNUM,LikesportNUM,average) S ...
- C++中 #include<>与#include""
#include<> 使用尖括号表示在包含文件目录中去查找(包含目录是由用户在设置环境时设置的),而不在源文件目录去查找: #include"" 使用双引号则表示首先在 ...
- 【C#点滴记录】ASP.NET 使用C# 导出Word 和Excel
原文摘自 慧优米网,链接地址:http://huiyoumi.wang/upload/forum.php?mod=viewthread&tid=797&extra= 好了正文来了 今天 ...
- SDP(13): Scala.Future - far from completion,绝不能用来做甩手掌柜
在前面几篇关于数据库引擎的讨论里很多的运算函数都返回了scala.Future类型的结果,因为我以为这样就可以很方便的实现了non-blocking效果.无论任何复杂的数据处理操作,只要把它们包在一个 ...