3D positioning algorithm for low cost mobile robots

A new 3D positioning algorithm for low cost robots is proposed. The algorithm is based on a Finite State Machine to estimate the position and orientation of the robot. The system sets dynamically the parameters of the algorithm and makes it independent of the noise in the sensors. The algorithm has been tested for differential wheel drive robots, however it can be used with different types of robots in a simple way. To improve the accuracy of the system, a new reference system based on the accelerometer of the robot is presented which reduces the accumulative error that the odometry produces.