I am using a tri-axis accelerometer and tri-axis gyroscope to measure the linear acceleration of a body. I need to get the orientation of the body in euler form in order to rotate the accelerometer readings from the body frame into the earth frame. Please help I'm so stuck
$\begingroup$
$\endgroup$
Add a comment
|
3 Answers
$\begingroup$
$\endgroup$
Look into a complementary filter. It isn't the correct way to go out this but it will give you usable data for attitudes around level. It's also worth mentioning that you will not be able to track yaw. There is no way to account for bias/noise with the two sensors you've listed.
complementary filter: http://www.pieter-jan.com/node/11
$\begingroup$
$\endgroup$
First you need to integrate the output from the gyro to get the actual X, Y and Z angles.
angleX = gyroAngleX + gyroInputX
angleY = gyroAngleY + gyroInputY
However this value will drift over time so you will need to use a complementary filter or kalman filter. Personally, I would recommend a complementary filter because it is much simpler to implement.
First you must find the angle from the accelerometer using a little bit of trigonometry.
accelAngleX = atan2(accelY, accelZ) * 180/M_PI;
accelAngleY = atan2(-accelX, sqrt(accelY*accelY + accelZ*accelZ)) * 180/M_PI;
Then get the actual angle using this formula.
angleX = 0.98*angleX + 0.02*accelAngleX
angleY = 0.98*angleY + 0.02*accelAngleY
The variables above must be the same variables used when calculating the gyro angle. The 0.98 and 0.02 can be tuned to get the best output, but they should always add up to one.
$\begingroup$
$\endgroup$
Using the Gyroscope
First thing to note is that the gyroscope is reporting angular rates with respect to the sensor. So if the sensor is rotating with some rate, the data it's outputting will be in the frame of the rotating sensor, NOT with respect to the fixed global frame. In other words, the X-Y-Z coordinate axes of the sensor are spinning with the sensor, whereas the global X-Y-Z axes remain fixed.
So the first task (before integrating), is to convert the angular rates from the sensor-body frame to angular rates in the global frame.
Sections 9.1, 9.2, and 9.3 in this OCW pdf lays out how to do this pretty well: https://ocw.mit.edu/courses/mechanical-engineering/2-017j-design-of-electromechanical-robotic-systems-fall-2009/course-text/MIT2_017JF09_ch09.pdf
Once you have the angular rates in the global frame, you can then integrate and accumulate the angles as mentioned in other answers.
Using the Accelerometer
Pitch and roll Euler angles in the global frame can also be calculated with the accelerometer.
When the sensor is sitting at rest, it will sense the force due to gravity in the negative Z direction as 9.8 m/s^2. Using this knowledge, we can find how much the sensor has pitched or rolled, by calculating what component of the gravity vector has moved from the negative Z axis, to other axes.
Equations that you can use:
Pitch Angle $= \arctan(\frac{A_y}{\sqrt{A_x^2+A_z^2}})$
Roll Angle $= \arctan(\frac{A_x}{\sqrt{A_y^2+A_z^2}})$
Note that this won't give exactly correct results when the sensor is being accelerated by an external force (when you are pushing or rotating it), because there are other unknown forces besides gravity acting on the sensor.
