1.Mahony姿态解算算法笔记（一）
2.ESP32学习笔记----MPU6050使用
3.PX4源码理解--src目录介绍
4.手机陀螺仪怎么打开
5.ORB-SLAM3 源码剖析：IMU 预积分
6.Gyroflow-RustIMU积分算法源码解析
Mahony姿态解算算法笔记（一）
本文使用 Zhihu On VSCode 创作并发布
mahony 算法是陀螺陀螺常见的姿态融合算法，将加速度计，国际磁力计，源码源码陀螺仪共九轴数据，世界融合解算出机体四元数，开源该算法可到其网站下载源码 x-io.co.uk/open-source-...
该篇仅介绍融合加速度计和陀螺仪的陀螺陀螺公益类 网站源码六轴数据算法，由于笔者水平有限，国际文中难免存在一些不足和错误之处，源码源码诚请各位批评指正。世界
首先，开源姿态解算中的陀螺陀螺姿态实际上值得是机体坐标系与地理坐标系的旋转关系。其常用描述形式有三种：欧拉角，国际方向余弦矩阵，源码源码四元数。世界
四元数的开源定义与复数非常类似，唯一的区别是复数只有一个虚部，而四元数一共有三个。所有的四元数 q ∈ H （H代表四元数的发现者William Rowan Hamilton）都可以写成下面这种形式：
其中：
与复数类似，四元数就是基{ 1，i，j，k}的线性组合，同样的，四元数也可以写成向量形式：
另外的，我们也可以将实部与虚部分开，即通过一个三维向量表示虚部，从而将四元数表示为标量与向量的有序对形式：
四元数的加减运算与标量乘法同复数类似，只需将分量各自运算即可。与复数相同，四元数的标量乘法同样遵循交换律，即[公式]，eom指标源码其中s为标量。
四元数乘法较为特殊，并不遵循交换律，也就是说在一般情况下[公式]。如果有两个四元数[公式] 和 [公式] ，由 [公式] 可得其乘积为：
写成矩阵形式则有：
这个矩阵变换相当于左乘[公式]，由于四元数不符合交换律，所以右乘[公式]的变换是一个不同的矩阵：
与复数相似，实部为0的四元数被称之为纯四元数，空间中的三维向量可以用纯四元数的形式表示，这种表示方法在四元数表示旋转的推导过程中有重要应用。
定义模长为1的四元数为单位四元数，用于表示旋转的四元数一定为单位四元数。
该部分省略推导过程，具体可以参考 krasjet.github.io/quate...
利用四元数表示旋转的定理，任意向量v 绕着以单位向量定义的旋转轴 u 旋转 θ 度后的 v' 可以使用四元数乘法来获得四元数旋转公式:
其中：
通过罗德里格旋转，我们可以推导出四元数表示旋转的一般形式，即三个四元数相乘，通过四元数乘法的矩阵表示形式，我们可以将四元数旋转公式表示为矩阵形式：
任意向量v 沿着以单位向量定义的旋转轴 u 旋转 θ 角度后的 v' 可以使用矩阵乘法来获得：
同样也是将向量从机体坐标系b 到大地坐标系 R 的姿态矩阵 [公式]（也称为坐标转换矩阵）即为：
这里再给出从大地坐标系R 转换到机体坐标系 b 的坐标转换矩阵 [公式]
其中：
对应四元数为：
也可写成有序数对形式，即2.1旋转公式中的：
得到四元数后，可以通过四元数的值反解出机体坐标系的欧拉角，同样的这里省略推导过程直接给出公式：
求解该微分方程即可得到当前四元数的值。但计算机中的计算是离散的，所以我们需要对该微分方程进行离散化处理，这样才可以有效的通过单片机或其他数字控制器进行求解：
首先，对于六轴数据，计算角度有两种方法，一种是通过对角速度积分得到角度，另一种则是zxing源码打包通过对加速度进行正交分解得到角度。但这两种方式均存在不足，通过角速度积分得到角度时，角速度的误差会在积分过程中被不断放大从而影响数据准确性。而加速度计是一种特别敏感的传感器，电机旋转产生的震动会给加速度计的数据中带来高频噪声。
通过融合两种数据以获得准确姿态，这里通过加速度计补偿角速度。设有大地坐标下的重力加速度g，把 g 通过姿态矩阵（坐标转换矩阵）的逆（意味着从地理坐标系 R 到机体坐标 b 系）变换到机体坐标系，得到其在机体坐标系下的**理论重力加速度向量 [公式] **，则两者的变换关系可通过前文给出的姿态矩阵得出：
不难看出，将重力加速度向量变换至机体坐标系后，恰好是矩阵的最后一列。这样一来，我们就得到了由描述刚体姿态的四元数推导出的理论重力加速度向量 [公式] 。另外，我们还可以通过加速度计测量出实际重力加速度向量 [公式] 。
这里的理论重力加速度向量[公式] 和实际重力加速度向量 [公式] 之间必然存在偏差，而这个偏差很大程度上是由陀螺仪数据产生的角速度误差引起的，所以根据理论向量和实际向量间的偏差，就可以补偿陀螺仪数据的误差，进而解算出较为准确的姿态，即将隐含在四元数中的角速度误差显化。
理论重力加速度向量和实际重力加速度向量均是向量，反应向量间夹角关系的运算有两种：内积（点乘）和外积（叉乘），考虑到向量外积模的大小与向量夹角呈正相关，故通过计算外积来得到向量偏差[公式]：
在进行叉乘运算前，应先将理论向量[公式] 和实际向量 [公式] 做单位化处理，有：
考虑到实际情况中理论向量[公式] 和实际向量 [公式] 偏差角不会超过°，而当[公式]在±°内时，google chrome 源码[公式] 与[公式]的值非常接近，因此上式可进一步简化为：
得到向量偏差后，即可通过构建PI补偿器来计算角速度补偿值：
其中，比例项用于控制传感器的“可信度”，积分项用于消除静态误差。[公式]越大，意味着通过加速度计得到误差后补偿越显著，即是越信任加速度计。反之[公式]越小时，加速度计对陀螺仪的补偿作用越弱，也就越信任陀螺仪。而积分项则用于消除角速度测量值中的有偏噪声，故对于经过零篇矫正的角速度测量值，一般选取很小的[公式]。最后将补偿值补偿给角速度测量值，带入四元数差分方程中即可更新当前四元数。
考虑到四元数不具备直观几何意义，故最后还需通过四元数反解出欧拉角。这里直接套用上文给出的公式即可：
回到欧拉角的计算，最终通过四元数反解出欧拉角，完成姿态解算过程。
本文仅介绍了基于四元数的mahony姿态解算算法的主要原理与步骤，包括四元数的定义与性质、四元数表示姿态与旋转、传感器数据融合以及姿态解算的具体方法。通过融合加速度计和陀螺仪的数据，利用四元数表示机体坐标系与大地坐标系之间的旋转关系，实现姿态的准确估计。在传感器数据融合部分，通过理论重力加速度向量与实际重力加速度向量的健康之家源码偏差来补偿陀螺仪的角速度误差，提高姿态估计的准确性。最后，通过四元数反解欧拉角，完成姿态解算的全过程。在实际应用中，还需要根据具体情况进行算法的优化与调整，以满足不同场景的需求。
ESP学习笔记----MPU使用
ESP-S3 SoC芯片支持多种功能，包含在官方资料ESP-IDF编程指南与I2C驱动程序中。使用IDF 5.1.1开发环境，结合ESP-S3-LCD-EV-Board-MB开发板与mpu模块进行开发。MPU-是一款集成6轴运动追踪设备，融合了3轴陀螺仪和3轴加速度计，具备全面运动追踪数据输出，适用于手势识别、游戏控制器、可穿戴设备等场景。该设备可通过I2C总线进行通信，提供倾斜角度、旋转速率、温度等信息，适用于便携式设备的运动检测和追踪。
MPU-传感器的引脚定义中，使用IO与IO两个引脚与mpu模块的SCL与SDA引脚连接，ADO引脚接地时地址为0x，接VCC时地址为0x。通过乐鑫组件管理器搜索mpu组件，并在工程目录下使用特定命令添加组件。修改main.c文件以获取mpu传感器数据。
开发过程涉及初始化I2C接口与mpu传感器（i2c_sensor_mpu_init函数），获取加速度计测量值（mpu_get_acce函数）、陀螺仪值（mpu_get_gyro函数）与传感器温度（mpu_get_temp函数）。mpu_complimentory_filter函数通过整合加速度值与陀螺仪值计算出pitch与roll的角度。
项目的GitHub仓库与CSDN教程提供详细指导与源代码资源，以便深入学习与实践。
PX4源码理解--src目录介绍
PX4的中心代码及二次开发关键代码主要位于src目录中，包含以下分类目录：
drivers目录下包含各种硬件设备驱动，如传感器（加速度计、陀螺仪）、电机驱动、GPS等，负责硬件通信与数据交换。
example目录提供示例代码，帮助开发者理解PX4固件中功能和技术的使用。
include目录内有PX4固件使用的头文件，定义数据结构、函数原型、宏定义等，支持核心功能与模块。
lib目录汇集通用库与工具，支持PX4功能，包含数学库、通信协议库、数据结构库等。
module目录是PX4固件核心模块所在，包含多个子目录，每个对应特定模块，如飞行控制、姿态控制、位置控制、传感器驱动、任务管理等。
systemcmds目录存放系统命令源代码，用于配置、管理及测试PX4飞行控制系统。
templates/template_module目录内是模板模块源代码，展示如何创建自定义模块并集成到PX4飞行控制系统中。
手机陀螺仪怎么打开
以安卓手机为例，打开陀螺仪的方法是：
1、进入手机系统设置界面，在下方选项中点击“辅助功能”。
2、点击之后弹框提示确认信息，将手机水平放置之后点击“校准”。
3、进入校准之后等待系统完成操作，即可开启陀螺仪。
Android是由Google公司和开放手机联盟领导并开发的一种基于Linux的自由且开放源代码的操作系统，主要使用于移动设备。其最初由AndyRubin开发，后被Google于年8月收购。之后Google与家硬件制造商、软件开发商及电信营运商组建开放手机联盟，共同研发改良Android系统，完成开发后，Google以Apache开源许可证的授权方式，发布了Android的源代码。Android一词的本义指“机器人”，同时也是Google于年月5日宣布的基于Linux平台的开源手机操作系统的名称，该平台由操作系统、中间件、用户界面和应用软件组成。
ORB-SLAM3 源码剖析：IMU 预积分
IMU的数据结构在ORB-SLAM3中用于表示机体坐标系中的测量值。在特定时刻，加速度计测量线加速度和陀螺仪测量角速度。假设这些测量值包含高斯白噪声，且偏置建模为随机游走，其导数也是高斯白噪声。将重力转换到机体坐标系后，得到连续视觉帧间的IMU预积分结果。这些预积分包括旋转、速度和位置测量，以及整个测量向量的协方差矩阵。
在ORB-SLAM3中，每帧的IMU预积分在tracking线程中计算，具体由Tracking::PreintegrateIMU()函数执行。每帧间的IMU测量通过src/ImuTypes.cc中的Preintegrated::IntegrateNewMeasurement()进行积分。主要步骤如下：首先进行偏置校正，然后计算位置、速度的增量，接着计算旋转的增量。旋转变化量以李代数中的旋转向量表示，并通过指数映射转换为旋转矩阵。旋转矩阵按旋转顺序右乘。最后，更新协方差矩阵，并调整与偏置修正相关的位置、速度和旋转雅可比。
IMU的偏置校正、测量、标定和预积分类定义在include/ImuTypes.h文件中。
值得注意的是，对于初学者，了解GDB调试方法是提高ORB-SLAM3源码理解效率的重要步骤。GDB提供了一系列功能，允许开发者在运行程序时设置断点、查看变量值、追踪程序执行流程等，从而深入分析代码行为和潜在问题。
Gyroflow-RustIMU积分算法源码解析
在深入解析Gyroflow-Rust库中的IMU积分算法之前，我们首先需要明确，积分算法在将原始的陀螺仪角速度和加速度计读数转换为实际IMU的方向四元数，对于视频稳像至关重要。Gyroflow v1.4.2提供了多种可选积分算法，包括Madgwick、Mahony以及互补滤波器，其中互补滤波器以最小的水平漂移提供较好的估计结果，且是默认集成方法。 ### 源码解析 为了全面理解IMU积分算法在Gyroflow-Rust中的实现，我们将逐步解析其核心步骤。首先，算法通过UI界面与数据交互，根据选择的积分方法进行操作。 #### UI界面数据交互 算法通过用户界面接受指令，调用指定的积分方法。 #### 互补滤波器思维导图 互补滤波器结合了陀螺仪和加速度计的数据，利用加速度计锁定地平线，以最小的水平漂移提供IMU方向的估计。 #### 默认构造函数default() 此函数设置初始条件，并根据系统状态初始化方向四元数。 #### 加速度初始化方向四元数 在系统稳定后，利用加速度数据初始化方向四元数。 #### 检查稳定状态 算法监控系统状态，当稳定时长超过设定阈值时，更新陀螺仪零偏。 #### 角速度预测 在预设的时间间隔内，预测角速度以更新方向四元数。 #### 修正四元数 通过加速度计算修正四元数，SLERP插值用于优化四元数。 #### 修正与归一化 通过四元数乘法，修正估计的方向四元数并进行归一化。 #### 新增内容 相较于ROS中的互补滤波器实现，Gyroflow-Rust在加速度数据处理、重力加速度自适应计算以及自适应增益计算方面进行了优化调整。 ### 注意事项与改进 在计算角速度向量模长时，原始ROS实现中存在小笔误。通过在GitHub上提出问题，作者已进行修正。 ### 参考资料 在深入研究Gyroflow-Rust库的IMU积分算法时，参考以下资源将大有裨益：Gyroflow-RustAuto Sync自动同步模块算法解析
Gyroflow-RustLens Calibrator相机标定工具使用、自定义修改以及算法解析
论文阅读互补滤波器详细推导_源码解析_数据集实测_Keeping a Good Attitude: A Quaternion Based Orientation Filter for IMUs
