I am involved in a project of a robot which uses some sensors to follow a given trajectory. The robot will run with the image created in QNX. One of the most vital sensor used here is an IMU (Inertial Measurement Unit) that can be used to get acceleration, angular velocity as well as rotation matrix of the robot.
Since the function is to follow a trajectory, my first thought is that I have to obtain a position estimation of the robot from the IMU. Hence to do this, I need to somehow integrate the acceleration into velocity, and I am stuck here. Anybody has experience in integrating in C++? I thought I can use MATLAB to make a function and convert it to C++ code. I know that it can be done in Windows, but, is it possible in QNX? If so, anybody willing to teach me how to do it?
Hmm or are there any other way that can be done to control a robot with only acceleration, angular velocity and rotation matrix from the IMU? without the position?
If you can’t tell the initial position of the robot, you need to somehow move the robot to a “reset” position.
Yes you can use Matlab to generate C++ code ( which is usually OS independant) and then move that over the QNX machine.
Getting position from accelerometer is very tricky, error accumulates over time very fast. Plus you got noise to take care of. First time I played with such a device ( 6 years ago) it was sitting still on the table, but after 2 minutes the software I wrote told me we moved by 5 meters…
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)
Yup2, in my program later, I will define which one is the initial position, which will be the starting point of the robot. Thx a lot for the suggestion.
For the noise/drift which causes error in the double integration of the acceleration, I am also very aware of that, however, since my project is on using IMU to do a position tracking, it looks like I have to stick on it ahahaha, can’t use other better stuff such as GPS.
Hmm however… still I am really a beginner in MATLAB and QNX, hm… how do you convert a MATLAB code into C++? is it using the “mcc -m” matlab compiler? then after it has been converted, how to use it in the C++ code? will it be used as a function? which then we can give in some parameters for it?
