Mario raises the first and most important issue - dead reckoning (whether with interial measurement units or vehicle speed and steering measurements) will always deviate from the true position with a rate that depends greatly on the quality of the models used and the accuracy of the sensors. From experience with outdoor vehicle automation, these issues can only be resolved through the utilisation of exteroceptive measurements (GPS measurements, tracking of landmark features etc.). If you are interested, the field of SLAM (simultaneous localisation and map building) concers the simultaneous tracking of the observer’s position as well as discovering and tracking appropriate landmark features. There is probably 10+ years of literature associated with this approach.

As for performing estimation schemes under QNX there are *many* approaches. There is an open-source library known as Bayes++ which provides a comprehensive (and at times near-impenetrable interfaces) collection of computational schemes (but I have not actually used this under QNX). Matlab’s C++ code can actually perform quite well provided that the original code leverages the internal optimisations associated with heavy matrix manipulations. As an example, when dealing with large matrices, the internal (highly-optimised) algorithms outperform libraries such as the boost/ublas approach as they use block algorithms instead of full-matrix calculations.

I would focus your attention firstly on constructing an appropriate estimation scheme (in matlab cause its quicker) and subsequently looking at implementation details. Unfortunately as you will find, the hardest problems with the filtering stuff once you have your head around the mathematics are associated with computational tricks and approaches to create a scheme which is numerically stable (using cholesky decompositions where matrix inversions are required etc…), tractable and sufficiently robust to floating-point limitations.

Some references to look at:

M.S. Grewal, L.R. Weill and A. P. Andrews “Global Positioning Systems, Inertial navigation and Integration” ((John Wiley and Sons, 2001) - Even comes with code.

Y. Bar-Shalom et. al. “Estimation with Applications to Tracking and Navigation” (John Wiley and Sons, 2001)

Eli Brookner “Tracking and Kalman Filtering Made Easy” (John Wiley and Sons, 1998)