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.