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 ...
随机推荐
- 数列分块总结——题目总版(hzwer分块九题及其他题目)(分块)
闲话 莫队算法似乎还是需要一点分块思想的......于是我就先来搞分块啦! 膜拜hzwer学长神犇%%%Orz 这九道题,每一道都堪称经典,强力打Call!点这里进入 算法简述 每一次考试被炸得体无完 ...
- 【CJOJ1167】【洛谷1894】[USACO4.2]完美的牛栏
题面 Description 农夫约翰上个星期刚刚建好了他的新牛棚,他使用了最新的挤奶技术.不幸的是,由于工程问题,每个牛栏都不一样.第一个星期,农夫约翰随便地让奶牛们进入牛栏,但是问题很快地显露出来 ...
- Android OpenGL ES 开发(九): OpenGL ES 纹理贴图
一.概念 一般说来,纹理是表示物体表面的一幅或几幅二维图形,也称纹理贴图(texture).当把纹理按照特定的方式映射到物体表面上的时候,能使物体看上去更加真实.当前流行的图形系统中,纹理绘制已经成为 ...
- APNS IOS 消息推送沙盒模式和发布模式
在做.NET向IOS设备的App进行消息推送时候,采用的是PushSharp开源类库进行消息的推送,而在开发过程中,采用的是测试版本的app,使用的是测试的p12证书采用的是ApnsConfigura ...
- Could not get dialect instance.
一般此错误都是和数据库有关,请确认数据库配置文件是否配置正确,或者确认数据库是否连接正常
- Linux环境下安装weblogic10.3.2
附件:weblogic10.3.2版本zip包和部署war包提取地址: weblogic.zip:https://pan.baidu.com/s/1Y1tmAhlXsO5Q4zPvOKGG5g 提取 ...
- MSIL实用指南-创建字段
本篇讲解怎么创建字段,主要是在修饰符的创建上. 创建字段的方法是TypeBuilder.DefineField,传入字段名称.字段类型.字段修饰符等参数,返回一个FieldBuilder对象.先看这一 ...
- tp框架的MVC模式
一.定义 MVC全名是Model View Controller,是模型(model)-视图(view)-控制器(controller)的缩写,一种软件设计典范,用一种业务逻辑.数据.界面显示分离的方 ...
- Android Studio报错:the selected directory is not a valid home for unknow sdk
今天在使用Android Studio的时候不知道怎么了,没有import module,视图里面也没有android视图,查看project设置.提示我的SDK路径无效:the selected d ...
- ASP.NET MVC编程——控制器
每一个请求都会经过控制器处理,控制器中的每个方法被称为控制器操作,它处理具体的请求. 1操作输入参数 控制器的操作的输入参数可以是内置类型也可以是自定义类型. 2操作返回结果 结果类型 调用方法 备注 ...