Inertial navigation system: am I doing it wrong?

Question

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?

Answer 1

One possibility is that your approach is hugely sensitive to measurement uncertainty: integrating noisy signals can be a huge problem. You might think about ways to average the measurements over time, so that they're (hopefully!) more stable.

Another possibility is that your instrument doesn't output the data in quite the format that you think. Have you verified that your inputs are sensible?