Issues with the comparison between the estimated position and ground-truth one

[robocar2018]

@mbrossar , thanks for the great paper on dead reckoning using IEKF.

I found there was an issue when we made comparison between the estimated position and the ground-truth one. Basically in the code, we use the following logic to compute the position, in this mode, we assume a flat-earth model , not a wgs84 ellipsoid model,

acc = Rot_prev.dot(u[3:6] - b_acc_prev) + self.g 
v = v_prev + acc * dt
p = p_prev + v_prev*dt + 1/2 * acc * dt**2

However, the ground truth position is computed from a Mercator projection , which is basically a nonlinear transformation from the (latitude, longitude). So after Mercator projection (code is shown below), for example, vehicle moving distance of 1 meter on the ellipsoid surface is NOT equal to 1 meter after the Mercator projection.

def pose_from_oxts_packet(packet, scale):
        """Helper method to compute a SE(3) pose matrix from an OXTS packet.
        """
        er = 6378137.  # earth radius (approx.) in meters

        # Use a Mercator projection to get the translation vector
        tx = scale * packet.lon * np.pi * er / 180.
        ty = scale * er * np.log(np.tan((90. + packet.lat) * np.pi / 360.))
        tz = packet.alt
        t = np.array([tx, ty, tz])

        # Use the Euler angles to get the rotation matrix
        Rx = KITTIDataset.rotx(packet.roll)
        Ry = KITTIDataset.roty(packet.pitch)
        Rz = KITTIDataset.rotz(packet.yaw)
        R = Rz.dot(Ry.dot(Rx))

        # Combine the translation and rotation into a homogeneous transform
        return R, t

A reasonable way to compare seems to be like: Using an ellipsoid mode, and since v is resolved in the ned-frame, we can compute the delta-longitude and delta-latitude, then updated both longitude and latitude. Then we project those longitude and latitude to the x, y using Mercator projection, and finally we could compare to the ground truth. How do you think? Thanks.