Code: bugfixes and modifications to optical flow fusion

The scale factor and other state estimates still destablise during flight

Further experiments with  removing optimisations in range fusion are
required.