Reference Available Below:
An efficient orientation fillter for inertial and inertial/magnetic sensor arrays (Sebastian O.H. Madgwick)
Various algorithms of varying complexity and computational requirements are available to determine the orientation of an object using 9DOF sensors. The Kalman filter was considered and not selected due to issues of simulation, complexity, computational load, and lack of knowledge and experience to take advantage of numerically well conditioned forms of the filter.
Some existing autopilots are currently using a "DCM" algorithm that is computationally efficient for small microcontrollers (from Dr. Mahoney). The algorithm use gps velocity vector, accelerometers, a wind speed and direction estimator and optionally, a magnetic sensor, to null out gyroscope drift errors. The algorithm operates upon a 9-element Discrete Cosine Matrix to describe 3 unit vectors which can be used to determine the Euler Angles of a body relative to the Earth frame.
The algorithm I will use on XmegaPilot is very similar to "DCM". It also uses 3-axis gryoscopes, accelerometers and magnetometer, as a 9DOF sensor array. The MARG algorithm by Sebastian O.H. Madgwick operates upon quaternion vectors. This algorithm, also uses two inertial vectors, for the purpose of nulling out gyroscope drift errors. Other than quaternion math, a subtle difference between the two approaches exist in the method that vector error matching is tracked. The "DCM" algorithm uses a Proportional-Integral feedback loop to track the orientation vector. This MARG algorithm uses a gradient descent algorithm that follows along the vector lines of gravity and magnetic fields to arrive at the "single point" which represents orientation.
Both "MARG" and "DCM" are estimated to achieve comparable static perfomance, dynamic performance comparisons are difficult to predict and will be a source of development and refinement as the project moves forward.
Sebastian O.H. Madgwick has graciously provided research and code examples available here: