adding robot position and orientation estimation
manual add dummy code from vector's