Add encoder dead reckoning code