Transformation from WGS84 to NED in EKF2

Hi there,
I’m trying to compare the GPS Position to the local/global position. For this i’m trying to convert the coordinates from WGS84 to NED in the same way, the EKF2 does it. To check if my transformation is right I apply it to the global position and compare the results with the local position, which to my understanding then should be the same.

I already tried two conversions, but both have an offset of ~1.5m at 500m from origin.

In the first conversion i first convert the WGS84 coordinates to ECEF, then calculate the offset to the reference position. I then convert this offset to an NED.

The second conversion is an approximation. I don’t quite understand it yet so this is the python code for it:

def wgs84_to_ned_alternative2(
    ref_coords: list, lat: pd.Series, lon: pd.Series, alt: pd.Series
) -> tuple[pd.Series, pd.Series]:
    """
    Convert WGS84 coordinates to NED coordinates based on a reference point.

    Args:
        ref_coords (list): The reference coordinates in [lat, lon, alt].
        lat (pd.Series): The latitude values to convert.
        lon (pd.Series): The longitude values to convert.
        alt (pd.Series): The altitude values to convert.

    Returns:
        tuple[pd.Series, pd.Series]: North, East meters to reference point.
    """

    # Constants
    a = 6378137.0  # Semi-major axis of Earth ellipsoid [m]
    e = 0.0818191908426  # Eccentricity of Earth ellipsoid

    # Convert reference latitude to radians
    lat_ref_rad = grad_zu_rad(ref_coords[0])

    # Compute auxiliary radius
    R = a / np.sqrt(1 - (e * np.sin(lat_ref_rad)) ** 2)

    # Transformation matrix T (diagonal elements only, since the others are zero)
    T11 = ((1 - e**2) / a**2) * R**3 + ref_coords[2]
    T22 = (R + ref_coords[2]) * np.cos(lat_ref_rad)
    T33 = -1

    # Compute relative vectors
    rel_lat = grad_zu_rad(lat - ref_coords[0])  # Convert latitude difference to radians
    rel_lon = grad_zu_rad(
        lon - ref_coords[1]
    )  # Convert longitude difference to radians
    rel_alt = alt - ref_coords[2]

    # Apply transformation
    north = T11 * rel_lat
    east = T22 * rel_lon

But both conversions end up giving me a wrong result.

I tried to look in the EKF source code but i did not find the location of the transformation.

Can anyone help?

Okay so I found the conversion in the geo.cpp file. In a comment there I found the link to http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html, where this conversion is described:

As a reference they give: Snyder, J. P. Map Projections–A Working Manual. U. S. Geological Survey Professional Paper 1395. Washington, DC: U. S. Government Printing Office, pp. 191-202, 1987.