Code: Corrected bugs in optical flow fusion prediction

The derivations now need to be re-done, and the jacobians and Kalman gains
 for the various observation models updated