网络上流传的姿态解算的“IMUupdate”MU算法出处是什么？
代码如下所示//=====================================================================================================
// IMU.c
// S.O.H. Madgwick
// 25th September 2010
//=====================================================================================================
// Description:
//
// Quaternion implementation of the 'DCM filter' .
//
// 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 发表于 2017-9-6 23:08
就是这个名字的算法
应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪？应该有相应的论文吧， 书9500 发表于 2017-9-6 23:14
应该是这个人写的“S.O.H. Madgwick”
我想知道他最初发表在哪？应该有相应的论文吧， ...
爱国谷歌一下 第一条就是了 ttoto 发表于 2017-9-7 10:22
爱国谷歌一下 第一条就是了
找到的是关于梯度下降的论文 书9500 发表于 2017-9-7 19:42
找到的是关于梯度下降的论文
就是这篇，具体我也没有看过。 ttoto 发表于 2017-9-7 19:45
就是这篇，具体我也没有看过。
我贴的算法是互补滤波的，和他还不一样。 是这个吗？
// Quaternion implementation of the 'DCM filter' .
附件文件，page12， “Mahony et al 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 lsdavid 发表于 2017-9-8 12:50
是这个吗？
// Quaternion implementation of the 'DCM filter' .
这个应该是梯度下降法的论文。
http://blog.csdn.net/nemol1990/article/details/23102643?utm_source=tuicool&utm_medium=referral 书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/
亲们，找到一个网站，还没细看，比较仔细可以解决疑惑
页:
[1]