nabbla (nabbla1) wrote,
nabbla
nabbla1

Categories:

atan(y/x) на QuatCore

До сих пор у меня алгоритм для видеоизмерителя параметров сближения обходился без тригонометрии: все "кватернионные" вычисления удалось свести к сложениям и умножениям, включая и нормировку.

Но всё же, чтобы тупо выполнить условия ТЗ и выдать УГЛЫ, которые у нас просят (курс, тангаж, крен), нужно под самый конец преобразовать кватернион в эти углы.

Я так прикинул, для этого нужна ровно одна обратная тригонометрическая функция, которую условно назвал atan(y/x), не путать с atan2(y,x)!

Последняя выдаёт угол между осью OX и направлением на точку (x,y), с учётом квадранта. Как правило, ответ лежит в диапазоне от -pi до +pi, т.е "полный оборот". Но у нас сплошь используются половинные углы в кватернионах, поэтому результат надо ещё умножить на 2, и как раз такая "разборчивость к квадранту" оказывается излишней! Как раз-таки самый простой atan(y/x), выдающий значения от -pi/2 до pi/2, оказывается предпочтительнее, только вот "в явном виде" выполнять деление - наверное не лучшее решение. Когда мы приближаемся к плюс-минус pi/2, x стремится к нулю, аргумент арктангенса стремится к бесконечности и "достигает" её на краях.

Уж лучше этой функции дать два аргумента: x,y, т.е реализовать некий atan(y,x), в котором на самом деле мы пойдём "окольным путём".

Я хочу реализацию в целых 16-битных числах. Каким считать масштаб y,x - не так уж важно, главное, он один и тот же. А результат будет иметь формат Q3.13, т.е 3 бита перед запятой, 13 после запятой, со знаком, т.е диапазон от -4 до 3,9999 радиан. Цена младшего разряда около 25 угловых секунд, значит, точность порядка 12 угловых секунд, что соответствует точности задания угла кватернионом с 16-битными компонентами.

Хочу точность как минимум 0,1°, а лучше точнее. При этом максимально компактный код, т.к память "поджимает". А вот время выполнения меня не сильно заботит: нам отводится 200 мс на обработку измерений, а пока мы укладываемся в 200..300 мкс.


Сужение диапазона входных данных ("формулы приведения")

Обычно именно с этого начинается выполнение. Скажем, если x<0, мы сразу заменяем x на -x, а y на -y, и теперь из 4 квадрантов осталось только два: первый и четвёртый, т.е справа от оси Y.

Далее при желании можно уползти в первый квадрант, проверив знак y, и если он отрицательный - убрать его и "запомнить", что знак ответа также надо поменять, т.е atan(-y,x) = -atan(y,x)

Далее, в первом квадранте можно ограничиться углами от 0 до pi/4 (т.е 45°), в противном случае поменяв местами x и y и "запомнив", что нужно результат "отразить", т.е atan(y,x) = pi/2 - atan(x,y).

Вот это последнее соотношение уже здорово помогает: теперь если действительно "честно" делить y на x, ответ всегда будет от 0 до 1.

Арктангенс от 0 до 1

Далее обычно заменяют atan(x) на некоторый многочлен, чтобы посчитать значение с нужной точностью в диапазоне аргумента от 0 до 1. Ряд Тейлора здесь категорически не подходит, ведь именно atan(1) даёт красивое выражение для π:



Оно красивое, но ОЧЕНЬ ПЛОХО СХОДЯЩЕЕСЯ! Нужно порядка 2000 (!!!) слагаемых, чтобы выйти на точность в наши 25 угловых секунд. А если ограничиться приближением atan(x)≈x, то при x=1 мы получим ошибку в 12°, что-то многовато!

Вот на сайте nvidia предлагают многочлен 11-й степени: https://developer.download.nvidia.com/cg/atan2.html
по сути, они предлагают такую формулу:


(многочлен здесь уже выписан по "схеме Горнера", для ускорения его вычисления)

Ошибка составляет не более 0,7 угловой секунды, неплохо! Получается, мне и не нужно такой здоровенный здесь использовать.

А может лучше арксинус?

На самом деле, в нашей задаче у меня имеется дополнительная информация по поводу значений x,y: они худо-бедно нормализованы! Кроме того, у меня уже есть процедура для нормировки вектора (x,y), так что это, по сути, "бесплатно".

А если так, то можно обойтись без деления y/x (у нас в процессоре и деления нет, поэтому и здесь небольшая проблема. Правда, метод Ньютона мы уже применили, довольно эффективно, но всё равно хотелось бы обойтись), используя значение y "напрямую".

Потому как arcsin(x) в диапазоне выходных значений от 0 до 45° имеет получше сходимость. Тут даже приближение arcsin(x)≈x почти что "прокатывает": при угле в 45° имеем y=0,707107. Возьмём это как угол в радианах - и получим 40,5° вместо требуемых 45°, ошибка 10% фактически.

Можно даже улучшить этот результат "малой кровью", если положить arcsin(x)≈1,0811x. Тогда максимальная ошибка в диапазоне от 0 до 45° составит 1,2°. Для приближения "первого порядка" очень даже неплохо!

Ещё на 22,5° довернуть?
На самом деле и приведённый по ссылке метод с арктангенсом, и рассмотренный нами арксинус работали в диапазоне от -45° до 45° (от -pi/4 до pi/4), т.е из трёх формул приведения (сократить число квадрантов до 2, убрать нижний квадрант, убрать промежутки 45..90 и -45..-90) там можно было использовать две. Дальше со знаком "-" мы уже вполне справлялись - многочлен вводился строго нечётный, поэтому нижний квадрант обрабатывался столь же хорошо.

Можно попробовать ещё одну "подлянку" сделать - всё-таки сначала перейти в один единственный первый квадрант (оба аргумента, x и y, положительные), а потом "повернуть вектор" на 22,5°. Такой поворот в QuatCore осуществляется очень легко, благодаря функции FMPM (Fused Multiply Plus-Minus) и адресации i^k.

Тогда мы будем иметь дело с углами -22,5°..+22,5°, или -pi/8..pi/8. Теперь если использовать простейшее приближение asin(x)≈x, максимальная ошибка будет на краю диапазона, когда x≈0,3827, и составит она 0,57°.

Лучший результат из линейных даст приближение asin(x)≈33408/32768*x≈1,0195x (из тех констант, что представимы в 16 битах), получится максимальная ошибка в 0,146°.

Но увы, и здесь ошибка выходит больше, чем хотелось бы.

И ещё на 11,25° ?
В принципе, эту эпопею можно продолжить и дальше. Можно даже чуть по-другому сформулировать: если на прошлом этапе получили y>0, то теперь повернём вектор (x,y) на 11,25° ПО ЧАСОВОЙ СТРЕЛКЕ, в противном случае ПРОТИВ. В обоих случаях выйдем на диапазон -11,25°..+11,25°. Теперь простейшее приближение asin(x)≈x даст максимальную ошибку 0,072° как раз на краю диапазона. Если же ещё добавить множитель, asin(x)≈32927/32768*x≈1,0048x, максимальная ошибка составит 0,018°, или 66 угловых секунд. Правда, если мы подставляли в арктангенс компоненты кватерниона, то это мы нашли половинный угол - его ещё надо будет умножить на 2, поэтому ошибка удвоится и составит уже 0,036°. Наверное, меня это вполне устроит. Или можно провернуть тот же трюк ещё один раз, до 5,625°, где простейшее приближение asin(x)≈x даст максимальную ошибку 0,009°, а если добавить множитель, asin(x)≈32808/32768*x≈1,0012x, максимальная ошибка составит 0,0023°, или 8 угловых секунд. При умножении на 2 будет 16 угловых секунд - вот этого точно хватит! (дальше просто бессмысленно - у нас разрядности не хватит точнее выразить!)

CORDIC?
Такой подход, где вектор в зависимости от знака одной своей координаты претерпевает поворот на один и тот же угол, либо в одну, либо в другую сторону, очень напоминает офигительный метод CORDIC, или COordinate Rotation DIgital Computer, за одним исключением. В CORDIC вектор по сути умножался на матрицу


Где начинали с n=0, затем шло n=1, и так далее, пока не придём к необходимой точности. Такие матрицы хороши тем, что не требуют умножения как такового, достаточно сложений и сдвигов! Эти матрицы соответствуют вполне определённым углам: 45°, 26,57°, 14,04°, 7,125° и так далее, в общем, atan(2-n). Эти углы нужно сохранить в таблице и при каждом повороте либо прибавлять очередной угол, либо вычитать его из результата. Это немного не те углы, которые мы хотели (45°, 22,5°, 11,25° и т.д), но отличие не очень сильное. Да, возможные интервалы будут получаться чуточку больше, но в целом жить можно.

Казалось бы, "то что доктор прописал". Но увы, в QuatCore сейчас нет сдвигателя на произвольное число разрядов, так называемого Barrel Shifter. Это довольно-таки громоздкая хреновина. В случае 16 бит она будет состоять из 4 последовательно идущих мультиплексоров. Первый "выбирает", делать ли сдвиг на 1, второй - делать ли сдвиг на 2, третий - на 4, четвёртый - на 8. В итоге доступны сдвиги от 0 до 15 бит. Меня тут больше пугает даже не количество ЛЭ, которые на неё нужно отдать, а задержка распространения - на моей медленной ПЛИС могу в 25 МГц не уложиться. Конечно, можно посерединке ещё защёлку поставить, будет сдвиг осуществляться за 2 такта, но опять же, всю логику АЛУ перекраивать, или вообще ВТОРОЕ АЛУ поставить, не хочется пока...

И ещё есть подлянка: длина вектора при использовании таких матриц непрерывно растёт, особенно в начале. При повороте на 45° мы удлиняем его в 1,414 раза (корень из 2), затем ещё в 1,12 раза, затем 1,03 раза, 1,0078 раза, и так далее. Финальное удлинение всегда одно и то же, нужно просто его учитывать, и, к примеру, внести в тот коэффициент, с которым мы считаем арксинус.

Главное только не выйти за пределы представимых чисел "по ходу дела". Скорее всего, придётся в самом начале поделить вектор на 2, и на этом 1 бит точности сразу потеряем.

Унификация наше всё!
А вот использование произвольных "комплексных чисел", выражающих поворот и масштабирование, позволяет все этапы алгоритма загнать в один-единственный цикл!

Начинаем мы со значений x и y, причём x2+y2 = 1. (если это не так, можно вызвать процедуру нормировки, она у нас уже есть). Эти значения можно ещё представить как действительную и мнимую часть комплексного числа. Пусть это будет V = x + iy.

Результат A (angle) пока что объявляем нулевым.

Проверяем знак x. Если знак "+", умножаем наше комплексное число V на i (поворачиваем на 90 градусов против часовой стрелки) и прибавляем к результату 0, в противном случае умножаем на -i и вычитаем из результата 0. (если на этом этапе прибавлять/вычитать 90 градусов вместо нуля, то вроде бы можно получить atan2(y,x) вместо нашего atan(y/x), но ещё инициализировать A надо не нулём, а то ли 90, то ли -90 градусами, не думал об этом слишком сильно, мне atan2 нафиг не сдался :))

Сейчас мы получаем заведомо y > 0 (т.к x перешёл в y в результате поворота на 90°, и мы проследили, чтобы поворот прошёл в нужную сторону), а x в диапазоне от -1 до +1. В него перешёл y.

Переходим к следующей итерации. Проверяем знак x. Если знак "+", умножаем комплексное число на exp(i*pi/4), т.е крутим на 45° против часовой стрелки, и прибавляем к результату pi/4. В противном случае умножаем на exp(-i*pi/4) и вычитаем pi/4.

Потом всё то же самое, только pi/8 (22,5°), затем pi/16 (11,25°) и, при желании, pi/32 (5,625°).

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

Дурацкий пример, x=1, y=0, т.е V = 1 + 0i
На первой итерации умножаем на i, получая V = 0 + 1i.
Далее, у "нуля" знак "+", поэтому умножаем на exp(i*pi/4), получая V = -0,707106781186547 + 0,707106781186548i, A = pi/4.
Далее, знак "-", поэтому умножаем на exp(-i*pi/8), получая V = -0,38268343236509 + 0,923879532511287i, A = pi/4 - pi/8 = pi/8.
Далее, знак "-", поэтому умножаем на exp(-i*pi/16), получая V=-0,195090322016128 + 0,98078528040323i, A = pi/8 - pi/16 = pi/16.
И давайте ещё одну итерацию. Знак "-", поэтому умножаем на exp(-i*pi/32), получая V=-0,0980171403295605 + 0,995184726672197i, A = pi/16 - pi/32 = pi/32.

Итерации завершены, теперь умножаем действительную часть на 1,0012, получаем -0,098134760897956. Прибавляем это значение к A и получаем ответ: 4,00095267250461E-5 радиан, или 8 угловых секунд.

Таким вот хитрым путём мы нашли atan(0), и даже почти не ошиблись :)))

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

Можно посчитать навскидку, сколько времени занимают эти вычисления. Имеем 5 итераций, на каждой мы умножаем 2 комплексных числа, это 4 команды FMA / FMPM, именно они львиную долю отжирают. Навскидку, каждая по 18 тактов, итого 72 такта на итерацию, 360 тактов всего. При длительности одного такта 40 нс (тактовая частота 25 МГц), выходит 14,4 мкс. И таких арктангенсов мне нужно посчитать максимум 5 штук (активные тангаж-курс, пассивные тангаж-курс и взаимный крен) каждые 200 мс. Ну да, аж 72 мкс на это потрачу, ужас-то какой :)

А по-другому никак?
Вон, для деления, нормировки, а также для возведения в степень (-1/2) (тот самый InvSqrt, прославленный в Quake3) мы успешно использовали метод Ньютона, который обеспечивает квадратичную сходимость. А тут застряли с долгим и нудным делением отрезка на 2, ещё и с кучей констант на каждый шаг. Неужели и здесь что-нибудь такое итерационное нельзя?

Что-то мне пока кажется - НЕЛЬЗЯ. Как бы когда речь шла о делении, о корне, о нормировке, мы легко и непринуждённо могли "проверить себя". Если действительно обратную величину посчитали - умножим её на исходную, должны получить 1. Получили что-то другое - имеем НЕВЯЗКУ, которая быстренько ведёт нас в нужном направлении. Та же история с квадратным корнем или с нормой вектора/кватерниона.

Но здесь мы не можем проверить, насколько точно мы посчитали АРКТАНГЕНС, т.к это требует считать ТАНГЕНС, а он, зараза, тоже так просто не считается! Поэтому методов сильно лучше представленного я сходу не вижу.

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

Преобразование из декартовых координат в полярные
Можно всё-таки избавиться от требования x2+y2 = 1, тут и без него мы вполне успешно "загоним" вектор в узенькую "щель" от -5,625° до + 5,625°, с сохранением его длины. А если так, можно ещё и его длину очень точно выдать. "Нулевое приближение" - просто взять значение y, ошибёмся не более чем на 0,48%. Скажем, для нашего нахождения масштаба можно не заморачиваться с метриками, а вызвать эту же самую процедуру, если уж она тут будет. Да, она вычисляется помедленнее, но времени у меня вагон :) Но может, 20..25 строк кода сэкономим, это ж целых 50 байт!

В общем, такой вариант тоже надо иметь в виду.


Похоже, нащупали неплохой способ, осталось его реализовать на ассемблере.

PS. Хотя забавно, как этот способ допускает максимальную ошибку при вычислении atan(0). У него получается не ноль, а 8 угловых секунд, которые, надеюсь, округлятся-таки до нуля при выдаче 16-битного результата. Хотя казалось бы, уж в нуле ошибиться никак нельзя, тут практически любой сходу даст правильный ответ!
Tags: ПЛИС, математика, программки, работа, странные девайсы
Subscribe

  • Тестируем atan1 на QuatCore

    Пора уже перебираться на "железо" потихоньку. Решил начать с самого первого алгоритма, поскольку он уже был написан на ассемблере. В программу внёс…

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

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

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

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

  • Post a new comment

    Error

    Anonymous comments are disabled in this journal

    default userpic

    Your IP address will be recorded 

  • 2 comments