I'm using the Madgwick Filter to calculate estimates of velocity and position for some testing. The simulated data I'm comparing it to has ground truth values of the position, velocity, Euler angles, and direction cosine matrices.
The Madgwick Filter works by updating its quaternion each iteration based on the data's rate of change. However, its initial value is [1 0 0 0] unless I manually set it. I want the initial value to be as close as possible to the ground truth so I can get accurate drift values.
The library I'm using has functions to change representations between quaternions, DCMs, rotation matrices, and Euler angles. I can manage once I have accurate values in one of these forms.
The data with initial conditions is from this site at the bottom: Data
tl;dr - How can raw sensor data from an Accelerometer, Gyrometer, and Magnetometer be combined to calculate the initial quaternion, DCM, rotation matrix, or Euler angles?
I asked the question, though I figured it out so I might as well answer.
Turns out, using the sensor data doesn't provide enough information. You need to know the starting configuration before any data comes in. Lucky for me, this was given by the provided data set.
Well-defined functions are then able to convert this set to any of the needed forms.
This can be tested using the acceleration data:
Therefore,
After this operation, the RotatedAcceleration data at rest reads [0 0 9.8760], which is the amount of gravity listed on the spec sheet. Convert the Rotation Matrix into a quaternion and use that as the initial orientation.