0

I have acceleration (x,y,z) and roll, pitch, heading data from a 3-axis IMU. I want to compensate the acceleration data based on the angles (r,p,h) measured by the gyro. For example, when the IMU is flat and stationary (z-axis up), the accelerations read [ax,ay,az]=[0,0,-1]. When the IMU is flat and stationary (x-axis up), the accelerations read [ax,ay,az]=[-1,0,0]. And finally when the IMU is flat and stationary (y-axis up), the accelerations read [ax,ay,az]=[0,-1,0].

Since data was collected with the IMU not being perfectly flat and level, I need to remove g-components from my acceleration data.

Below is my first pass at removing the g-components. What is another approach I can use because I can already tell that this is computationally intensive as the size of my data file increase. 

%% 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
4

1 回答 1

1

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

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

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

于 2015-02-26T04:09:44.823 回答