amoBBS 阿莫电子论坛

 找回密码
 注册
搜索
bottom↓
查看: 454|回复: 9

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

[复制链接]
发表于 2017-9-6 23:07:18 | 显示全部楼层 |阅读模式
代码如下所示


//=====================================================================================================
// 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
//====================================================================================================
发表于 2017-9-6 23:08:38 来自手机 | 显示全部楼层
就是这个名字的算法
 楼主| 发表于 2017-9-6 23:14:09 | 显示全部楼层
ttoto 发表于 2017-9-6 23:08
就是这个名字的算法

应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪?应该有相应的论文吧,
发表于 2017-9-7 10:22:32 | 显示全部楼层
书9500 发表于 2017-9-6 23:14
应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪?应该有相应的论文吧, ...

爱国谷歌一下 第一条就是了
 楼主| 发表于 2017-9-7 19:42:50 | 显示全部楼层
ttoto 发表于 2017-9-7 10:22
爱国谷歌一下 第一条就是了

找到的是关于梯度下降的论文
发表于 2017-9-7 19:45:47 | 显示全部楼层
书9500 发表于 2017-9-7 19:42
找到的是关于梯度下降的论文

就是这篇,具体我也没有看过。
 楼主| 发表于 2017-9-7 22:09:49 | 显示全部楼层
ttoto 发表于 2017-9-7 19:45
就是这篇,具体我也没有看过。

我贴的算法是互补滤波的,和他还不一样。
发表于 2017-9-8 12:50:47 | 显示全部楼层
是这个吗?
// 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

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
 楼主| 发表于 2017-9-8 20:35:50 | 显示全部楼层
lsdavid 发表于 2017-9-8 12:50
是这个吗?
// Quaternion implementation of the 'DCM filter' [Mayhony et al].

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

http://blog.csdn.net/nemol1990/a ... utm_medium=referral
 楼主| 发表于 2017-9-8 23:13:55 | 显示全部楼层
书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/


亲们,找到一个网站,还没细看,比较仔细可以解决疑惑
友情提示:标题不合格、重复发帖,将会被封锁ID。详情请参考:论坛通告:封锁ID、获得注册邀请码、恢复被封ID、投诉必读
您需要登录后才可以回帖 登录 | 注册

本版积分规则

手机版|Archiver|阿莫电子论坛(原ourAVR/ourDEV) ( 粤ICP备09047143号 )

GMT+8, 2017-9-24 13:06

阿莫电子论坛, 原"中国电子开发网"

© 2004-2016 www.amobbs.com, 原www.ourdev.cn, 原www.ouravr.com

快速回复 返回顶部 返回列表