Mag and velocity measurement fix. Fault detection working.