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×2长度（时间）的矩阵，它对所有旋转操作进行编码。类似地，建立一个大的矢量，尺寸2的长度（时间），编码所有的加速度矢量。然后，在适当的维度上进行单个矩阵乘法和sum操作，即可立即为您提供所有数据。

3轴IMU重力补偿

1 个答案: