问题描述
我基于以下论文在 UE4 上用 c++ 编写了代码并将其安装在 android 智能手机上；
一种用于惯性和惯性磁传感器阵列的高效定向滤波器（https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf）本文末尾有代码。我是这样写的;
m_pMadgwickAHRS->MadgwickAHRS::MadgwickAHRSupdateIMU(m_vMygyroscope.X,m_vMygyroscope.Y,m_vMygyroscope.Z,m_vMyacceleration.X,m_vMyacceleration.Y,m_vMyacceleration.Z);//Get data from device gyroscope and accelerator and Feed it in the function
#define deltat 0.01f // sampling period in seconds
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f) // gyroscope measurement error in rad/s (shown as 5 deg/s)
#define beta1 sqrt(3.0f / 4.0f) * gyroMeasError // compute beta1
//Initialized quaternion q0~q3 values in function above it as q0=1,q1=q2=q3=0
void MadgwickAHRS::MadgwickAHRSupdateIMU(float gx,float gy,float gz,float ax,float ay,float az){
float recipnorm; //vector norms
float qDot1,qDot2,qDot3,qDot4; //quaternion derrivative from gyroscopes elements
float f_1,f_2,f_3; //objective function elements
float J_11or24,J_12or23,J_13or22,J_14or21,J_32,J_33; //objective function Jacobian elements
float s0,s1,s2,s3; //estimated direction of the gyroscope error
// Axulirary variables to avoid reapeated calcualtions
float halfSEq_1 = 0.5f * q0;
float halfSEq_2 = 0.5f * q1;
float halfSEq_3 = 0.5f * q2;
float halfSEq_4 = 0.5f * q3;
float twoSEq_1 = 2.0f * q0;
float twoSEq_2 = 2.0f * q1;
float twoSEq_3 = 2.0f * q2;
// normalise accelerometer measurement
recipnorm = sqrt(ax * ax + ay * ay + az * az);
ax /= recipnorm;
ay /= recipnorm;
az /= recipnorm;
f_1 = twoSEq_2 * q3 - twoSEq_1 * q2 - ax;
f_2 = twoSEq_1 * q1 + twoSEq_3 * q3 - ay;
f_3 = 1.0f - twoSEq_2 * q1 - twoSEq_3 * q2 - az;
J_11or24 = twoSEq_3; //J_11 negated in matrix multiplication
J_12or23 = 2.0f * q3;
J_13or22 = twoSEq_1; //J_12 negated in matrix multiplication
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21; //negated in matrix multiplication
J_33 = 2.0f * J_11or24; //negated in matrix multiplication
// Compute the gradient (matrix multiplication)
s0 = J_14or21 * f_2 - J_11or24 * f_1;
s1 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
s2 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
s3 = J_14or21 * f_1 + J_11or24 * f_2;
// normalise the gradient
recipnorm = sqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
s0 /= recipnorm;
s1 /= recipnorm;
s2 /= recipnorm;
s3 /= recipnorm;
// Compute the quaternion derrivative measured by gyroscopes
qDot1 = -halfSEq_2 * gx - halfSEq_3 * gy - halfSEq_4 * gz;
qDot2 = halfSEq_1 * gx + halfSEq_3 * gz - halfSEq_4 * gy;
qDot3 = halfSEq_1 * gy - halfSEq_2 * gz + halfSEq_4 * gx;
qDot4 = halfSEq_1 * gz + halfSEq_2 * gy - halfSEq_3 * gx;
// Compute then integrate the estimated quaternion derrivative
q0 += (qDot1 - (beta1 * qDot1)) * deltat;
q1 += (qDot2 - (beta1 * qDot2)) * deltat;
q2 += (qDot3 - (beta1 * qDot3)) * deltat;
q3 += (qDot4 - (beta1 * qDot4)) * deltat;
//put it in the quaternion values
vQuat.W = q0;
vQuat.X = q1;
vQuat.Y = q2;
vQuat.Z = q3;
}
在此之后，我得到四元数并在其他 .cpp 上调用它并使用 .Euler();(UE4) 将其更改为 Euler
我的问题是当我移动我的智能手机时，它在漂移，它说当我将我的设备移动 90 度时它只移动了 45 度，并且在我摇晃我的设备后，我的度数都是随机方向的。在 Y 轴上它只能在 -90-90 度之间移动，所以如果我旋转 90 度而不是 -90 度，有时它会卡在那里。
有什么想法可以解决这些问题吗？
谢谢。
解决方法
暂无找到可以解决该程序问题的有效方法，小编努力寻找整理中！
如果你已经找到好的解决方法，欢迎将解决方案带上本链接一起发送给小编。
小编邮箱:dio#foxmail.com (将#修改为@）