oriDiff.m

Go to the documentation of this file.

%> @brief this function calculates the difference between estimated and true
%> orientations. the initial yaw is aligned first.
%>
%> @param ori_est - estimated orientations (quaternion vector)
%> @param ori_true - true orientations (quaternion vector)
%>
%> @retval d - orientation differences in degrees (vector)
%> @retval ori_est - estimated orientations after yaw alignment (quaternion vector)
%>
%> @file oriDiff.m
%>
%> @copyright see the file @ref LICENSE in the root directory of the repository
function [d, ori_est] = oriDiff(ori_est, ori_true)
 
arguments
    ori_est (:,1) quaternion
    ori_true (:,1) quaternion
end
 
assert(length(ori_est) == length(ori_true), "Length of orientations must be equal");
 
% initial reference yaw
ref_yaw = ori_true(1).eulerd("ZYX", "frame");
ref_yaw = ref_yaw(1);
 
% Initial estimation yaw
est_yaw = ori_est(1).eulerd("ZYX", "frame");
est_yaw = est_yaw(1);
 
% align initial yaw
diff = ref_yaw - est_yaw;
q_corr = quaternion([diff,0,0], "eulerd", "ZYX", "frame");
 
% apply correction
ori_est = q_corr .* ori_est;
 
% calculate differences
d = ori_est.conj .* ori_true;
 
% get absolute differende
d = 2*acos(d.normalize.parts) * 180 / pi;
d = mod(d + 180, 360) - 180;
 
end