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 it's quaternion each iteration based on the data's rate of change. However, it's 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?