Ликбез по кватернионам, часть 14: линейный метод Мортари-Маркли
Оглавление "Ликбеза по кватернионам":
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Прошло почти 40 лет с публикации метода Дэвенпорта и алгоритма QUEST. Он хорошо зарекомендовал себя во многих космических миссиях, отправившись к другим планетам и даже за границы Солнечной системы, но математики всё это время не оставляли надежд отыскать метод попроще, но в то же время столь же точный.
Одними из самых упорных были Дэниэл Мортари (Daniele Mortari) и Ф. Лэндис Маркли (F. Landis Markley), которые исследовали эту тематику на протяжении 30-40 лет, и в 2007 году обнаружили [D. Mortari, L. F. Markley, P. Singla, “Optimal linear attitude estimator,” J. Guid. Control Dyn. 30(6), 1619-1627], что если несколько переформулировать оптимизационную задачу, то она внезапно сведётся к решению системы из трёх линейных уравнений с тремя неизвестными!
Похоже, что это их открытие не наделало должного «шуму», отчасти потому что алгоритм QUEST уже был написан на всех возможных языках программирования, поэтому не так уж страшно, если в нём тяжело разобраться (а это именно так: мистер Шустер подарил своим коллегам баночку аспирина с запиской «принять перед работами по QUEST», тогда как профессора, которые хотели во что бы то ни стало завалить студентов, задавали им вывести этот алгоритм). Но проблема была отчасти и в том, что в новой статье говорилось о «конформном отображении Кейли», которое ставит матрицы поворота и кососимметричные матрицы во взаимно-однозначное соответствие, что и позволяет превратить задачу о нахождении матрицы поворота в задачу о нахождении кососимметричной матрицы, и такой «абстрактный» подход мог отпугнуть многих практиков. Возможно, они посмотрели на метод QUEST, затем на OLAE (Optimal Linear Attitude Estimator), потом снова на QUEST и снова на OLAE, и решили: «Я всё же QUEST выбираю. С ним я уже 20 лет знаком, а этого кота первый раз вижу!».
Но на самом деле, в постановке задачи Мортари-Маркли есть четкий геометрический смысл, который мы сейчас продемонстрируем.
Сперва отметим, что постановка задачи зачастую делается по принципу «искать под фонарём» - не потому что там потерял, а потому что искать легко. Кто вообще сказал, что наилучшей оценкой будет такая, которая минимизирует среднеквадратичную ошибку!? А почему бы не попробовать минимизировать сумму разностей, взятых по абсолютной величине, или попытаться снизить максимальную ошибку?
А ведь есть ещё разные варианты, как оценивать «расстояние» между двумя направлениями. Мы брали квадрат длины вектора разности, а ведь можно для оценки брать угол между векторами, и задача станет уже другой!
Зачастую говорится так: принципиальной разницы между подходами нет, для «приличных» исходных данных, когда ошибки малы, все подходы дадут практически тот же результат, но именно задача минимизации среднеквадратичной ошибки лучше всего поддаётся анализу! В задачах минимизации максимальной ошибки (минимакс) или минимизации суммы модулей ошибок мы неизбежно приходим к линейному программированию, или того хуже, выпуклой оптимизации, которые тоже решаются, но далеко не так красиво.
Поэтому нет ничего сильно «криминального», если мы попробуем найти какую-нибудь другую формулировку оптимизационной задачи, вместо задачи Вахбы (см. часть 12).
Мы уже заметили, что поворот вектора с помощью кватерниона (а тем более с помощью углов Эйлера-Крылова) записывается достаточно сложным образом. У нас есть исходный вектор, есть вектор, выражающий ось поворота и число, выражающее угол поворота, и нам нужно «честно» найти две компоненты, расположенные перпендикулярно к оси поворота, одна идёт вдоль исходного вектора (и пропорциональна cosφ), другая - поперёк (и пропорциональна sinφ).
Нельзя ли оценить, перейдёт ли один вектор в другой при повороте, не осуществляя непосредственно этот поворот? А если не перейдёт, то насколько велика получается ошибка? Уж не пустить ли вектора «навстречу друг другу» каким-то образом?
Итак, у нас есть эталонный вектор
, измеренный вектор
и некоторый поворот вокруг оси
на угол φ. Мы хотим проверить: действительно ли при таком повороте вектора
получится вектор
.
Как мы делали в части 5, разложим каждый из векторов на продольные и поперечные компоненты. Продольные компоненты параллельны вектору
, поперечные перпендикулярны ему:
При повороте продольная компонента измениться не может, поэтому, если векторы действительно совмещаются этим поворотом, должно получиться
. Чтобы оценить ошибку по продольной оси, достаточно вычесть эти компоненты друг из друга.
Теперь рассмотрим поперечные составляющие. На рисунке изображены векторы
в плоскости, перпендикулярной к
. Если они действительно переходят друг в друга при повороте, то они должны располагаться на одной окружности. Векторы выходят из одной точки O - центра окружности, а их концы мы обозначаем точками A, B. В каждой из них мы можем провести касательную к окружности, и если угол поворота не равен 180°, эти касательные пересекутся в некоторой точке O’. Из равенства двух треугольников на рисунке, а также зная, что между касательной и радиусом окружности прямой угол, мы можем вывести, что
Нас так заинтересовали касательные, поскольку мы очень легко можем найти векторы
и
:
где
- упоминавшийся нами в части 13 вектор Родрига.
Можно предложить такую оценку, что поворот, описываемый вектором Родрига, действительно переводит вектор
в вектор
:
- мы строим векторы
, выходящие из точки O,
- к ним мы прибавляем векторы
и
, соответственно. Если поворот верен, то эти суммы должны оказаться равны, т.е концы векторов действительно придут в одну точку O’;
- наконец, мы прибавляем векторы
и
, соответственно. Если эти векторы равны (а так и должно быть, если мы правильно нашли поворот!), то мы поднимемся из плоскости, перпендикулярной
, на одну и ту же высоту. Если же мы окажемся на разной высоте, это также свидетельствует об ошибке.
Заметим, что мы сложили между собой продольную и поперечные компоненты векторов и пришли к исходным векторам.
Теперь мы можем написать простой критерий, что вектор
действительно переходит в вектор
при повороте, который описывается вектором Родрига
:
То же самое мы можем записать в матричной форме:
\vec{\mathbf{r}}=(E-[\vec{\boldsymbol{\theta}}\times])\vec{\mathbf{b}}\,\,\,\,\,\,\,(14.1))
Лирическое отступление (о конформном отображении Кейли, на первый раз можно пропустить).
Выпишем в явном виде матрицу из левой части:
)
(для упрощения, мы обозначили координаты вектора
буквами x, y, z)
Её определитель равен 1+x2+y2+z2. Учитывая, что
и
, получаем определитель
Матрица в правой части отличается только знаком x,y,z, поэтому определитель у неё точно такой же.
Как видим, определитель никогда не обращается в ноль, поэтому к любой из этих матриц можно взять обратную. Тогда, помножив обе части (14.1) слева на^{-1})
, мы можем записать в матричном виде формулу для поворота вектора с помощью вектора Родрига:
^{-1}(E+[\vec{\boldsymbol{\theta}}\times])\vec{\mathbf{r}})
Или,
где R - матрица поворота, определяемая по формуле
^{-1}(E+[\vec{\boldsymbol{\theta}}\times]))
Это и есть отображение Кэйли. Выпишем, чему равна матрица R в явном виде:
^{-1}\left(\begin{array}{ccc}1&-z&y\\z&1&-x\\-y&x&1\end{array}\right)=)
\left(\begin{array}{ccc}1&-z&y\\z&1&-x\\-y&x&1\end{array}\right)=)
)
Что-то смутно знакомое… Если вектор Родрига при повороте на угол φ вокруг оси
определяется как
,)
а кватернион поворота на тот же угол вокруг той же оси:
то
Подставляя данные значения в матрицу, получим:
)
Наконец, учитывая, что
, можно переписать её в виде
)
то есть, мы пришли в точности к матрице поворота из части 5, подойдя к задаче совершенно с другой стороны и вообще не используя формализма кватернионов!
Но продолжим формулировать оптимизационную задачу. Если правильный поворот, в точности переводящий вектор
в вектор
, удовлетворяет условию
\vec{\mathbf{r}}=(E-[\vec{\boldsymbol{\theta}}\times])\vec{\mathbf{b}},)
то вектор ошибки мы можем записать в виде
\vec{\mathbf{r}}-(E-[\vec{\boldsymbol{\theta}}\times])\vec{\mathbf{b}}=(\vec{\mathbf{r}}-\vec{\mathbf{b}})+[\vec{\boldsymbol{\theta}}\times](\vec{\mathbf{r}}+\vec{\mathbf{b}}))
Как видно, вместо самих векторов нам понадобятся их суммы и разности. Обозначим их:
Лирическое отступление 2.
Если исходные векторы имеют единичную длину (а именно такова была постановка навигационной задачи!), то векторы
и
окажутся взаимно ортогональными:
=(\vec{\mathbf{r}}+\vec{\mathbf{b}},\vec{\mathbf{r}}-\vec{\mathbf{b}})=|\vec{\mathbf{r}}|^2+\cancel{(\vec{\mathbf{b}},\vec{\mathbf{r}})}-\cancel{(\vec{\mathbf{b}},\vec{\mathbf{r}})}-|\vec{\mathbf{b}}|^2=1-1=0)
Как ни странно, данный факт никак не будет использован в дальнейших выкладках.
Теперь мы можем сформулировать задачу следующим образом: найти вектор
такой, чтобы минимизировать величину
Как обычно, wn - весовые коэффициенты (в этот раз включить их в длину векторов не удастся), а индекс m, вероятно, означает Mortary-Markley, в честь авторов данной формулировки.
В этот раз мы имеем дело с задачей безусловной оптимизации, то есть на вектор
не накладывается никаких дополнительных ограничений!
Когда перед нами возникает оптимизационная задача, возникает жгучее желание сразу же взять частные производные по всем оптимизируемым параметрам и приравнять их к нулю! Так, в предыдущей главе ( Дэвенпорт берёт след) легко было прийти к задаче на собственные значения/собственные вектора, сразу же взяв дифференциал, и просто «в лоб» мы пришли бы к верному условию, затратив буквально полстраницы. Проблема лишь в том, что мы находим необходимое условие, а вот показать, что из 4 возможных собственных чисел нужно взять именно максимальное (чтобы найти глобальный максимум функции) мы бы не смогли. Именно для этого пришлось провести достаточно хитроумные выкладки и до последнего не брать производную!
Не будем спешить и сейчас и, для начала, распишем квадрат модуля в виде скалярного произведения вектора на себя:
=)
+|[\vec{\boldsymbol{\theta}},\vec{\mathbf{s}}_n]|^2)
Первое слагаемое не зависит от искомого вектора, а потому не представляет для нас интереса.
Второе слагаемое представляет собой смешанное произведение, его можно преобразовать следующим образом:
=2(\vec{\mathbf{d}}_n,\vec{\boldsymbol{\theta}},\vec{\mathbf{s}}_n)=2(\vec{\boldsymbol{\theta}},[\vec{\mathbf{s}}_n,\vec{\mathbf{d}}_n]))
Третье слагаемое неотрицательно, что пригодится нам в дальнейшем. Поработаем над ним:
\vec{\boldsymbol{\theta}})
Поскольку это выражение неотрицательно при любом значении
, матрица
является неотрицательно определённой (положительно полуопределённой) по определению. Кроме того, данная матрица симметричная.
Теперь мы можем переписать условие минимума следующим образом:
}=(\vec{\boldsymbol{\theta}},\sum_{n=1}^{N}w_n[\vec{\mathbf{s}}_n,\vec{\mathbf{d}}_n])+\frac{1}{2}\vec{\boldsymbol{\theta}}^T\left(\sum_{n=1}^{N}w_n[\vec{\mathbf{s}}_n\times]^T[\vec{\mathbf{s}}_n\times]\right)\vec{\boldsymbol{\theta}}\rightarrow{min})
Обозначим суммы:
Матрица M - симметричная и неотрицательно определённая, поскольку является суммой симметричных, неотрицательно определённых матриц. Если задача не вырождена (у нас больше одной звезды, и они не лежат на одной прямой), матрица M будет положительно определённой.
И матрицу, и вектор
можно вычислить - для этого нужны только исходные данные.
Условие минимума запишется так:
}=\vec{\boldsymbol{\theta}}^T\vec{\mathbf{v}}+\frac{1}{2}\vec{\boldsymbol{\theta}}^{T}M\vec{\boldsymbol{\theta}}\rightarrow{min})
Возьмём дифференциал, учитывая симметрию матрицы M:
}=d\vec{\boldsymbol{\theta}}^T\vec{\mathbf{v}}+d\vec{\boldsymbol{\theta}}^TM\vec{\boldsymbol{\theta}}=d\vec{\boldsymbol{\theta}}^T(\vec{\mathbf{v}}+M\vec{\boldsymbol{\theta}}))
В минимуме дифференциал должен обращаться в ноль, откуда получим необходимое условие:
При невырожденной матрице мы получим ровно одно решение.
Наконец, чтобы доказать, что это решение соответствует минимуму функции, возьмём второй дифференциал:
}=d\vec{\boldsymbol{\theta}}Md\vec{\boldsymbol{\theta}})
Поскольку M - положительно определённая матрица, второй дифференциал тоже будет положителен в каждой точке, а значит, любой найденный экстремум может быть только минимумом.
Получив вектор Родрига, мы можем преобразовать его в кватернион, например, составив ненормированный кватернион:
и отнормировав его:
Вот, собственно, и всё.
Выпишем шаги алгоритма:
1. Из "эталонных" векторов
и "измеренных" векторов
строим суммы и разности:
2. По векторам сумм и разности вычисляем вектор
и матрицу M:
3. Решаем линейное матричное уравнение (что то же самое - систему из 3 уравнений с 3 неизвестными):
4. Из вектора Родрига
получаем кватернион:
Если при решении уравнения мы видим, что вектор Родрига убегает куда-то в бесконечность, это означает - мы подобрались к особой точке. В этом случае надо отвернуть на 180 градусов по одной из осей координат, а потом, выдавая ответ, не забыть довернуться обратно. Как говорилось ранее - в задачах астронавигации это не проблема. Подробности - в оригинальной статье.
Кто знает - может быть, ещё 30 лет спустя Мортари и Маркли (а может, кто-то ещё) порадуют нас новым алгоритмом, который будет линейным и не будет содержать особых точек?
Дальше мне хочется привести примеры использования алгоритмов астронавигации, а ещё рассказать, как можно всю математику кватернионов реализовать на числах с фиксированной точкой и на процессоре, который умеет только складывать и сдвигать. Помню должок - интегрирование угловых скоростей высших порядков.
[Spoiler (click to open)]
Часть 1 - история вопроса
Часть 2 - основные операции
Часть 3 - запись вращения через кватернионы
Часть 4 - кватернионы и спиноры; порядок перемножения
Часть 5 - практическая реализация поворота
Часть 5 1/2 - введение метрики, "расстояния" между поворотами
часть 5 5/8 - метрика ненормированных кватернионов
часть 5 11/16 - красивая псевдометрика произвольных кватернионов
Часть 5 3/4 - исследуем "пространство поворотов"
Часть 5 7/8 - почти изотропный ёжик
Часть 6 - поворот по кратчайшему пути
Часть 6 1/4 - кратчайший поворот в общем случае
Часть 6 2/4 - поворот, совмещающий два направления
Часть 6 3/4 - кватернион из синуса и косинуса угла
Часть 6 7/8 - "уполовинивание угла" на плоскости
Часть 7 - интегрирование угловых скоростей, углы Эйлера-Крылова
Часть 8 - интегрирование угловых скоростей, матрицы поворота
Часть 8 1/2 - ортонормирование матрицы и уравнения Пуассона
Часть 9 - интегрирование угловых скоростей с помощью кватернионов
Часть 10 - интегрирование угловых скоростей, методы 2-го порядка
Часть 10 1/2 - интегрирование с поддержанием нормы
Часть 11 - интегрирование угловых скоростей, методы высших порядков (в разработке)
Часть 12 - навигационная задача
Часть 13 - Дэвенпорт берёт след!
Часть 14 - линейный метод Мортари-Маркли
Часть 15 - среднее от двух кватернионов
Часть 15 1/2 - проверка и усреднение кватернионов
Часть 16 - разложение кватерниона на повороты
Часть 17 - лидарная задача
Задачка к части 17
Дэвенпорт VS Мортари-Маркли
Мортари-Маркли берут реванш!
Дэвенпорт VS Мортари-Маркли, раунд 3
Прошло почти 40 лет с публикации метода Дэвенпорта и алгоритма QUEST. Он хорошо зарекомендовал себя во многих космических миссиях, отправившись к другим планетам и даже за границы Солнечной системы, но математики всё это время не оставляли надежд отыскать метод попроще, но в то же время столь же точный.
Одними из самых упорных были Дэниэл Мортари (Daniele Mortari) и Ф. Лэндис Маркли (F. Landis Markley), которые исследовали эту тематику на протяжении 30-40 лет, и в 2007 году обнаружили [D. Mortari, L. F. Markley, P. Singla, “Optimal linear attitude estimator,” J. Guid. Control Dyn. 30(6), 1619-1627], что если несколько переформулировать оптимизационную задачу, то она внезапно сведётся к решению системы из трёх линейных уравнений с тремя неизвестными!
Похоже, что это их открытие не наделало должного «шуму», отчасти потому что алгоритм QUEST уже был написан на всех возможных языках программирования, поэтому не так уж страшно, если в нём тяжело разобраться (а это именно так: мистер Шустер подарил своим коллегам баночку аспирина с запиской «принять перед работами по QUEST», тогда как профессора, которые хотели во что бы то ни стало завалить студентов, задавали им вывести этот алгоритм). Но проблема была отчасти и в том, что в новой статье говорилось о «конформном отображении Кейли», которое ставит матрицы поворота и кососимметричные матрицы во взаимно-однозначное соответствие, что и позволяет превратить задачу о нахождении матрицы поворота в задачу о нахождении кососимметричной матрицы, и такой «абстрактный» подход мог отпугнуть многих практиков. Возможно, они посмотрели на метод QUEST, затем на OLAE (Optimal Linear Attitude Estimator), потом снова на QUEST и снова на OLAE, и решили: «Я всё же QUEST выбираю. С ним я уже 20 лет знаком, а этого кота первый раз вижу!».
Но на самом деле, в постановке задачи Мортари-Маркли есть четкий геометрический смысл, который мы сейчас продемонстрируем.
Сперва отметим, что постановка задачи зачастую делается по принципу «искать под фонарём» - не потому что там потерял, а потому что искать легко. Кто вообще сказал, что наилучшей оценкой будет такая, которая минимизирует среднеквадратичную ошибку!? А почему бы не попробовать минимизировать сумму разностей, взятых по абсолютной величине, или попытаться снизить максимальную ошибку?
А ведь есть ещё разные варианты, как оценивать «расстояние» между двумя направлениями. Мы брали квадрат длины вектора разности, а ведь можно для оценки брать угол между векторами, и задача станет уже другой!
Зачастую говорится так: принципиальной разницы между подходами нет, для «приличных» исходных данных, когда ошибки малы, все подходы дадут практически тот же результат, но именно задача минимизации среднеквадратичной ошибки лучше всего поддаётся анализу! В задачах минимизации максимальной ошибки (минимакс) или минимизации суммы модулей ошибок мы неизбежно приходим к линейному программированию, или того хуже, выпуклой оптимизации, которые тоже решаются, но далеко не так красиво.
Поэтому нет ничего сильно «криминального», если мы попробуем найти какую-нибудь другую формулировку оптимизационной задачи, вместо задачи Вахбы (см. часть 12).
Мы уже заметили, что поворот вектора с помощью кватерниона (а тем более с помощью углов Эйлера-Крылова) записывается достаточно сложным образом. У нас есть исходный вектор, есть вектор, выражающий ось поворота и число, выражающее угол поворота, и нам нужно «честно» найти две компоненты, расположенные перпендикулярно к оси поворота, одна идёт вдоль исходного вектора (и пропорциональна cosφ), другая - поперёк (и пропорциональна sinφ).
Нельзя ли оценить, перейдёт ли один вектор в другой при повороте, не осуществляя непосредственно этот поворот? А если не перейдёт, то насколько велика получается ошибка? Уж не пустить ли вектора «навстречу друг другу» каким-то образом?
Итак, у нас есть эталонный вектор
, измеренный вектор
и некоторый поворот вокруг оси
на угол φ. Мы хотим проверить: действительно ли при таком повороте вектора
получится вектор
.
Как мы делали в части 5, разложим каждый из векторов на продольные и поперечные компоненты. Продольные компоненты параллельны вектору
, поперечные перпендикулярны ему:
При повороте продольная компонента измениться не может, поэтому, если векторы действительно совмещаются этим поворотом, должно получиться
. Чтобы оценить ошибку по продольной оси, достаточно вычесть эти компоненты друг из друга.
Теперь рассмотрим поперечные составляющие. На рисунке изображены векторы
в плоскости, перпендикулярной к
. Если они действительно переходят друг в друга при повороте, то они должны располагаться на одной окружности. Векторы выходят из одной точки O - центра окружности, а их концы мы обозначаем точками A, B. В каждой из них мы можем провести касательную к окружности, и если угол поворота не равен 180°, эти касательные пересекутся в некоторой точке O’. Из равенства двух треугольников на рисунке, а также зная, что между касательной и радиусом окружности прямой угол, мы можем вывести, что
Нас так заинтересовали касательные, поскольку мы очень легко можем найти векторы
и
:
где
- упоминавшийся нами в части 13 вектор Родрига.
Можно предложить такую оценку, что поворот, описываемый вектором Родрига, действительно переводит вектор
в вектор
:
- мы строим векторы
, выходящие из точки O,
- к ним мы прибавляем векторы
и
, соответственно. Если поворот верен, то эти суммы должны оказаться равны, т.е концы векторов действительно придут в одну точку O’;
- наконец, мы прибавляем векторы
и
, соответственно. Если эти векторы равны (а так и должно быть, если мы правильно нашли поворот!), то мы поднимемся из плоскости, перпендикулярной
, на одну и ту же высоту. Если же мы окажемся на разной высоте, это также свидетельствует об ошибке.
Заметим, что мы сложили между собой продольную и поперечные компоненты векторов и пришли к исходным векторам.
Теперь мы можем написать простой критерий, что вектор
действительно переходит в вектор
при повороте, который описывается вектором Родрига
:
То же самое мы можем записать в матричной форме:
Лирическое отступление (о конформном отображении Кейли, на первый раз можно пропустить).
Выпишем в явном виде матрицу из левой части:
(для упрощения, мы обозначили координаты вектора
буквами x, y, z)
Её определитель равен 1+x2+y2+z2. Учитывая, что
и
, получаем определитель
Матрица в правой части отличается только знаком x,y,z, поэтому определитель у неё точно такой же.
Как видим, определитель никогда не обращается в ноль, поэтому к любой из этих матриц можно взять обратную. Тогда, помножив обе части (14.1) слева на
, мы можем записать в матричном виде формулу для поворота вектора с помощью вектора Родрига:
Или,
где R - матрица поворота, определяемая по формуле
Это и есть отображение Кэйли. Выпишем, чему равна матрица R в явном виде:
Что-то смутно знакомое… Если вектор Родрига при повороте на угол φ вокруг оси
определяется как
а кватернион поворота на тот же угол вокруг той же оси:
то
Подставляя данные значения в матрицу, получим:
Наконец, учитывая, что
, можно переписать её в виде
то есть, мы пришли в точности к матрице поворота из части 5, подойдя к задаче совершенно с другой стороны и вообще не используя формализма кватернионов!
Но продолжим формулировать оптимизационную задачу. Если правильный поворот, в точности переводящий вектор
в вектор
, удовлетворяет условию
то вектор ошибки мы можем записать в виде
Как видно, вместо самих векторов нам понадобятся их суммы и разности. Обозначим их:
Лирическое отступление 2.
Если исходные векторы имеют единичную длину (а именно такова была постановка навигационной задачи!), то векторы
и
окажутся взаимно ортогональными:
Как ни странно, данный факт никак не будет использован в дальнейших выкладках.
Теперь мы можем сформулировать задачу следующим образом: найти вектор
такой, чтобы минимизировать величину
Как обычно, wn - весовые коэффициенты (в этот раз включить их в длину векторов не удастся), а индекс m, вероятно, означает Mortary-Markley, в честь авторов данной формулировки.
В этот раз мы имеем дело с задачей безусловной оптимизации, то есть на вектор
не накладывается никаких дополнительных ограничений!
Когда перед нами возникает оптимизационная задача, возникает жгучее желание сразу же взять частные производные по всем оптимизируемым параметрам и приравнять их к нулю! Так, в предыдущей главе ( Дэвенпорт берёт след) легко было прийти к задаче на собственные значения/собственные вектора, сразу же взяв дифференциал, и просто «в лоб» мы пришли бы к верному условию, затратив буквально полстраницы. Проблема лишь в том, что мы находим необходимое условие, а вот показать, что из 4 возможных собственных чисел нужно взять именно максимальное (чтобы найти глобальный максимум функции) мы бы не смогли. Именно для этого пришлось провести достаточно хитроумные выкладки и до последнего не брать производную!
Не будем спешить и сейчас и, для начала, распишем квадрат модуля в виде скалярного произведения вектора на себя:
Первое слагаемое не зависит от искомого вектора, а потому не представляет для нас интереса.
Второе слагаемое представляет собой смешанное произведение, его можно преобразовать следующим образом:
Третье слагаемое неотрицательно, что пригодится нам в дальнейшем. Поработаем над ним:
Поскольку это выражение неотрицательно при любом значении
, матрица
является неотрицательно определённой (положительно полуопределённой) по определению. Кроме того, данная матрица симметричная.
Теперь мы можем переписать условие минимума следующим образом:
Обозначим суммы:
Матрица M - симметричная и неотрицательно определённая, поскольку является суммой симметричных, неотрицательно определённых матриц. Если задача не вырождена (у нас больше одной звезды, и они не лежат на одной прямой), матрица M будет положительно определённой.
И матрицу, и вектор
можно вычислить - для этого нужны только исходные данные.
Условие минимума запишется так:
Возьмём дифференциал, учитывая симметрию матрицы M:
В минимуме дифференциал должен обращаться в ноль, откуда получим необходимое условие:
При невырожденной матрице мы получим ровно одно решение.
Наконец, чтобы доказать, что это решение соответствует минимуму функции, возьмём второй дифференциал:
Поскольку M - положительно определённая матрица, второй дифференциал тоже будет положителен в каждой точке, а значит, любой найденный экстремум может быть только минимумом.
Получив вектор Родрига, мы можем преобразовать его в кватернион, например, составив ненормированный кватернион:
и отнормировав его:
Вот, собственно, и всё.
Выпишем шаги алгоритма:
1. Из "эталонных" векторов
и "измеренных" векторов
строим суммы и разности:
2. По векторам сумм и разности вычисляем вектор
и матрицу M:
3. Решаем линейное матричное уравнение (что то же самое - систему из 3 уравнений с 3 неизвестными):
4. Из вектора Родрига
получаем кватернион:
Если при решении уравнения мы видим, что вектор Родрига убегает куда-то в бесконечность, это означает - мы подобрались к особой точке. В этом случае надо отвернуть на 180 градусов по одной из осей координат, а потом, выдавая ответ, не забыть довернуться обратно. Как говорилось ранее - в задачах астронавигации это не проблема. Подробности - в оригинальной статье.
Кто знает - может быть, ещё 30 лет спустя Мортари и Маркли (а может, кто-то ещё) порадуют нас новым алгоритмом, который будет линейным и не будет содержать особых точек?
Дальше мне хочется привести примеры использования алгоритмов астронавигации, а ещё рассказать, как можно всю математику кватернионов реализовать на числах с фиксированной точкой и на процессоре, который умеет только складывать и сдвигать. Помню должок - интегрирование угловых скоростей высших порядков.