I just learnt Arduino a couple of weeks ago and my goal currently is to obtain position from accelerometer and gyroscope data. To do this, I am using quaternions, which I obtained through the Madgwick filter. These are the steps that I am trying to follow:
- Get accelerometer and gyroscope raw data.
- Input them in the Madgwick filter to get quaternions.
- Calculate rotation matrix from quaternions.
- Implement the rotation matrix to the body-frame acceleration in order to obtain the inertial-frame acceleration.
- Double integrate the inertial-frame acceleration to get position.
However, as I read in Madgwick's paper, the filter fusion algorithm outputs an estimated orientation of the sensor frame relative to the Earth frame. On the other hand, I am looking for the orientation relative to the inertial frame. Is there any way to do this such as convert the gyroscope data from body frame to inertial frame before inserting it to the Madgwick filter or transform the quaternions from body frame to inertial frame?
I am hoping someone could point me to clear resources on this sort of problem, or explain the procedure, and would be deeply grateful for any assistance.