CORDIC для вычисления амплитуды и фазы

В сети достаточно информации о применении CORDIC для расчета синуса и косинуса. Но не часто встречается описание того, как получить из "синуса" и "косинуса" амплитуду и фазу. Такое вычисление часто требуется при построении синхронного детектора (LockIn Amplifier).

Суть синхронного детектора заключается в том, что на исследуемый объект подается раскачка с какого-то генератора. Например генератор можно подключить к пьезокерамике, которая далее может механически раскачивать какую-то балку. Как у пьезокерамики, так и у балки есть свои резонансные частоты. Если подать генератором именно эту резонансную частоту, то колебания значительно усилятся. Это можно использовать для нахождения резонансной частоты объектов - т.е. подать меняющуюся частоту и смотреть отклик.

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

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

Синхронный детектор хорошо описан тут - ВОССТАНОВЛЕНИЕ СИГНАЛА МЕТОДОМ DIGITAL LOCK-IN AMPLIFIER.

Сильно упрощая, мы подаем на объект частоту с генератора, и снимаем с объекта отклик на эту частоту. В этом периодическом отклике содержится амплитуда колебаний вызванных генератором и фаза относительно раскачивающего сигнала.

• Если этот периодический отклик умножить на сигнал с генератора, то получается сигнал MagSin.
• Если этот отклик умножить на сигнал с генератора но сдвинутый на четверть периода, то получается сигнал MagCos.

Далее, следуя простым тригонометрическим формулам можно посчитать из сигналов MagSin и MagCos значение амплитуды и фазы. Но работа с тригонометрическими формулами напрямую занимает много ресурсов микроконтроллера/FPGA. Поэтому часто используют CORDIC алгоритм. Этот алгоритм позволяет свести вычисление к серии сложений и сдвигов.

Код вычисления амплитуды и фазы с доступными объяснениями можно найти на данной странице CORDIC FAQ

Данный код был использован мной для проверки работоспособности в python. Код переписан под fixed-point арифметику, т.к. целые числа считаются значительно быстрее, и это ближе к железу.

import math

g_cordic_table = list()
g_cordic_table_len = 0
g_mag_scale = 1

HALF_PI = math.pi / 2

def cordic_init(max_len, fix_scale, fix_offset):
    global g_mag_scale

    g_cordic_table_len = max_len
    K = 1.0
    for i in range(0, max_len):
        phase_rads = math.atan(K)        
        g_cordic_table.append(round(fix_scale * phase_rads) )
        K *= 0.5
    
    g_mag_scale = 1 * fix_scale
    mag, dummy_phase = cordic_calc_mag_phase(fix_scale, 0, fix_scale, fix_offset, False)
    g_mag_scale = round(fix_scale / mag * fix_scale)

def cordic_calc_mag_phase(msin, mcos, fix_scale, fix_offset, show_log):
    # rotate by an initial +/- 90 degrees
    I = mcos
    Q = msin

    if I < 0:
        if Q > 0:
            I = Q  #subtract 90 degrees
            Q = -mcos
            acc_phase_rads = round(-HALF_PI * fix_scale)
        else:
            I = -Q  #add 90 degrees
            Q = mcos
            acc_phase_rads = round(HALF_PI * fix_scale)
    else:
        acc_phase_rads = 0

    # rotate using "1 + jK" factors
    shift = 0
    for phase_rads in g_cordic_table:
        tmp_I = I
        if Q >= 0:
            I += Q >> shift
            Q -= tmp_I >> shift
            acc_phase_rads -= phase_rads
        else:
            I -= Q >> shift
            Q += tmp_I >> shift
            acc_phase_rads += phase_rads
            
        shift += 1

        if show_log:
            print('{0}, {1}, {2} '.format(I, Q, acc_phase_rads))

    mag = (I * g_mag_scale) >> fix_offset
    return (mag, -acc_phase_rads)


def cordic_test_mag_phase(phase_step_rads, mag, prec, fix_scale, fix_offset):
    phase = -math.pi
    angle_cnt = math.floor((2 * math.pi) / phase_step_rads)

    for i in range(0, angle_cnt):
        msin_i = round(math.sin(phase) * mag * fix_scale)
        mcos_i = round(math.cos(phase) * mag * fix_scale)

        out_mag_i, out_phase_i = cordic_calc_mag_phase(msin_i, mcos_i, fix_scale, fix_offset, False)
        out_mag = out_mag_i / fix_scale
        out_phase = out_phase_i / fix_scale

        mag_err = abs(mag - out_mag)
        phase_err = abs(phase - out_phase)

        log = 'Mag: {0:.{6}f}, {1:.{6}f}, {2:.{6}f}, Phase: {3:.{6}f}, {4:.{6}f}, {5:.{6}f}' \
               .format(mag, out_mag, mag_err, \
                    math.degrees(phase), math.degrees(out_phase), math.degrees(phase_err), prec)
        print(log)

        phase += phase_step_rads


# ==============   TEST    ==============
FIX_SCALE = 32767
FIX_OFFS = 15

cordic_init(16, FIX_SCALE, FIX_OFFS)
cordic_test_mag_phase(math.radians(15), 10.0, 5, FIX_SCALE, FIX_SCALE)

Результат получается следующий:

#    inp_amp   out_amp  amp_error        inp_phase   out_phase  phase_error
Mag: 10.00000, 9.99982, 0.00018, Phase: -180.00000, -180.00076, 0.00076
Mag: 10.00000, 9.99979, 0.00021, Phase: -165.00000, -164.99793, 0.00207
Mag: 10.00000, 9.99979, 0.00021, Phase: -150.00000, -149.99859, 0.00141
Mag: 10.00000, 9.99982, 0.00018, Phase: -135.00000, -134.99926, 0.00074
Mag: 10.00000, 9.99979, 0.00021, Phase: -120.00000, -119.99992, 0.00008
Mag: 10.00000, 9.99982, 0.00018, Phase: -105.00000, -105.00059, 0.00059
Mag: 10.00000, 9.99982, 0.00018, Phase:  -90.00000,  -90.00125, 0.00125
Mag: 10.00000, 9.99979, 0.00021, Phase:  -75.00000,  -74.99842, 0.00158
Mag: 10.00000, 9.99979, 0.00021, Phase:  -60.00000,  -59.99909, 0.00091
Mag: 10.00000, 9.99982, 0.00018, Phase:  -45.00000,  -44.99975, 0.00025
Mag: 10.00000, 9.99979, 0.00021, Phase:  -30.00000,  -30.00042, 0.00042
Mag: 10.00000, 9.99982, 0.00018, Phase:  -15.00000,  -15.00108, 0.00108
Mag: 10.00000, 9.99982, 0.00018, Phase:    0.00000,   -0.00175, 0.00175
Mag: 10.00000, 9.99979, 0.00021, Phase:   15.00000,   15.00108, 0.00108
Mag: 10.00000, 9.99979, 0.00021, Phase:   30.00000,   30.00042, 0.00042
Mag: 10.00000, 9.99982, 0.00018, Phase:   45.00000,   44.99975, 0.00025
Mag: 10.00000, 9.99979, 0.00021, Phase:   60.00000,   59.99909, 0.00091
Mag: 10.00000, 9.99982, 0.00018, Phase:   75.00000,   74.99842, 0.00158
Mag: 10.00000, 9.99976, 0.00024, Phase:   90.00000,   90.00125, 0.00125
Mag: 10.00000, 9.99979, 0.00021, Phase:  105.00000,  105.00059, 0.00059
Mag: 10.00000, 9.99979, 0.00021, Phase:  120.00000,  119.99992, 0.00008
Mag: 10.00000, 9.99982, 0.00018, Phase:  135.00000,  134.99926, 0.00074
Mag: 10.00000, 9.99979, 0.00021, Phase:  150.00000,  149.99859, 0.00141
Mag: 10.00000, 9.99982, 0.00018, Phase:  165.00000,  164.99793, 0.00207

Здесь тест при амплитуде 10 проверяет расчет фазы от -180 до 180 с шагом в 15 градусов. Используются 16-ти битные fixed-point значения, где 15-й бит это знак числа, а оставшиеся биты - это дробная часть.

Собственно, суть нахождения амплитуды и фазы заключена в этом коде:

    shift = 0
    for phase_rads in g_cordic_table:
        tmp_I = I
        if Q >= 0:
            I += Q >> shift
            Q -= tmp_I >> shift
            acc_phase_rads -= phase_rads
        else:
            I -= Q >> shift
            Q += tmp_I >> shift
            acc_phase_rads += phase_rads
            
        shift += 1

Мы берем значение сигнала MagCos, которое здесь I, и крутим CORDIC-ом к значению 0 (кладем на ось Х)! Накопленная фаза до сдвига в 0, это и есть начальная фаза сигнала, а амплитуда - это получившееся значение I на оси Х. По коду стоит отметить, что сдвиг на "» shift" это и есть замена деления/умножения на операцию сдвига. Т.е. значения фазы в таблице CORDIC подбираются так, чтобы быть кратными двойке. Следующий шаг "кручения" CORDIC в 2 раза меньше предыдущего, так получается сходимость к правильному результату. Чем длиннее таблица CORDIC, тем выше точность вычисления.

magSin_in и magCos_in на картинке обозначают входные значения сигналов, для которых происходит расчет амплитуды (MAG) и фазы (PHASE).

Для примера будем увеличивать частоту подающего генератора и снимать сигналы magSin и magCos с АЦП. Допустим мы сняли значения этих сигналов для 2000 значений генератора. Если подсунуть массивы этих точек в функцию cordic_test_mag_phase() то мы получим сигнал амплитуды и фазы:

Значения по оси X на картинках это индексы в массивах. Но зная с какой частоты генератора мы начали измерение сигналов и с каким шагом увеличивали частоту можно отразить эту ось Х в значениях частоты. И тогда можно по графику MAG можно оценить на какой частоте происходит резонанс системы. При съеме сигналов фаза специально была подкручена так, чтобы на резонансе значение было равно 0. Отклонение фазы от 0 позволяет отслеживать как уходит резонанс при изменении условий в системе.

В общем случае, частота снимаемого сигнала может оказаться отличной от частоты возбуждения. Балка имеет свой собственный резонанс, и чем ближе частота раскачки к резонансной частоте балки, тем сильнее отклик.

По формулам видно, что синхронный детектор формирует сигналы MagSin и MagCos, которые являются разностными. Т.е. они зависят от разности между частотой раскачки и частотой отклика. Получается, что фильтр низкой частоты пропускает только сигналы с частотой близкой к частоте раскачки. Если сигнал будет отличаться от частоты раскачки больше, чем частота среза фильтра, то фильтр не пропустит такой сигнал.

Таким образом, синхронный детектор работает в полосе частот, ширина которой равна удвоенной частоте фильтра низких частот.