nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

Прокрустово ложе 16 бит

Продолжаю пытаться впихнуть "алгоритм сопровождения" видеоизмерителя параметров сближения в 16-битную целочисленную арифметику. Получается что-то не очень...

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

С этим как будто бы разобрался. Как оказалось, можно повысить масштаб по 4-му параметру (сдвиг по оси X, т.е. изменение дальности) в 4 раза, и по первым 3 аж в 16 раз (а может и больше) - и тогда LDLT (точнее, UDUT)-разложение происходит "на ура", и пока я решил не обращать матрицу, а просто решить систему линейных уравнений. Именно это нужно для "целевой задачи". Обращение матрицы даёт ковариационную матрицу шума измерений, которую от нас не просили, на самом деле. Но пока хрен бы с ней - её можно посчитать "плюс-минус лапоть", ошибка по 10% на каждый компонент - вообще ни о чём.

Как будто бы всё зашевелилось, но проблема пришла, откуда не ждали. Вектор в правой части системы уравнений оказался практически всегда нулевым. Он должен выражать "ненормированные сдвиги", и вот их он вообще не чувствует, как оказалось...


Мы начинаем с величин fny, fnz - координат точки под номером n на фотоприёмной матрице. У нас ось X направлена вдоль оптической оси объектива, поэтому точки на матрице имеют координаты y,z.

Далее мы вычисляем gny, gnz - расчётные координаты точек на фотоприёмной матрице, при значениях параметров, найденных на прошлом шаге (или ещё лучше - чуть экстраполированных к текущему шагу).

И то, и другое можно выразить в пикселях, точнее с ценой младшего разряда в 1/64 пикселя, поскольку разрешение матрицы 1024х1024, и мы как раз уместимся в диапазон -32768..32767. Либо мы можем перейти в радианы, от -0,25 до +0,25 радиан. Это соответствует диапазону -14°..+14°, тогда как поле зрения по горизонтали и вертикали: 24° (-12°..+12°) - то что надо.

Находим разность, "вектор невязки":
hny = fny - gny,
hnz = fnz - gnz.

Мы вполне уверены, что переполнения тут не случится, т.к для этого надо промазать на пол-экрана, но это автоматически приводит к срыву сопровождения и переходу к алгоритму захвата.

И точность мы пока не теряем - цена младшего разряда по-прежнему порядка 1/64 пикселя, меньше, чем ожидаемая ошибка измерения яркостных центров. Пока всё хорошо.

А вот дальше происходит интересная вещь - мы ищем проекцию каждого вектора невязки на 6 различных осей. Две из них тривиальны: Y, Z, они нужны для нахождения параллельного переноса в поперечных направлениях. Другие 4 чуть сложнее. Одна ось - для определения дальности. Для каждой точки эта ось направлена внутрь, "к центру", так что если все точки начинают стягиваться в центр, мы понимаем, что отдаляемся (дальность растёт). Для измерения крена мы направляем ось по касательной, и т.д.

Математически оно выглядит так:


Первые 3 координаты - реакция на повороты "мишени" вдоль осей X,Y,Z (крен, пассивный тангаж и пассивный курс, соответственно), последние 3 - реакция на параллельные переносы по осям X,Y,Z.

Выражения для mn14, mn24 выглядят так:





Их "физический смысл" - на сколько радиан сдвинется точка на фотоприёмной матрице, если аппарат отдалится на 1 метр по оси X (т.е вдоль оптической оси). Bn (bnx, bny, bnz) - координаты в метрах точки "мишени" под номером n, относительно начала координат мишени. t0 - вектор параллельного переноса, измеренный на предыдущем шаге.

Если мы работаем непосредственно в метрах, то всё "совсем плохо" в плане динамического диапазона. В знаменателе стоит квадрат дальности, мы работаем с дальностями от 0,5 метра до 300 метров, т.е один только знаменатель может отличаться в 360 000 раз!

Но мы вроде как сделали "финт ушами", и перешли к работе с "мантиссами", т.е на самом деле t0x всё время лежит в диапазоне 1..2, а тогда t0y, t0z - в диапазоне -0,5..+0,5. Координаты Bn на больших дальностях получаются совсем малы, но потихонечку "догоняют" вектор t0.

После этого "финта" значения mn14, mn24 лежат в диапазоне -0,21..0,21, причём края диапазона достигаются, когда мы наблюдаем свою мишень у самого края поля зрения (т.е t0y≈tan(12°)t0x). Тогда мы можем сделать, чтобы диапазон -0,25..+0,25 соответствовал числам -32768..32767, как обычно, и кажется, что жизнь начинает удаваться. (хотя первая реакция была - даже чуть расширить диапазон, ведь у нас будут складываться проекции по 4..8 точкам, причём по 2 осям, мало ли накопится больше чем надо!).

Не тут-то было. Действительно, если мы наблюдаем нашу мишень у самого края поля зрения, то эти коэффициенты становятся большими, поскольку даже если объект так далеко, что кажется "точечным" (мы не можем различить отдельных его частей), при наблюдении его "сбоку" его движение на нас приведёт к заметному изменению угла. Т.е сначала были координаты X=300, Y=64 и угол

atan(64/300) ≈ 12,043°

затем мы приблизились на 1 метр, стало X=299, Y=64 и угол

atan(64/299) ≈ 12,082°.

Разница целых 2 угловых минуты, или почти 2 пикселя - это мы элементарно заметим! (разумеется, тут возникнет сильнейшая перекрёстная связь с поперечными смещениями, т.е мы зарегистрируем как большую компоненту Kn4, так и Kn5, и как раз для устранения этой связи нам нужна матрица 6×6. Но об этом позже).

А вот если мишень расположена строго по оптической оси, то будь она точечной - мы бы вообще не почувствовали её приближения! Единственное, как можно его отследить - по изменению "углового размера", расстоянию между несколькими точками. Если между ними расстояние 2 метра, то на дистанции 300 метров мы видим их разнесёнными на 16,29 пикселей, а с дистанции 299 метров - разнесёнными на
16,35 пикселей, т.е разница в 0,06 пикселя - её едва заметишь!

В этом и заключается нынешняя проблема. Задав фиксированный масштаб коэффициентов mn14, mn24 так, чтобы они никогда не переполнились, я на самом "ходовом" сценарии захода прямой наводкой (когда мы пытаемся держать мишень "в перекрестии") получаю такие коэффициенты для 4 точек:
172; -10
-197; -98
142; -36
16;  0

при диапазоне -32768..32767.

И чтобы при умножении на такой коэффициент не получить нули, у нас изначально должны выходить смещения как минимум на 2,5 пикселя, а это, простите, приближение метров на 40.

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

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

3. плюнуть и перейти на числа с плавающей точкой. На слабенькую ПЛИС 5576ХС6Т наверняка придётся забить, но тогда заведомо весь этот "узел" разрубится

4. а может, дело не в целых числах как таковых, а просто в их разрядности. Перейти на 32 бита - и всё замечательно зафурычит.

Боюсь, пока все 4 не перепробую - не успокоюсь...


Poll #2096734 Видеоизмеритель параметров сближения

Как всё же заставить его работать?

Ещё чуть-чуть поковыряться с 16 битами - и всё получится
1(10.0%)
По-другому ввести степени свободы в надежде на более стабильные коэффициенты
2(20.0%)
Плюнуть и перейти на плавающую точку
2(20.0%)
Перейти на бОльшую разрядность, например 32 бита
5(50.0%)
Tags: ПЛИС, кватернионы-это просто (том 1), математика, работа, странные девайсы
Subscribe

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 11 comments