Added PID_nav init, doppler direction verified