Введение
Всем привет, я работаю программистом DSP контроллеров в фирме, которая занимается разработкой и производством радаров. В начале моего пути, при решении задач по интеграции алгоритмов ЦОС, приходилось по долгу искать пути оптимизации вычислений.
То, что я собираюсь здесь изложить скорее всего будет интересно новичкам. Многие способы давным давно известны и реализованы в библиотеках. Однако, в свое время я потратил много часов для поиска разрозненной информации и понимания всех тонкостей.
Но довольно слов, давайте к делу.
Углы
Операций с углами в ЦОС много. Все начинается с нахождения фазы комплексного числа, далее их нужно складывать, вычитать, усреднять, находить дисперсию и т.д. При этом, нужно постоянно пользоваться функцией которая отсекает лишние периоды, ведь тригонометрический круг описывает только 360°.
Нормировка
Решение, которое решает практически все проблемы угловой математики и избавляет от кучи ненужных проверок, заключается в том, чтобы внутри алгоритма углы хранились и обрабатывались в нормированном виде. Эта нормировка должна соответствовать диапазону какого-нибудь целочисленного типа. Обычно я использую двухбайтный тип данных, такой точности вполне хватает. Однако, для наглядности в примере буду использовать однобайтный uint8.
Итак, угол хранится в типе uint8, где 0 это 0°, а 255 это почти 360°. Таким образом, 90° это 64, 180° это 128, а 270° это 192.
В чем же профит?
После сложения или вычитания больше не нужно отбрасывать лишние периоды и дергать функцию wrapTo360 (привет пользователям MatLab). При переполнении все отбросится само.
Например, если мы к 270° прибавим 180°, то во флотовой арифметике получим 450°, после нужно проверить больше ли результат 360° и если да, то вычесть лишние 360°. В результате получится 90°.
Теперь посчитаем тоже самое с нашей нормировкой: сложим 192 и 128, из-за переполнения uint8 сразу получим 64, что как раз-таки и соответствует 90°.
Благодаря свойствам дополнительного кода, приятным бонусом будет быстрый перевод в человеческие градусы или радианы в конце работы ЦОС алгоритма, при этом, изменение нотации на (-180°..+180°) никак не повлияет на производительность, просто одна ассемблерная инструкция поменяется на другую:
#define TO_ANGLE_COEF (360.0/256)
uint8 angle = 192; //270°
//0..360
float angle_deg_360 = angle * TO_ANGLE_COEF; //270
//-180..+180
float angle_deg_180 = (sint8) angle * TO_ANGLE_COEF; //-90
Очевидно, что единственным минусом данного подхода будет ошибка, которая зависит от выбранной нормировки. Например, в случае нормирования на 256 значений uint8 дискретность будет 360/256 = 1,4 градуса. Но если выбрать двухбайтный тип, то дискретность станет всего 360/65536 = 0.00549 градуса.
Усреднение
Усреднение углов в ЦОС используется для снижения влияния шумов.
Допустим, мы хотим усреднить 90° и 100°. (90 + 100)/2 = 95. То есть, 95° это середина дуги, между 90° и 100°.
Особенность усреднения углов заключается в том, что когда они "дребезжат" вокруг точки разрыва, классическая формула дает результат, в прямом смысле, противоположный. Например, результат усреднения двух близких углов 3° и 355°: (3°+355°)/2 = 179°. Но по смыслу понятно, что результат должен быть 359. То есть среднее двух углов это не просто середина дуги, а середина кратчайшей дуги.
Обычно такая проблема решается "костылем" с кучей проверок, добавлением или вычитанием лишних периодов и тд. Но в случае с предложенной нормировкой решение получается элегантным, на мой взгляд:
#define P_A(s1, s2) (uint8)((s1) - (sint8)((s1) - (s2))/2) //average two phases
Выглядит не понятно, но присмотритесь: s1 - s2 это длина дуги. Мы не знаем какая это дуга - длинная или короткая, но нам нужна именно короткая. Какая дуга получится - зависит от направления вычитания.
Длинная дуга, это дуга больше 180°, то есть больше половины длины тригонометрической окружности. Когда такая длина дуги преобразуется в знаковый тип, то значение >= 180° (напоминаю, в предложенной нормировке >= 128) станет отрицательным.
Например, s1 = 271°, s2 = 1°, разница будет 270°, что больше 180°. При преобразовании к знаковому типу она превратится в -90°, поделится пополам -45° и отнимется от s1. Так как вычитаемое отрицательное, то в результате получится сложение и результат будет 271°+45° = 316°, что корректно, в отличии от результата классической формулы.
Если же в результате разница будет меньше 180°, то эта формула также корректно сработает. Поменяем s1 и s2 местами, теперь s1 = 1°, s2 = 271°, s1 - s2 получится 90° (у нас же автоматический wrap, еще не забыли?). Преобразование к знаковому типу ничего не поменяет, и теперь 90°/2 отнимется от 1°, с автоматическим wrap'ом получим те же 316°.
Минусом данного способа расчета среднего является то, что усреднять получится только выборки длинною степень двойки.
Дисперсия
Как известно, дисперсия служит для оценки разброса величины, в нашем случае угла. Есть алгоритмы, где отбор точек отражения из спектра происходит не по амплитуде, а именно по дисперсии фазовых разностей.
Для вычисления дисперсии, в первую очередь, необходимо среднее значение, его рассмотрели на предыдущем шаге.
В формуле дисперсии для углов, основная особенность такая же как и в вычислении среднего. Нужно находить разницу между средним и текущим, то есть найти длину дуги, а точнее кратчайшей дуги. Затем возводить в квадрат, накапливать сумму, поделить на n (или n-1, смотря что вам нужно) и затем извлечь корень. Как я уже писал в разделе про усреднение, кратчайшую дугу найти просто:
(uint8)abs((sint8)(ave - sn))
Здесь нужно быть осторожным со взятием модуля, потому что если аргумент будет равен INT_MIN, то модуль окажется отрицательным. Поэтому следует перед вычислением модуля проверять аргумент либо использовать модуль с насыщением.
Но, скорее всего, следом нужно будет возводить значение в квадрат, поэтому можно убрать модуль и сэкономить еще немного ресурсов:
(sint8)(ave - sn)
Такая конструкция не будет ошибаться в точках разрыва. Так что пользуйтесь.
ДОПОЛНЕНО 14.07.2023:
Вычисление среднего угла из выборки произвольной длины
Выше описан способ подсчета среднего угла для двух углов. Таким образом, с помощью этого способа получится рекурсивно усреднять только выборки длинною степень двойки. Но что если нам нужно усреднить выборку с отличным от этого значения количеством углов?
Давайте еще раз посмотрим на выражение, усредняющее пару углов:
#define P_A(s1, s2) (uint8)((s1) - (sint8)((s1) - (s2))/2)Эта формула является ни чем иным, как частным случаем (для двух элементов) простого скользящего среднего. Нужно сделать ее универсальной - обернуть в цикл и добавить некоторые переменные. В результате получится такой листинг:
#define FROM_ANGLE(x) (uint8)((x)/TO_ANGLE_COEF) //from 0..360 angles
uint8 angles[] = {FROM_ANGLE(0), FROM_ANGLE(160), FROM_ANGLE(320)};
int nagles = sizeof(angles)/sizeof(uint8);
uint8 ave = angles[0];
for(int i = 1; i < nagles; i ++)
ave -= ((sint8)(ave - angles[i]))/ (i + 1);
printf("%f", TO_ANGLE_360(ave));Среднее инициализировано первым значением выборки, что логично. После этого цикл смещает это среднее к очередному элементу на дельту, при этом, с каждым следующим элементом влияние дельты становится меньше. Это происходит за счет деления этой дельты на количество уже принявших участие в расчёте среднего элементов.
Добавим к этому фокусы для вычисления кратчайшей дуги, и вот, идеальный алгоритм без единого ветвления.
Эти приемы всегда работают правильно, включая случаи, когда дребезг угла вокруг точки разрыва не очевидный: тут безошибочно определяется необходимая дуга и для выборок с большим разбросом получается корректное значение. Попробуйте, например, усреднить 0, 160, 320 градусов - в классической формуле получится 160. Но если углы визуализировать на тригонометрическом круге, то станет понятно, что "центр масс" такой выборки совсем не 160, потому что 0 и 320 достаточно близкие углы, чтобы перетянуть среднее к себе. С этим алгоритмом для такой выборки ожидаемо получим 40 градусов.
Заключение
Дополнительный код, с которым работает любой процессор, это просто находка для подобных круговых величин. Многие академические формулы, которые предназначены для числовой прямой изобилуют костылями когда нужно работать с такими величинами. Но стоит немного подумать и все значительно упрощается.
Данные приемы значительно ускоряют работу встраиваемых систем, однако могут оказаться полезными и на ПК в приложениях где считается большое количество углов. Поднятие точности до 4 байтного типа удовлетворит любой задаче.
Всем спасибо за внимание.
