3轴IMU重力补偿

Question

我有来自 3 轴 IMU 的加速度 (x,y,z) 和滚动、俯仰、航向数据。我想根据陀螺仪测量的角度（r，p，h）来补偿加速度数据。例如，当 IMU 平坦且静止（z 轴向上）时，加速度读数为 [ax,ay,az]=[0,0,-1]。当 IMU 平坦且静止（x 轴向上）时，加速度读数为 [ax,ay,az]=[-1,0,0]。最后，当 IMU 平坦且静止（y 轴向上）时，加速度读数为 [ax,ay,az]=[0,-1,0]。

由于在 IMU不是完全平坦和水平的情况下收集数据，我需要从我的加速度数据中删除 g 分量。

下面是我第一次删除 g 组件。我可以使用的另一种方法是什么，因为我已经知道随着数据文件大小的增加，这是计算密集型的。

%% Remove gravity from X, Y, Z components 
refPlane = [r0 p0 h0]; % [2deg 4deg 60deg] IMU was not level during data collection. 

for i = 1:length(time)
    deltaAngleX = roll(i) - refPlane(1); %degrees
    deltaAngleY = pitch(i) - refPlane(2); %degrees

    if ( deltaAngleX > 0) % roll ++, ay --, az ++
        cX_X = 0;
        cY_X = -sind ( deltaAngleX ) ;
        cZ_X = cosd ( deltaAngleX ) ;
    elseif ( deltaAngleX < 0 )
        cX_X = 0;
        cY_X = sind ( deltaAngleX ) ;
        cZ_X = cosd ( deltaAngleX ) ;
    end

    if ( deltaAngleY > 0 ) % roll ++, ay --, az ++
        cX_Y = sind ( deltaAngleY ) ;
        cY_Y = 0 ;
        cZ_Y = sind ( deltaAngleY ) ;
    elseif ( deltaAngleY < 0 )
        cX_Y = -sind ( deltaAngleY ) ;
        cY_Y = 0 ;
        cZ_Y = sind ( deltaAngleY ) ;
    end

    ax(i) = ax(i) + cX_X + cX_Y;
    ay(i) = ay(i) + cY_X + cY_Y;
    az(i) = az(i) + cZ_X + cZ_Y;

end

Answer 1

您可以使用旋转矩阵 (http://en.wikipedia.org/wiki/Rotation_matrix)。目前，您的大部分代码都在写出矩阵乘法，一次一个组件。 Matlab 的矩阵乘法速度很快，因此它可能会加快速度。

更进一步，您可以矢量化您的代码。这是一个很大的话题，所以我将给出一个广泛的想法。关键是，Matlab 特别适合处理大型矩阵上的运算，并且运行此类运算的速度比运行for 循环要快得多。

例如，在 Matlab 中，您可以将sin 函数应用于矩阵M 的每个元素，只需编写sin(M)。使用repmat 之类的操作，尝试构建一个大矩阵，尺寸为 2 x 2，长度（时间），对所有旋转操作进行编码。类似地，建立一个大向量，按长度（时间）为 2 维，对所有加速度向量进行编码。然后在适当的维度上进行单个矩阵乘法和sum 运算将立即为您提供所有数据。