qua);          % Brute-force normalization

        % DCM correction
        DCMbn = qua2dcm(qua);

        % Attitude correction, method 1
        %         euler = qua2euler(qua);
        %         roll_e(i) = euler(1);
        %         pitch_e(i)= euler(2);
        %         yaw_e(i)  = euler(3);

        % Attitude correction, method 2
        roll_e(i)  = roll_e(i)  - kf.xp(1);
        pitch_e(i) = pitch_e(i) - kf.xp(2);
        yaw_e(i)   = yaw_e(i)   - kf.xp(3);

        % Velocity correction
      