3-axis IMU gravity compensation

Question

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

Answer 1

You could use the rotation matrix (http://en.wikipedia.org/wiki/Rotation_matrix). Currently most of your code is writing out the matrix multiplication, one component at a time. Matlab is fast at multiplying matrices, so it might speed things up a bit.

To go further, you can vectorize your code. This is a big topic so I will just give the broad idea. The point is that Matlab is especially well adapted to handle operations on large matrices and will run such operations much faster than it will run for loops.

For instance, in Matlab, you can apply the sin function to every element of a matrix M just by writing sin(M). Using operations like repmat, try to build up a large matrix, of dimension 2 by 2 by length(time), that encodes all your rotation operations. Similarly, build up a large vector, dimension 2 by length(time), that encodes all your acceleration vectors. Then a single matrix multiplication and sum operation across the appropriate dimension will give you all your data at once.