微信公众号搜"智元新知"关注
微信扫一扫可直接关注哦!

为 IMU 加速证明创建算法

如何解决为 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 举报,一经查实,本站将立刻删除。