Matlab implementation of dead reckoning algorithm