Orientation Kinematics with Direction Cosine Matrices
In a sentence, a direction cosine matrix is a 3×3 matrix containing the cosines between each of the 9 possible pairs of axes of two separate Cartesian coordinate systems.
In the context of inertial measurement units in robotics applications, a 3D vector could be represented in either the global (earth) or the body (local) frames of reference. For example, the location of an end effector may be represented as <1, 0, 0> in the body frame but have different and changing x, y, and z components in the global frame, and vice versa.
There is plenty of documentation on direction cosine matrices available on the web:
But the gist of it is that in talking about DCMs, we are considering only very small intervals of time, on the order of milliseconds. Certain assumptions can be made. Within these timeframes:
- An axis of a DCM, its rotational vector, and its linear velocity are approximately orthogonal to each other.
- Thus, the magnitude of the angular velocity of a unit vector approximately equals its linear velocity.
If, in sketching out various DCMs, matrices and vectors on paper in an effort to understand what is going on, you notice that you are drawing vectors in any more dimensions than one (maybe two, definitely not three), stop.
All you should need is this rotation matrix calculated from gyro outputs:
I’ve implemented this in imu.cpp (with lots of in-line documentation) for my tricopter on my github repository. (This is a direct link to the file, but it might break if I ever decide to change the filename.)
Update the DCM fast enough (50 to 200 Hz?) with the rotation matrix, and provided your gyro isn’t drifting too badly, the DCM should be fairly accurate even without input from an accelerometer and magnetometer.