Среднее вращение кватерниона
quatAverage = meanrot(quat)quatAverage = meanrot(quat,dim)quatAverage = meanrot(___,nanflag)quatAverage = meanrot(quat)quat вдоль первого измерения массива, размер которого не действительно равняется 1.
Если quat является вектором, meanrot(quat) возвращает среднее вращение элементов.
Если quat является матрицей, meanrot(quat) возвращает вектор - строку, содержащий среднее вращение каждого столбца.
Если quat является многомерным массивом, то mearot(quat) действует вдоль первого измерения массива, размер которого не равняется 1, обрабатывая элементы как векторы. Эта размерность становится 1, в то время как размеры всех других размерностей остаются то же самое.
Функция meanrot нормирует входные кватернионы, quat, прежде, чем вычислить среднее значение.
quatAverage = meanrot(quat,dim)dim. Например, если quat является матрицей, то meanrot(quat,2) является вектор-столбцом, содержащим среднее значение каждой строки.
quatAverage = meanrot(___,nanflag)NaN от вычисления для какого-либо из предыдущих синтаксисов. meanrot(quat,'includenan') включает все значения NaN в вычисление, в то время как mean(quat,'omitnan') игнорирует их.
Создайте матрицу кватернионов, соответствующих трем наборам Углов Эйлера.
eulerAngles = [40 20 10; ... 50 10 5; ... 45 70 1]; quat = quaternion(eulerAngles,'eulerd','ZYX','frame');
Определите среднее вращение, представленное кватернионами. Преобразуйте среднее вращение в Углы Эйлера в градусах для удобочитаемости.
quatAverage = meanrot(quat) eulerAverage = eulerd(quatAverage,'ZYX','frame')
quatAverage =
quaternion
0.88863 - 0.062598i + 0.27822j + 0.35918k
eulerAverage =
45.7876 32.6452 6.0407
Используйте meanrot по последовательности кватернионов, чтобы составить в среднем аддитивный шум.
Создайте вектор 1e6 кватернионы, расстояние которых, как задано функцией dist, от кватерниона (1,0,0,0) нормально распределено. Постройте Углы Эйлера, соответствующие шумному вектору кватерниона.
nrows = 1e6; ax = 2*rand(nrows,3) - 1; ax = ax./sqrt(sum(ax.^2,2)); ang = 0.5*randn(size(ax,1),1); q = quaternion(ax.*ang ,'rotvec'); noisyEulerAngles = eulerd(q,'ZYX','frame'); figure(1) subplot(3,1,1) plot(noisyEulerAngles(:,1)) title('Z-Axis') ylabel('Rotation (degrees)') hold on subplot(3,1,2) plot(noisyEulerAngles(:,2)) title('Y-Axis') ylabel('Rotation (degrees)') hold on subplot(3,1,3) plot(noisyEulerAngles(:,3)) title('X-Axis') ylabel('Rotation (degrees)') hold on
Используйте meanrot, чтобы определить средний кватернион, учитывая вектор кватернионов. Преобразуйте в Углы Эйлера и постройте результаты.
qAverage = meanrot(q); qAverageInEulerAngles = eulerd(qAverage,'ZYX','frame'); figure(1) subplot(3,1,1) plot(ones(nrows,1)*qAverageInEulerAngles(:,1)) title('Z-Axis') subplot(3,1,2) plot(ones(nrows,1)*qAverageInEulerAngles(:,2)) title('Y-Axis') subplot(3,1,3) plot(ones(nrows,1)*qAverageInEulerAngles(:,3)) title('X-Axis')
meanrot и ограниченияАлгоритм meanrot
Функция meanrot выводит кватернион, который минимизирует норму Фробениуса в квадрате различия между матрицами вращения. Рассмотрите два кватерниона:
q0 не представляет вращения.
q90 представляет 90 вращений степени вокруг оси X.
q0 = quaternion([0 0 0],'eulerd','ZYX','frame'); q90 = quaternion([0 0 90],'eulerd','ZYX','frame');
Создайте развертку кватерниона, qSweep, который представляет вращения от 0 до 180 градусов об оси X.
eulerSweep = (0:1:180)'; qSweep = quaternion([zeros(numel(eulerSweep),2),eulerSweep], ... 'eulerd','ZYX','frame');
Преобразуйте q0, q90 и qSweep к матрицам вращения. В цикле вычислите метрику, чтобы минимизировать для каждого члена развертки кватерниона. Постройте результаты и возвратите значение Эйлеровой развертки, которая соответствует минимуму метрики.
r0 = rotmat(q0,'frame'); r90 = rotmat(q90,'frame'); rSweep = rotmat(qSweep,'frame'); metricToMinimize = zeros(size(rSweep,3),1); for i = 1:numel(qSweep) metricToMinimize(i) = norm((rSweep(:,:,i) - r0),'fro').^2 + ... norm((rSweep(:,:,i) - r90),'fro').^2; end plot(eulerSweep,metricToMinimize) xlabel('Euler Sweep (degrees)') ylabel('Metric to Minimize') [~,eulerIndex] = min(metricToMinimize); eulerSweep(eulerIndex)
ans =
45
Минимум метрики соответствует развертке Угла Эйлера в 45 градусах. Таким образом, meanrot задает среднее значение между quaterion([0 0 0],'ZYX','frame') и quaternion([0 0 90],'ZYX','frame') как quaternion([0 0 45],'ZYX','frame'). Вызовите meanrot с q0 и q90, чтобы проверить тот же результат.
eulerd(meanrot([q0,q90]),'ZYX','frame')
ans =
0 0 45.0000
Ограничения
Метрика, что использование meanrot, чтобы определить среднее вращение не уникально для кватернионов значительно далеко друг от друга. Повторите эксперимент выше для кватернионов, которые разделяются 180 градусами.
q180 = quaternion([0 0 180],'eulerd','ZYX','frame'); r180 = rotmat(q180,'frame'); for i = 1:numel(qSweep) metricToMinimize(i) = norm((rSweep(:,:,i) - r0),'fro').^2 + ... norm((rSweep(:,:,i) - r180),'fro').^2; end plot(eulerSweep,metricToMinimize) xlabel('Euler Sweep (degrees)') ylabel('Metric to Minimize') [~,eulerIndex] = min(metricToMinimize); eulerSweep(eulerIndex)
ans = 159
Средние значения кватерниона обычно вычисляются для вращений, которые являются друг близко к другу, который делает случай ребра показанным в этом примере вряд ли в реальных приложениях. Чтобы составить в среднем два кватерниона, которые являются значительно далеко друг от друга, используйте функцию slerp. Повторите эксперимент с помощью slerp и проверьте, что возвращенное среднее значение кватерниона более интуитивно для больших расстояний.
qMean = slerp(q0,q180,0.5); q0_q180 = eulerd(qMean,'ZYX','frame')
q0_q180 =
0 0 90.0000
meanrot определяет среднее значение кватерниона, , согласно [1] (Sensor Fusion and Tracking Toolbox). кватернион, который минимизирует норму Фробениуса в квадрате различия между матрицами вращения:
[1] Markley, Ф. Лэндис, Янг Чен, Джон Лукас Крэссидис и Яаков Ошман. "Средние Кватернионы". Журнал Руководства, Управления и Динамики. Издание 30, Выпуск 4, 2007, стр 1193-1197.