nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Хороший алгоритм, да рангом не вышел!

С нахрапу реализовать алгоритм сопровождения на ассемблере (худо-бедно описание здесь), в целых числах не получилось. Решил пока отставить в сторону масштабирование и сразу посмотреть, а что там с итоговой матрицей 6х6 и вектором 6х1?

Пока я всё это дело ковырял в плавающей точке, матрица получалась вот такой (с 300 метров, прямой наводкой):


(матрица симметричная, верхний треугольник не рисуем, чтобы не захламлять рисунок)

Числа прям подходящие для целочисленного исполнения - диагональные не превышают 65536 (а мы знаем, что на диагонали у нас неотрицательные значения), все остальные не превышают 32768 - прямо то что надо.

Но это лишь случайность. По мере сближения, эти числа будут меняться в широких пределах.


Размерность матрицы довольно интересная: пикс2/рад2 для верхнего блока 3х3, пикс2/рад/м для боковых блоков 3х3, и пикс22 для нижнего блока 3х3.

Оно и понятно: неизвестные у нас тоже "разнородные" - три угла (первые 3 переменные) и три компоненты вектора параллельного переноса (оставшиеся 3).

Тот факт, что туда угодили "пиксели" - не очень хорошо, так уж исторически вышло, что я оптимизировал расхождение между реальными координатами точек на "картинке" и "спроецированными" по данным 6 параметрам взаимного положения, поэтому они повсюду и "пролезли".

Сейчас я уже понял, что удобнее работать в безразмерных величинах, грубо говоря в радианах (или скорее тангенс от угла в радианах, но поле зрения 24°, т.е не более 12° от центра, поэтому разница не очень велика), что приводит попросту к делению всех компонент на 24172. Видимо, это одна из причин, почему я не стал так делать - работать с тысячными и миллионными долями как-то психологически неуютно!

Дальше напрашивался шаг: работать не с метрами непосредственно, а с "масштабированными" метрами, в соответствии с нашей самопальной "плавающей точкой". Т.е у вектора параллельного переноса есть мантиссы (1..2 для X-компоненты и -1..+1 для Y,Z) и общая экспонента, от -1 до 8 (т.е для дистанций от 0,5 метра до 512 метров).

Мы обязаны были учитывать экспоненту, когда складывали вектор параллельного переноса с координатами точек мишени, а вот после этого, она чудесным образом отовсюду сокращается! Оно и понятно: мы делаем проецирование трёхмерных точек на плоскость фотоприёмной матрицы, и поскольку экспонента для всех общая, она и сократится.

И если 3 неизвестных - "поправку к вектору параллельного переноса" - тоже принять в этом масштабе, то порядок компонент матрицы практически "стабилизируется" - два самых больших компонента будут почти всегда вблизи единицы, меняясь не более чем в 4 раза, по мере того, как мантисса меняется от 1 до 2, а в этой симметричной матрице берётся квадрат.

Кроме того, такой подход очень близок принципу, изложенному в предновогодней загадке и новогодней отгадке: мы выбрали неизвестные ровно в том масштабе, в котором они будут использоваться дальше, который мы подобрали, чтобы выполнялись требования ТЗ. И именно тогда вычислительные ошибки при работе с этой матрицей должны оказать наименьшее влияние на результат.

Казалось, что жизнь идёт на лад, пока я не построил матрицу по описанной методике. Вот что получилось:


может, понятнее будет записать всю симметричную матрицу целиком:


Просто шикарно: у неё нулевой определитель, уравнение имеет множество решений!
Правда, всё не так страшно, как показалось на первый взгляд: её ранг равен 5, т.е решений конечно бесконечно много, но хотя бы "по одной оси". Убери сверху единицу - будет ранг 4 - уже хуже. Да и в целом, многие коэффициенты, получается, имеют очень низкую точность представления, когда задействовано буквально несколько битов.

Потом, правда, я заметил, что это уравнение для углов φx, φy, φz, а мы на самом деле должны искать половинные углы, чтобы из них составить "кватернион малого поворота" и домножить имеющийся. Из-за этого верхний блок 3х3 нужно домножить на 4, а боковые - на 2. И ещё в данном конкретном случае можно всё домножить на 2, поскольку можно попытаться держать на диагонали беззнаковые числа, от 0 до 65535. Если всё это сделать, получим такое:



Эта матрица, по крайней мере, невырожденная, решение системы уравнений снова одно :) Но столь "пустой" верхний квадрат 4х4 несколько напрягает.

В принципе, это "вскрывает" проблему измерения всех 6 компонент с большой дальности. Действительно, первые 3 переменные - это угол крена (он ещё худо-бедно здесь представлен, только очень сильно "перемешан" с параллельными переносами) и два "пассивных угла" (тангажа и курса), они же "ракурсы". То есть, мы должны с дистанции 300 метров догадаться, повёрнут ли стыковочный узел строго к нам, или смотрит немножко вбок. И 4-я переменная, которая очень паршивенько здесь представлена - это дальность. С её измерением тоже проблемы, но и в ТЗ заложена точность 5 метров на дистанции 100..300 метров, и 5% на дистанции 0,5..100 метров.

Но всё равно мне это не нравится... Вот даже LDLT-разложение не работает нихрена, жалуется, что матрица не является положительно определённой, а нахождение обратной матрицы "в лоб" привело к появлению отрицательных элементов на главной диагонали, что является лютым ужасом - ведь эта обратная матрица должна "по совместительству" являться ковариационной матрицей шума измерений, и по главной диагонали - дисперсии, которые просто обязаны быть положительными!

Всё-таки надо это дело отмасштабировать чуть получше. Сейчас буду моделировать в разных вариантах...

Вот и не знаю - может, написать процедуры для работы с плавающей точкой - и не париться? Правда, в нынешнем виде это всё равно усложнит программу: вместо обычных ADD, SUB, MUL, FMA, и хитрого адреса типа [X+8i+k] в Src, придётся сначала занести значение, причём не очень понятно, куда, а потом вызвать процедуру. В общем, не складывается что-то каменный цветок.
Tags: tex, ПЛИС, кватернионы-это просто (том 1), математика, работа, странные девайсы
Subscribe

Recent Posts from This Journal

  • Формулы приведения, что б их... (и atan на ТРЁХ умножениях)

    Формулу арктангенса на 4 умножениях ещё немножко оптимизировал с помощью алгоритма Ремеза: Ошибка уменьшилась с 4,9 до 4,65 угловой секунды, и…

  • Алгоритм Ремеза в экселе

    Вот и до него руки дошли, причина станет ясна в следующем посте. Изучать чужие библиотеки было лениво (в том же BOOSTе сам чёрт ногу сломит), писать…

  • atan на ЧЕТЫРЁХ умножениях

    Мишка такой человек — ему обязательно надо, чтоб от всего была польза. Когда у него бывают лишние деньги, он идёт в магазин и покупает какую-нибудь…

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 0 comments