# Inertial navigation system: am I doing it wrong?

I'm trying to develop an inertial navigation system.

I can access data from an accelerometer sensor (acceleration on three axes) and gyroscope sensor (angular velocity on three axes).

First of all, I integrate my angular velocity data with respect of time, and get angles on all three axes at every moment ($x \to \phi, y \to \theta, z \to \psi$).

Then I feed the angles to this rotation matrix

and I use it to rotate my acceleration vector at that time, thus taking all my acceleration values on the same reference frame.

Finally, I integrate acceleration to get space travelled, using the simple formula $$s(t) = \frac12 a(t) t^2 + v(t - 1)t + s(t - 1)$$

My method seems to work fine with fake data, but performs really bad when I plug in the real data, the output is almost meaningless.

Am I doing something wrong with the math or I have to search the problem in my implementation?