匿名mahony互补滤波代码详解

下面是我对匿名源码修改整理后得到的代码，去掉了冗余便于讲解，此外给出了一些优化建议。
一、函数调用

二、Drv_Icm20602_Read()

读取16位ADC采样值

三、imu_origDataPrepare()

因为传感器安装角度和载体坐标系可能不一致，因此需要一个基础的坐标转换，最终得到x前、y右、z下的载体坐标系。

因为加速度计对高频抖动敏感，因此需要一个低通滤波器。陀螺仪加一个低通滤波无伤大雅。

当传感器不在载体中心时，载体旋转时传感器会有一个向心加速度，该加速度会被传感器测量出来，因此要去掉。

将16位ADC转换为物理单位值，对于该例程为±16g的加速度，单位是cm/s^2，就是说重力加速度测出来的值是980；角速度为±2000dps（degree per second）。

注：
之后使用时建议根据场景降低量程，提高精度

四、imu_postureSolution()
姿态解算的基本思路就是更新四元数，并且利用四元数到旋转矩阵的公式更新旋转矩阵。

下面是具体的实现：

初始时要求使模长为1，然后通过迭代得到正确的四元数。

这个是旋转矩阵的定义，最终求解欧拉角是利用该旋转矩阵。

读取载体坐标系下的加速度，并单位化。

利用四元数求旋转矩阵，同时也获得了世界坐标系下重力加速度(0,0,1)T在载体坐标系下的各轴分量。

求PI积分值。这里使用测量的角速度加上误差。由于误差是叉积，无量纲，因此可以理解为与角速度同一量纲，然后乘上时间就是更新的角度量。

说明：
1.初始四元数随意给定，因此刚开始会有一个较长的迭代时间将四元数收敛到正确的值。解决办法是刚开始（或判断无人机停止飞行时），将PI参数调大，使之快速收敛；
2.为了得到更好的效果，应当在初始开机后对加速度计、角速度计求零偏，即使用较长的时间求测量值的平均值，对之后的数据减去这个零偏。这个的好处包括：增大PI响应速度，在没有磁力计的情况下减小yaw漂移。

注：向量积无法区分0-90度和90-180度：

因此使用向量积作为误差，姿态角roll和pitch取值范围为±90度。
yaw之所以是±180度，是因为还增加了点乘判断向量方向：

五、imu_getEuler()

通过旋转矩阵求解欧拉角：
atan2相关知识汇总_weixin_42142612的博客-CSDN博客
https://blog.csdn.net/weixin_42142612/article/details/80972768
欧拉角和旋转矩阵的相互转换 - summer91 - 博客园
https://www.cnblogs.com/larry-xia/p/11926121.html

匿名mahony互补滤波代码详解相关推荐

曲率高斯滤波去噪python实现（附代码详解）
曲率高斯滤波去噪python实现(附代码详解) 曲率滤波的理论基础可以参考下曲率滤波的理论基础和应用,这篇博客介绍的思想完美的避开了一大堆数学公式,简直是我的福音,但还是要细看的,不然很容易忽略重点, ...
委托与事件代码详解与（Object sender,EventArgs e）详解
委托与事件代码详解 using System; using System.Collections.Generic; using System.Text; namespace @Delegate //自 ...
java调用js匿名函数参数,js匿名函数作为函数参数详解
由衷的感叹,js真是烦. 学到现在,渐渐理解了什么是:语言都是通用的,没有好不好,只有擅长不擅长. 继承,多态,甚至指针,c能实现,c++,java有,javascript(和java是雷锋和雷峰塔的 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...
go结构体初始化_go嵌套匿名结构体的初始化详解
go匿名结构体嵌套匿名结构体的示例代码片. type debugConfig struct { MaxQueueDepth int `json:"maxQueueDepth"` ...
【2023年第十一届泰迪杯数据挖掘挑战赛】B题：产品订单的数据分析与需求预测建模及python代码详解问题二
相关链接 [2023年第十一届泰迪杯数据挖掘挑战赛]B题:产品订单的数据分析与需求预测建模及python代码详解问题一 [2023年第十一届泰迪杯数据挖掘挑战赛]B题:产品订单的数据分析与需求预测 ...
委托与事件代码详解与（Object sender,EventArgs e）
http://apps.hi.baidu.com/share/detail/30334344 委托与事件代码详解与(Object sender,EventArgs e)详解 2008-02-04 21 ...
【CV】Pytorch一小时入门教程-代码详解
目录一.关键部分代码分解 1.定义网络 2.损失函数(代价函数) 3.更新权值二.训练完整的分类器 1.数据处理 2. 训练模型(代码详解) CPU训练 GPU训练 CPU版本与GPU版本代码区别 ...
html5代码转换为视频,HTML5中的视频代码详解
摘要腾兴网为您分享:HTML5中的视频代码详解,智学网,云闪付,易推广,小红书等软件知识,以及360win10,流量魔盒,fitbit,上港商城,安卓2.3.7,全民惠,五年级下册英语单词表图片,t ...
js php base64,JavaScript实现Base64编码与解码的代码详解
本篇文章给大家分享的是jJavaScript实现Base64编码与解码的代码详解,内容挺不错的,希望可以帮助到有需要的朋友一.加密解密方法使用//1.加密 var str = '124中文内容'; ...

匿名mahony互补滤波代码详解

匿名mahony互补滤波代码详解相关推荐

最新文章

热门文章