errors(i) = total_error;

    
        % Meridian and normal radii of curvature update
        [RM,RN] = radius(lat_e(i));

        % Radians-to-meters matrix
        Tpr = diag([(RM + h_e(i)), (RN + h_e(i)) * cos(lat_e(i)), -1]);

        % Position innovations in meters with lever arm correction
        zp = Tpr * ([lat_e(i); lon_e(i); h_e(i);] - [vor_dme.lat(gdx); vor_dme.lon(gdx); vor_dme.h(gdx);]) ...
            + (DCMbn * vor_dme.larm);

        % Velocity innovations with lever arm correction
      