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 ...
随机推荐
- Java 8 Date-Time API 详解
从Java版本1.0开始就支持日期和时间,主要通过java.util.Date类. 但是,Date类设计不佳. 例如,Date中的月份从1开始,但从日期却从0开始.在JDK 1.1中使用它的许多方法已 ...
- WPF自学入门(四)WPF路由事件之自定义路由事件
在上一遍博文中写到了内置路由事件,其实除了内置的路由事件,我们也可以进行自定义路由事件.接下来我们一起来看一下WPF中的自定义路由事件怎么进行创建吧. 创建自定义路由事件分为3个步骤: 1.声明并注册 ...
- I/HwPointEventFilter: do not support AFT because of no config
I/HwPointEventFilter: do not support AFT because of no config 这是华为对系统做了修改,默认不打印日志,要改配置 在拨号界面输入:以下进入工 ...
- [SDOI2015]约数个数和
Sol 首先有个结论 \(\sum_{i=1}^{m}\sum_{j=1}^{n}d(i*j)=\sum_{i=1}^{m}\sum_{j=1}^{n}\sum_{x|i}\sum_{y|i}[gcd ...
- 关于在Django中Json无法序列化datetime的解决办法
我们在网页设计时经常会在前端和后台进行交互,前端回传的方法可以时redirect一个地址加上显式的参数,第二个办法就是使用Ajax结构.那么在传到view函数中进行处理后是需要通过Json格式进行返回 ...
- log4j日志输出性能优化-缓存、异步
转载 1.log4j已成为大型系统必不可少的一部分,log4j可以很方便的帮助我们在程序的任何位置输出所要打印的信息,便于我们对系统在调试阶段和正式运行阶段对问题分析和定位.由于日志级别的不同,对系 ...
- 利用Cglib实现AOP
前文讲了, 可以利用Spring, Guice等框架提供的容器实现AOP, 如果想绕过容器, 直接注入Class, 可以利用Cglib为对象加上动态代理,实现代码切入, 但是每次调用比较繁琐, 因此我 ...
- css中的注意项,可能会帮助到大家哦!
CSS样式层叠表 1.link与@import的区别(5点) (1).link为XHTML的标签,可以引进CSS样式表,除了引进CSS文件还可以引进其他的文件如.js或.rss文件;@import为C ...
- tp框架-----文件上传
之前也做过文件上传,现在学了tp,用tp怎么做呢? 第一步:做一个Wenjian控制器: <?php namespace Ceshi\Controller; use Think\Controll ...
- centos7上安装ELK
author:headsen chen data :2017-12-04 18:00:57 notice:created by chen himself and not allowed to cop ...