public static double[] rotQuatAvg(CircularBuffer <SensorCalibratorData> calibrators)
{
double[] sums = new double[4];
double[] output = new double[4];
for (int i = 0; i < calibrators.Capacity; i++)
{
for (int j = 0; j < 4; j++)
{
sums[j] += calibrators[i].rotquat[j] * calibrators[i].rotquat[j];
}
}
for (int j = 0; j < 4; j++)
{
output[j] = Math.Sqrt(sums[j] / calibrators.Capacity);
}
return(output);
//adapted from Tolga Birdal
/*
* Based on
* Markley, F. Landis, Yang Cheng, John Lucas Crassidis, and Yaakov Oshman.
* "Averaging quaternions." Journal of Guidance, Control, and Dynamics 30,
* no. 4 (2007): 1193-1197.
* */
/*
* int M = calibrators.size;
* //A = zeros(4,4)
* double[,] A = new double[,] { { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 }, { 0, 0, 0, 0 } };
*
* for (int i = 0; i < M; i++)
* {
* //q = Q(i,:)';
* double[] q = new double[4];
* for (int j = 0; j < 4; j++ )
* q[j] = calibrators[i].rotquat[j];
*
* //A = q*q'+A; % rank 1 update
* for (int j = 0; j < 4; j++)
* for (int k = 0; k < 4; k++)
* {
* //q*q'[j, k] = q[j] * q[k];
* A[j, k] += q[j] * q[k];
* }
* }
*
* //% scale
* //A=(1.0/M)*A;
* double s = 1.0/M;
* for (int j = 0; j < 4; j++)
* for (int k = 0; k < 4; k++)
* A[j, k] *= s;
*
* //% Get the eigenvector corresponding to largest eigen value
* //[Qavg, Eval] = eigs(A,1);
* double[] wr, wi;
* double[,] vl, vr;
* alglib.rmatrixevd(A, 4, 3, out wr,out wi,out vl,out vr);
*
* ShowArrayInfo(wr);
* ShowArrayInfo(wi);
*
* ShowArrayInfo(vl);
* ShowArrayInfo(vr);
*
* double[] Qavg = new double[4];
* for (int i = 0; i < 4; i++)
* Qavg[i] = wr[i];
*
* return Qavg;
* */
}
