There are several questions here, so I'll try to address them all.
First, using the integrated gyro output. Integrating the gyro output, which provides angular velocity, gives you an orientation estimate. A lot of people that use the term "Euler Angles" generally actually mean Tait-Bryan angles. Proper Euler angles only use two axes, like an X-Z-X rotation, where the Tait-Bryan angles use all three axes, like an X-Y-Z rotation. Aviation/navigation angles like Roll-Pitch-Yaw (RPY) are Tait-Bryan angles.
If your question is how to convert those angles to a rotation matrix, then the answer is on the Wikipedia page for the Euler Angles under the "Rotation Matrix" section.
Step 4 in my other answer then uses that rotation matrix to transform the accelerometer outputs. It's done with a matrix multiplication:
$$
\left[\begin{matrix}
\ddot{x}_w \\
\ddot{y}_w \\
\ddot{z}_w \\
\end{matrix}\right] = \left[R\right] \left[\begin{matrix}
\ddot{x}_b \\
\ddot{y}_b \\
\ddot{z}_b \\
\end{matrix}\right]
$$
The Eigen library would be a great starting place for getting the tools to easily do matrix math. You could do it all by hand, but you're reinventing the wheel and liable to make some math error that's going to cost you a lot of time.
The big alternative here is, as I mention in the other answer and you mention here, the Madgwick filter!. The C implementation available there (warning: direct download) has orientation in quaternion form, with q0
, q1
, q2
, and q3
as global, and then there are two functions. The first is MadgwcikAHRSupdate
, which uses gyro inputs (gx
, gy
, gz
), accelerometer inputs (ax
, ay
, az
), and magnetometer inputs (mx
, my
, mz
).
If you don't have a magnetometer, use the second function, MadgwickAHRSupdateIMU
. It only uses gyro and accelerometer outputs.
Be sure to update the sample frequency; the default is 512 Hz.
The Madgwick filter will do a better job of estimating orientation than just integration of the gyro output because the Madgwick filter uses the gravity vector from the accelerometer readings to eliminate orientation drift.
Once you have the output of the Madgwick filter, which again is the orientation in quaternion, you again convert that to a rotation matrix, transform your accelerometer readings, and then double integrate to get position. The conversion from quaternion to rotation matrix is also on Wikipedia.
Give these a shot and let us know how it goes! Best of luck!