使用旋转矢量四元数从加速度计中去除重力

Question

如果这听起来是一个重复的问题，请忍受。但是我只想验证我使用的整个方法是否正确。基本上，我是根据从不同来源阅读的内容构建的。

我有一个IMU，从中可以得到加速度计和旋转矢量值。文档说了以下有关旋转矢量的信息。

  

旋转矢量提供方向输出，该方向输出表示为参考磁北的四元数   和重力。它是通过将加速度计，陀螺仪和磁力计的输出融合而产生的。旋转   向量是可用的最准确的方向估计。

旋转矢量四元数表明设备从地球坐标系旋转了多少。我想通过旋转矢量和加速度计的组合来获得设备坐标系上的线性加速度。而且我假设只要设备处于静止状态，线性加速度就应该为0。

这是我想出的数学。

• 从旋转矢量q中获取四元数
• 获取四元数的倒数，以便将重力表示在设备的坐标系上。 qPrime = inverse(q)
取大约一定数量的加速度信号的范数的中位数。 norm
• 将引力矢量创建为g = [0 0 norm]
• 将线性加速度设为a_linear = a_total - qPrime*g*conjugate(qPrime)

这大概是我的matlab / octave代码中的重要内容。

    %get the gravity norm at rest from 100 samples
    gNorm = median(sqrt(sum(Acc(1:100, 1:3).^2, 2)), 1);
    %a couple of lines and a loop later
    q_inv = qInv(q);
    a_p = qTimes(qTimes(q_inv, [0 g]), qConj(q_inv));
    LinAcc = [Acc(i, 1:3) - a_p(2:4)];

使用上面的代码后，我得到了一个如下图所示的图。

我的任何假设是否错误？我是否必须以四元数的倒数旋转到设备的坐标系，这是我的错吗？我是否应该认为诸如y的0.05m / s / s之类的误差来自诸如校准之类的其他来源，而不是来自数学？

Answer 1

等式正确。尖刺的声音有点怪异。您是否在取消重力乘法之前检查过该值？如果原始acc值具有与您的数学计算所不同的噪声。也可能是您的旋转矢量噪声。