)  = roll_e(i)  - kf.xp(1);
        pitch_e(i) = pitch_e(i) - kf.xp(2);
        yaw_e(i)   = yaw_e(i)   - kf.xp(3);

        <span class="comment">% Velocity correction</span>
        vel_e(i,1) = vel_e(i,1) - kf.xp(4);
        vel_e(i,2) = vel_e(i,2) - kf.xp(5);
        vel_e(i,3) = vel_e(i,3) - kf.xp(6);

        <span class="comment">% Position correction</span>
        lat_e(i) = lat_e(i) - kf.xp(7);
        lon_e(i) = lon_e(i) - kf.xp(8);
        h_e(i)   = h_e(i)   - kf.xp(9);

        <span class="co