Code : change to 22 state computation

This is an experiment that uses the terrain position estimate from the
2 state ekf instead of the 23rd state