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

2024-01-17 06:39| 来源: 网络整理| 查看: 265

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; flo

【本文地址】

公司简介

联系我们