三轴IMU重力补偿

Question

我有加速度(x，y，z)和三轴IMU的滚动，俯仰，航向数据.我想根据陀螺仪测量的角度(r，p，h)来补偿加速度数据。例如，当IMU是平的和静止的(z轴向上)时，加速度读取ax，ay，az=0,0，-1。当IMU是平的和静止的(x轴向上)时，加速度为ax，ay，az=-1,0,0。最后，当IMU是平的和静止的(y轴向上)时，加速度是ax，耶，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

您可以使用旋转矩阵(matrix)。目前，您的大部分代码都在编写矩阵乘法，一次只写一个部件。Matlab在矩阵乘法方面速度很快，所以它可能会加速一些事情。

为了更进一步，您可以将代码向量化。这是一个很大的话题，所以我只想给出一个大致的想法。关键是，Matlab特别适合于处理大型矩阵上的操作，并将运行比运行for循环快得多的操作。

例如，在Matlab中，只需编写sin，就可以将M函数应用于矩阵M的每个元素。使用像repmat这样的操作，尝试构建一个大的矩阵，按长度(时间)为2乘2，编码所有的旋转操作。同样，构建一个大的向量，维2的长度(时间)，编码所有的加速向量。然后，在适当的维度上，一个矩阵乘法和sum操作将同时给出所有的数据。