如何解决为 IMU 加速证明创建算法
我尝试设计一种适用于自行车的 IMU 算法。我使用该算法来估计 IMU 的欧拉角。 我只使用加速度计和陀螺仪来估计我的姿势。不幸的是,当我烘烤时,由于减速,我得到了错误的姿势估计。
这是我用来估计 IMU 的算法的一部分:
float recipnorm;
float halfvx,halfvy,halfvz;
float halfex,halfey,halfez;
float qa,qb,qc;
// Compute Feedback only if accelerometer measurement valid (avoids NaN in
// accelerometer normalisation)
if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// normalise accelerometer measurement
recipnorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipnorm;
ay *= recipnorm;
az *= recipnorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1 * q3 - q0 * q2;
halfvy = q0 * q1 + q2 * q3;
halfvz = q0 * q0 - 0.5f + q3 * q3;
// Error is sum of cross product between estimated and measured direction of
// gravity
halfex = (ay * halfvz - az * halfvy);
halfey = (az * halfvx - ax * halfvz);
halfez = (ax * halfvy - ay * halfvx);
// Compute and apply integral Feedback if enabled
if (twoKi > 0.0f) {
integralFBx +=
twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral Feedback
gy += integralFBy;
gz += integralFBz;
} else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional Feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);
// normalise quaternion
recipnorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipnorm;
q1 *= recipnorm;
q2 *= recipnorm;
q3 *= recipnorm;
使用该参数,我尝试修改:
#define sampleFreq (50f/2.f) // sample frequency in Hz
#define twoKpDef (2.0f * 0.1f) // 2 * proportional gain
#define twoKiDef (2.0f * 1.f) // 2 * integral gain
我无法从姿势估计中提取减速。每次刹车时,都会出现错误的角度估计。
有人知道更有效的算法吗?或者设置参数的方法。
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。