Code : Fix bug in range finder fusion and simplify calculation

The maths has been modified so that only the terrain offset state is
modified by range finder measurements.