This function computes the average (mean) quaternion.

[Qavg]=avg_quaternion_markley(Q)

% Averaging Quaternions
% Since quaternions are not regular vectors, but rather representations
% of orientation, an average quaternion cannot just be obtained by taking
% a weighted mean. This function implements the work done by paper by
% F. Landis Merkley to calculate the average quaternion. The algorithm
% explained by F. Landis Markley at:
% http://www.acsu.buffalo.edu/~johnc/ave_quat07.pdf
% For this particular implementation, I would also like to reference Mandar
% Harshe:
% http://www-sop.inria.fr/members/Mandar.Harshe/knee-joint/html/index.html
%
% This algorithm is compared by rotqrmean from VoiceBox and found to
% produce quite similar results, yet it is more elegant, much simpler to
% implement and follow. (Though, there might be difference in signs)
%
% Usage :
% Q is an Mx4 matrix, where each row stores a quaternion to be averaged.
% In return, the function outputs Qavg, which is a single quaternion
% corresponding to the average.
%
% Tolga Birdal
function [Qavg]=avg_quaternion_markley(Q)
% Form the symmetric accumulator matrix
A=zeros(4,4);
M=size(Q,1);
for i=1:M
q=Q(i,:)';
A=q*q'+A; % rank 1 update
end
% scale
A=(1.0/M)*A;
% Get the eigenvector corresponding to largest eigen value
[Qavg, Eval]=eigs(A,1);
end