【007】IMU数据融合之Mahony算法应用

最新推荐文章于 2024-07-26 08:12:10 发布

lucky_dog_2018

最新推荐文章于 2024-07-26 08:12:10 发布

阅读量1w

点赞数 12

分类专栏：嵌入式开发文章标签：数据融合姿态测量 IMU Mahony MPU

本文链接：https://blog.csdn.net/lucky_dog_2018/article/details/80173690

版权

本文介绍了IMU数据融合中的Mahony算法，用于减小噪声获取准确姿态信息。讨论了算法源码、使用注意事项，包括加速度计和陀螺仪输出符号的调整、单位转换以及Kp和Ki参数的调节。通过刚体旋转动力学方程，阐述了算法的工作流程，为后续的深入分析和项目应用奠定了基础。

摘要由CSDN通过智能技术生成

IMU数据融合之Mahony算法应用

关键词：Mahony，IMU，九轴，数据融合，滤波

经过前面几篇博客的努力，目前我们已经能够使用上位机获取并显示MPU9250的实时测量值，基于此本篇博客将介绍本着实用的原则介绍Mahony算法，对IMU的测量数据进行融合以减小噪声获得姿态信息。

Mahony算法源码

#define sampleFreq	512.0f			// sample frequency in Hz
#define twoKpDef	(2.0f * 0.5f)	// 2 * proportional gain
#define twoKiDef	(2.0f * 0.0f)	// 2 * integral gain

//---------------------------------------------------------------------------------------------------
// Variable definitions

volatile float twoKp = twoKpDef;											// 2 * proportional gain (Kp)
volatile float twoKi = twoKiDef;											// 2 * integral gain (Ki)
volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f;					// quaternion of sensor frame relative to auxiliary frame
volatile float integralFBx = 0.0f,  integralFBy = 0.0f, integralFBz = 0.0f;	// integral error terms scaled by Ki

//---------------------------------------------------------------------------------------------------
// Function declarations

float invSqrt(float x);
void MahonyAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
	float recipNorm;
	float halfvx, halfvy, halfvz;
	float halfex, halfey, halfez;
	float qa, qb, qc;

	// Compute