网络上流传的姿态解算的“IMUupdate”MU算法出处是什么？

书9500

代码如下所示


//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' [Mayhony et al].
//
// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.
//
// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated
// orientation.  See my report for an overview of the use of quaternions in this application.
//
// 用户必须调用 'IMUupdate()' 校准每个采样周期和解析 gyroscope ('gx', 'gy', 'gz')
// and 加速度计('ax', 'ay', 'ay') data.  陀螺仪的单位是弧度/秒，加速度
//单位是不相关的，是归为载体。
//
//=====================================================================================================

//----------------------------------------------------------------------------------------------------
// Header files

#include "IMU.h"
#include <math.h>

//----------------------------------------------------------------------------------------------------
// Definitions

#define Kp 2.0f                        // 比例增益支配率收敛到加速度计/磁强计
#define Ki 0.005f                // 积分增益支配率的陀螺仪偏见的衔接
#define halfT 0.5f                // 采样周期的一半

//---------------------------------------------------------------------------------------------------
// 变量定义

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;        // 四元数的元素，代表估计方向
float exInt = 0, eyInt = 0, ezInt = 0;        // 按比例缩小积分误差

//====================================================================================================
// Function
//====================================================================================================

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) {
        float norm;
        float vx, vy, vz;
        float ex, ey, ez;         
       
        // 测量正常化
        norm = sqrt(ax*ax + ay*ay + az*az);      
        ax = ax / norm;
        ay = ay / norm;
        az = az / norm;      
       
        // 估计方向的重力
        vx = 2*(q1*q3 - q0*q2);
        vy = 2*(q0*q1 + q2*q3);
        vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
       
        // 错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
        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;
       
        // 调整后的陀螺仪测量
        gx = gx + Kp*ex + exInt;
        gy = gy + Kp*ey + eyInt;
        gz = gz + Kp*ez + ezInt;
       
        // 整合四元数率和正常化
        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;  
       
        // 正常化四元
        norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
        q0 = q0 / norm;
        q1 = q1 / norm;
        q2 = q2 / norm;
        q3 = q3 / norm;
}

//====================================================================================================
// END OF CODE
//====================================================================================================

ttoto

就是这个名字的算法

书9500

ttoto 发表于 2017-9-6 23:08
就是这个名字的算法

应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪？应该有相应的论文吧，

ttoto

书9500 发表于 2017-9-6 23:14
应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪？应该有相应的论文吧， ...

爱国谷歌一下 第一条就是了

书9500

ttoto 发表于 2017-9-7 10:22
爱国谷歌一下 第一条就是了

找到的是关于梯度下降的论文

ttoto

书9500 发表于 2017-9-7 19:42
找到的是关于梯度下降的论文

就是这篇，具体我也没有看过。

书9500

ttoto 发表于 2017-9-7 19:45
就是这篇，具体我也没有看过。

我贴的算法是互补滤波的，和他还不一样。

lsdavid

是这个吗？
// Quaternion implementation of the 'DCM filter' [Mayhony et al].

附件文件，page12， “Mahony et al [35] showed that gyroscope bias drift may also be compensated for by simpler orientation filters through the integral feedback of the error in the rate of change of orientation. A similar approach will be used here.”

R. Mahony, T. Hamel, and J.-M. Pflimlin. Nonlinear complementary filters on the special orthogonal group. Automatic Control, IEEE Transactions on, 53(5):1203 –1218, june 2008

书9500

lsdavid 发表于 2017-9-8 12:50
是这个吗？
// Quaternion implementation of the 'DCM filter' [Mayhony et al].

这个应该是梯度下降法的论文。

http://blog.csdn.net/nemol1990/a ... utm_medium=referral

书9500

书9500 发表于 2017-9-8 20:35
这个应该是梯度下降法的论文。

http://blog.csdn.net/nemol1990/article/details/23102643?utm_source=t ...

http://www.olliw.eu/2013/imu-data-fusing/


亲们，找到一个网站，还没细看，比较仔细可以解决疑惑