Hi,
I'm using an IMU9150 which outputs data for acceleration, rotation and magnetometer information in 3 dimensions (so 9 pieces of data)
I am using the RTImuLib (https://github.com/richards-tech/RTIMULib) which works to use the data to determine the orientation of the data in 3 axis of rotation (alpha, beta and theta) and this works fine
But I'd like to take the accelerometer data and work out which way the module is being moved, e.g. upwards (away from the ground), downwards (towards the ground), as well as horizontal direction.
I plan to use the accelerometers, and I know they are not accurate, but I think they are good enough to give a rough idea of which direction the module is being moved (assuming I can average out the noise)
Anyway, the problem is that the accelerometers show 1.0G downwards all the time due to the force of gravity.
I need to deduct the gravity force on the accelerometer data to just leave the motion forces.
But the gravity shown by the accelerometers is relative to the plane of the module in X,Y and Z dimensions
so if the module is laying on a flat surface the values are 0,0,1.0 but if its rotated they will be values like 0.56,0.62,0.57
ie the sum of the values still adds up to 1.0, its just distributed across all 3 axis
Anyway. So what I need to do is take the gravity usual gravity vector (0,0,1.0) and rotate it through the angles in alpha beta and theta that the module is rotated (I get this data from the RTImuLib) then subtract this rotated gravity vector from the accelerometer data
This is where I'm struggling.
I have sound a reference on how to do this here
and so far I've got this far with the code
float accel[3]={0,0,0};// data will come from the accelerometers
float gravity[3]={0,0,1.0};// always vertically downwards at g = 1.0
float rotatedGravity[3];
float motionAcceleration[3];
// Angle of rotation will come from RTImuLib
float alpha= 0.1;// some aribitar values for testing
float beta= 0.2;// some aribitar values for testing
float theta = 0.3;// some aribitar values for testing
float rotationMatrix[3][3] =
{
{ cos(alpha)*cos(beta) , cos(alpha)*sin(beta)*sin(theta) - sin(alpha)*cos(theta) , cos(alpha)*sin(beta)*cos(theta) + sin(alpha)*sin(theta)},
{ sin(alpha)*cos(beta) , sin(alpha)*sin(beta)*sin(theta) + cos(alpha)*cos(theta) , sin(alpha)*sin(beta)*cos(theta) - cos(alpha)*sin(theta)},
{ -1* sin(beta) , cos(beta) * sin(theta) , cos(beta) * cos(theta) }
};
rotatedGravity[0]= gravity[0]*rotationMatrix[0][0] + gravity[1]*rotationMatrix[0][1] + gravity[3]*rotationMatrix[0][2] ;
rotatedGravity[1]= gravity[1]*rotationMatrix[1][0] + gravity[1]*rotationMatrix[1][1] + gravity[3]*rotationMatrix[1][2] ;
rotatedGravity[2]= gravity[2]*rotationMatrix[2][0] + gravity[1]*rotationMatrix[2][1] + gravity[3]*rotationMatrix[2][2] ;
motionAcceleration[0]=accel[0]-rotatedGravity[0];
motionAcceleration[1]=accel[1]-rotatedGravity[1];
motionAcceleration[2]=accel[2]-rotatedGravity[2];
};
However I'm not sure if this is correct or whether the order of rotation makes any difference (I think is does but I'm not sure how)
I'd appreciate any help in letting me know if what I'm doing is correct
Thanks
Roger