EKF fixes

Some EKF fixes, discussed here: https://github.com/PX4/Firmware/issues/973

@LorenzMeier, any comments on this? I want to cleanup estimator to replace position_estimator_inav ASAP, but I see a lot of magic and strange code that needs review.