the observable states at current vor_dme time
        ob(gdx) = rank(obsv(kf.F, kf.H));

        %% INS/vor_dme CORRECTIONS

        % Quaternion correction
        qua_skew = -skewm(qua(1:3));    % According to Crassidis, qua_skew should be
                                        % positive, but if positive NaveGo diverges.
        % Crassidis, Eq. A.174a
        Xi = [qua(4)*eye(3) + qua_skew; -qua(1:3)'];

        % Crassidis, Eq. 7.34
        qua = qua + 0.5 .* Xi * kf.xp(1:3);
        qua = qua / norm(