matlab - 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