Calculating heading error in different ways leads to different results in Matlab, why?

54 Views Asked by At

I try to compute the heading error of a trajectory which is estimated by a "visual inertial odometry" system (VIO). I tried it in 3 different ways which all resulted in 3 slightly different results. At first I thought the reason for this are numerical problems, but since Matlab works with double precision numbers, I don't think that's the reason.

From the VIO algorithm I get a series of quaternions $ \mathbf v$ which represent the estimated orientations. I also have a series of quaternions $ \mathbf g$ which represent the groundtruth orientations.

First way

To calculate the difference $\boldsymbol \delta$ between these two orientations I multiply one with the inverse of the other one. After that, the result is converted into euler angles for better visualisation. $$ \boldsymbol \delta = \mathbf v \cdot \mathbf g^{-1} $$ The matlab Code is the following, whereby the quaternions are represented as Nx4 double arrays in Matlab.

d = quatdivide(g, v);
d_euler = rad2deg(quat2eul(d));

Second way

Almost the same as in the first way, but in Matlab the quaternions are represented as Nx1 quaternion arrays. This is done by converting the orientations represented as Nx4 double arrays into Nx1 quaternion arrays using the quaternion() Matlab function. This allows to just divie two quaternions without using the function quatdivide():

v = quaternion(v);
g = quaternion(g);

d = v./g;
d_euler = rad2deg(quat2eul(d));

Third way

In the third method, the orientations represented by quaternions are first converted into euler angles which are then subtracted afterwards.

v_euler = (quat2eul(v_q));
g_euler = (quat2eul(g_q));
d_euler = (rad2deg(v_euler - g_euler));

for i=1:length(d_euler)
    for j=1:3
        if d_euler(i,j) > 180
            d_euler(i,j) = d_euler(i,j) - 360;
        end
        if d_euler(i,j) < -180
            d_euler(i,j) = d_euler(i,j) + 360;
        end
    end
end

Results

As can be seen in the following image, all three methods yield slightly different results: Results. What disturbs me the most is, that method 1 and 2 yield different results, but I can't figure out why. I also tried to change the order quatdivide(v, g) with no luck.