81а. Цифровые фильтры: КИХ, БИХ и CMSIS-DSP

В разделе 81 шум с датчика гасили скользящим средним и экспоненциальным сглаживанием — фильтрами, которые проще всего написать. Когда нужно точно: пропустить до 100 Гц и подавить всё выше 200, — берут настоящие цифровые фильтры. Их два вида: КИХ (с конечной импульсной характеристикой) и БИХ (с бесконечной). На STM32 их не пишут вручную: в библиотеке CMSIS-DSP есть готовые функции, оптимизированные под ядро Cortex-M, для чисел с плавающей точкой и для дробей с фиксированной точкой.

Все числа раздела — прогон: функции CMSIS-DSP 1.16.2 из пакета собраны gcc на компьютере (_verify/dsp81a/dsp81a.c), через каждый фильтр пропущены синусы четырёх частот, усиление измерено по установившемуся сигналу. Отдельно, без библиотеки, АЧХ посчитана по коэффициентам в Python — оба расчёта совпали до пятого знака. Сверяет _verify/dsp81a_check.py.

Как это называется в документации

Оригинал	Перевод
finite impulse response (FIR) filter	КИХ-фильтр: выход — взвешенная сумма входов
infinite impulse response (IIR) filter	БИХ-фильтр: в сумме участвуют и прошлые выходы
biquad, second-order section	биквад: звено второго порядка
taps	коэффициенты (отводы) КИХ-фильтра
group delay	групповая задержка
feedforward, feedback coefficients	коэффициенты прямой и обратной связи
postShift	сдвиг результата после накопления в Q15/Q31
cutoff frequency	частота среза

81а.1 Два вида фильтров

Скользящее среднее из раздела 81 — это КИХ со всеми коэффициентами 1/N, экспоненциальное сглаживание — БИХ первого порядка.

81а.2 КИХ: расчёт и проверка

Частота отсчётов 1000 Гц, срез 100 Гц, 31 коэффициент. Метод окна: идеальный фильтр нижних частот (функция sinc) умножается на окно Хэмминга, коэффициенты нормируются к сумме 1:

for (int n = 0; n < NTAPS; n++) {
    double m = n - (NTAPS - 1) / 2.0;
    double h = (m == 0) ? 2 * fc / FS : sin(2 * M_PI * fc / FS * m) / (M_PI * m);
    h *= 0.54 - 0.46 * cos(2 * M_PI * n / (NTAPS - 1));
    fir_c[n] = (float32_t)h;
    sum += h;
}

Коэффициенты симметричны — середина 0,19966, края нулевые. Прогон через arm_fir_f32 и arm_fir_q15:

• На частоте среза фильтр, построенный методом окна, ослабляет вдвое (−6 дБ), а не на −3 дБ, как аналоговые фильтры — частоту среза так и определяют.
• −70 дБ на 200 Гц — это попадание рядом с нулём АЧХ. Честнее смотреть на боковые лепестки: первый — −51,4 дБ на 166,7 Гц, а выше 200 Гц ни один не поднимается выше −62,6 дБ — подавление больше чем в 1000 раз.
• Q15 почти не уступает float: коэффициенты в 16 битах, а накапливает функция в 64-битном аккумуляторе.

Задержка. Импульс на входе даёт пик на выходе через 15 отсчётов = 15 мс — ровно (N − 1) / 2. У симметричного КИХ задержка одинакова для всех частот: форма сигнала сохраняется, он просто сдвигается. Для регулятора (раздел 47) 15 мс запаздывания в контуре — много; там фильтр короче или БИХ.

81а.3 БИХ: одно звено вместо 31 коэффициента

Звено второго порядка (биквад) с характеристикой Баттерворта на 100 Гц, коэффициенты по билинейному преобразованию:

Прогон через arm_biquad_cascade_df1_f32:

Пять умножений вместо 31 — но и спад мягче: −14 дБ на 200 Гц против −70 у КИХ. Круче — несколько звеньев подряд: функция так и называется — cascade, число звеньев задаётся при инициализации.

81а.4 Ловушка знака

Вот главная грабля CMSIS-DSP. Библиотека считает биквад так:

y[n] = b0x[n] + b1x[n−1] + b2x[n−2] + a1y[n−1] + a2y[n−2]

— с плюсом перед обратной связью. Большинство программ расчёта фильтров (и формулы выше) пишут с минусом. Значит, в массив коэффициентов CMSIS кладут {b0, b1, b2, −a1, −a2}:

float32_t cm_ok[5] = {(float32_t)c[0], (float32_t)c[1], (float32_t)c[2], (float32_t)-c[3], (float32_t)-c[4]};

Что будет, если положить коэффициенты как есть: прогон на всех четырёх частотах дал бесконечность. Знаменатель фильтра сменил знак, и один из полюсов оказался в точке −1,43 — вне единичного круга; фильтр неустойчив, выход растёт без предела. С правильным знаком полюсы комплексные, радиус 0,642 — внутри круга, фильтр устойчив.

Проверка на устойчивость для одного звена простая: |a2| < 1 и |a1| < 1 + a2 (в записи с минусом).

81а.5 БИХ в Q15: коэффициент больше единицы

Q15 — дробь от −1 до почти +1. А −a1 = 1,143 — больше единицы, в Q15 не помещается. Выход — поделить все коэффициенты на 2, а результат накопления сдвинуть на 1 бит влево: это параметр postShift = 1 при инициализации.

Порядок коэффициентов в Q15 тоже свой: {b0, 0, b1, b2, a1, a2} — между b0 и b1 стоит нулевое слово: так на Cortex-M4 коэффициенты берутся парами 16-битными SIMD-командами.

1105, 0, 2210, 1105, 18727, -6763      postShift = 1

Результат — почти как у float:

На ядре без плавающей точки (Cortex-M0, M3) Q15 — единственный быстрый путь; на M4 и M7 с FPU float обычно не медленнее и не требует этих хлопот.

81а.6 Как это выглядит в прошивке

1. Коэффициенты считают заранее — на компьютере или один раз при старте — и хранят в const-массиве.
2. Состояние фильтра — массив: у КИХ длиной NTAPS + BLOCK − 1, у биквада 4 числа на звено. Живёт всё время работы фильтра.
3. Отсчёты обрабатывают блоками: DMA складывает половину буфера АЦП (раздел 14), прерывание «половина готова» переводит коды этой половины в формат фильтра и вызывает arm_fir_f32, пока DMA заполняет вторую. Коды АЦП беззнаковые 12-битные: для float их пересчитывают как (code − 2048) / 2048.0f, для Q15 — как (int16_t)((code − 2048) * 16). Передать буфер АЦП в фильтр напрямую нельзя.
4. Размер блока задаётся при инициализации и должен совпадать с тем, что передают при каждом вызове.

81а.7 Грабли

БИХ «взорвался» — выход в бесконечность. Коэффициенты обратной связи положены без смены знака. В CMSIS — {b0, b1, b2, −a1, −a2}.

Биквад Q15 даёт мусор. Не учтён нулевой элемент {b0, 0, b1, b2, a1, a2} или коэффициент больше 1 без деления на 2 и postShift.

Частота среза «не та». У КИХ методом окна на частоте среза −6 дБ, у Баттерворта −3 дБ. Сравнивая фильтры, говорят, по какому уровню.

Фильтр запаздывает. КИХ на N коэффициентов задерживает на (N − 1)/2 отсчётов. В контуре регулирования это может раскачать систему.

Первые отсчёты странные. Состояние фильтра вначале нулевое — выход выходит на режим за время, равное длине КИХ (или несколько постоянных времени у БИХ).

Буфер состояния мал. У КИХ он NTAPS + BLOCK − 1, а не NTAPS: функция портит соседнюю память.

81а.8 Итоги

• КИХ: всегда устойчив, линейная фаза, задержка (N − 1)/2 — у фильтра на 31 коэффициент при 1 кГц это 15 мс; −6 дБ на частоте среза, выше 200 Гц подавление не меньше 62,6 дБ.
• БИХ-биквад: 5 умножений, −3,01 дБ на 100 Гц, спад мягче; для крутизны — каскад звеньев.
• В CMSIS обратная связь со знаком «+»: коэффициенты −a1, −a2. Иначе полюс −1,43 и бесконечность на выходе.
• Q15: коэффициенты больше 1 — делить на 2 и postShift = 1; нулевое слово после b0. Точность — до четвёртого знака как у float.

Полные исходники

dsp81a.c — КИХ и БИХ на CMSIS-DSP: float и Q15.

/* Раздел 81а: КИХ- и БИХ-фильтры на CMSIS-DSP 1.16.2, собранной gcc на компьютере.
 *
 * Частота отсчётов 1000 Гц, полоса пропускания до 100 Гц:
 *  1. КИХ 31 коэффициент, окно Хэмминга — arm_fir_f32 и arm_fir_q15;
 *  2. БИХ второго порядка (Баттерворт, 100 Гц) — arm_biquad_cascade_df1_f32 и _q15;
 *  3. ловушка знака: коэффициенты обратной связи в CMSIS со знаком «+».
 * Для каждого фильтра — усиление на 50, 100, 200 и 300 Гц по установившемуся синусу. */
#include <math.h>
#include <stdio.h>
#include <string.h>

#include "arm_math.h"

#define FS      1000.0
#define NTAPS   31
#define BLOCK   32
#define NSAMP   4096          /* 4096 отсчётов: 4,1 с — хватает на установку и замер */

static float32_t fir_c[NTAPS];

/* КИХ методом окна: идеальный ФНЧ (sinc) × окно Хэмминга, сумма коэффициентов = 1. */
static void design_fir(double fc)
{
    double sum = 0;
    for (int n = 0; n < NTAPS; n++) {
        double m = n - (NTAPS - 1) / 2.0;
        double h = (m == 0) ? 2 * fc / FS : sin(2 * M_PI * fc / FS * m) / (M_PI * m);
        h *= 0.54 - 0.46 * cos(2 * M_PI * n / (NTAPS - 1));
        fir_c[n] = (float32_t)h;
        sum += h;
    }
    for (int n = 0; n < NTAPS; n++)
        fir_c[n] = (float32_t)(fir_c[n] / sum);
}

/* Биквад ФНЧ (Баттерворт, Q = 1/√2) по билинейному преобразованию. Возвращает b0 b1 b2 a1 a2
 * в привычной записи y = b·x − a1·y[n−1] − a2·y[n−2]. */
static void design_biquad(double fc, double c[5])
{
    double w0 = 2 * M_PI * fc / FS, al = sin(w0) / (2 * M_SQRT1_2), cw = cos(w0);
    double a0 = 1 + al;
    c[0] = (1 - cw) / 2 / a0;
    c[1] = (1 - cw) / a0;
    c[2] = (1 - cw) / 2 / a0;
    c[3] = -2 * cw / a0;
    c[4] = (1 - al) / a0;
}

static float32_t xin[NSAMP], yout[NSAMP];
static q15_t xq[NSAMP], yq[NSAMP];

static void make_tone(double f, double amp)
{
    for (int i = 0; i < NSAMP; i++) {
        xin[i] = (float32_t)(amp * sin(2 * M_PI * f * i / FS));
        xq[i] = (q15_t)lrint(xin[i] * 32767);
    }
}

/* Амплитуда на последних 1000 отсчётах (ровно 50…300 целых периодов для наших частот). */
static double amp_f(const float32_t *y)
{
    double s = 0;
    for (int i = NSAMP - 1000; i < NSAMP; i++)
        s += (double)y[i] * y[i];
    return sqrt(2 * s / 1000);
}

static double amp_q(const q15_t *y)
{
    double s = 0;
    for (int i = NSAMP - 1000; i < NSAMP; i++)
        s += ((double)y[i] / 32768) * ((double)y[i] / 32768);
    return sqrt(2 * s / 1000);
}

static const double FREQ[] = {50, 100, 200, 300};

int main(void)
{
    design_fir(100);
    printf("fir_taps=%d", NTAPS);
    for (int n = 0; n < NTAPS; n++)
        printf("%s%.9f", n ? "," : "=", fir_c[n]);
    printf("\n");

    /* ── КИХ, float ── */
    static float32_t st_f[NTAPS + BLOCK - 1];
    static q15_t fir_q[NTAPS], st_q[NTAPS + BLOCK - 1];
    arm_float_to_q15(fir_c, fir_q, NTAPS);
    for (unsigned k = 0; k < 4; k++) {
        arm_fir_instance_f32 S;
        arm_fir_init_f32(&S, NTAPS, fir_c, st_f, BLOCK);
        make_tone(FREQ[k], 0.5);
        for (int i = 0; i < NSAMP; i += BLOCK)
            arm_fir_f32(&S, xin + i, yout + i, BLOCK);
        double gf = amp_f(yout) / 0.5;
        arm_fir_instance_q15 Q;
        arm_fir_init_q15(&Q, NTAPS, fir_q, st_q, BLOCK);
        for (int i = 0; i < NSAMP; i += BLOCK)
            arm_fir_q15(&Q, xq + i, yq + i, BLOCK);
        double gq = amp_q(yq) / 0.5;
        printf("fir f=%.0f gain_f32=%.5f db=%.2f gain_q15=%.5f\n", FREQ[k], gf, 20 * log10(gf), gq);
    }

    /* задержка КИХ: импульс на входе — пик на выходе */
    {
        arm_fir_instance_f32 S;
        arm_fir_init_f32(&S, NTAPS, fir_c, st_f, BLOCK);
        memset(xin, 0, sizeof xin);
        xin[0] = 1;
        arm_fir_f32(&S, xin, yout, BLOCK);
        arm_fir_f32(&S, xin + BLOCK, yout + BLOCK, BLOCK);
        int pk = 0;
        for (int i = 1; i < 2 * BLOCK; i++)
            if (yout[i] > yout[pk]) pk = i;
        printf("fir delay_samples=%d delay_ms=%.1f\n", pk, pk * 1000 / FS);
    }

    /* ── БИХ ── */
    double c[5];
    design_biquad(100, c);
    printf("biquad b0=%.9f b1=%.9f b2=%.9f a1=%.9f a2=%.9f\n", c[0], c[1], c[2], c[3], c[4]);
    /* CMSIS: y = b0x + b1x1 + b2x2 + a1y1 + a2y2 — обратная связь со знаком «+», поэтому −a1, −a2 */
    float32_t cm_ok[5] = {(float32_t)c[0], (float32_t)c[1], (float32_t)c[2], (float32_t)-c[3], (float32_t)-c[4]};
    float32_t cm_bad[5] = {(float32_t)c[0], (float32_t)c[1], (float32_t)c[2], (float32_t)c[3], (float32_t)c[4]};
    for (int variant = 0; variant < 2; variant++) {
        for (unsigned k = 0; k < 4; k++) {
            float32_t st[4] = {0};
            arm_biquad_casd_df1_inst_f32 S;
            arm_biquad_cascade_df1_init_f32(&S, 1, variant ? cm_bad : cm_ok, st);
            make_tone(FREQ[k], 0.5);
            for (int i = 0; i < NSAMP; i += BLOCK)
                arm_biquad_cascade_df1_f32(&S, xin + i, yout + i, BLOCK);
            double g = amp_f(yout) / 0.5;
            printf("biquad_%s f=%.0f gain=%.5f db=%.2f\n", variant ? "matlab_sign" : "cmsis_sign", FREQ[k], g, 20 * log10(g));
        }
    }

    /* ── БИХ в Q15: |a1| > 1 не помещается в Q15 — коэффициенты делят на 2, postShift = 1 ── */
    q15_t cq[6];
    {
        double half[5] = {c[0] / 2, c[1] / 2, c[2] / 2, -c[3] / 2, -c[4] / 2};
        cq[0] = (q15_t)lrint(half[0] * 32768);
        cq[1] = 0;                                  /* в Q15 между b0 и b1 — нулевое слово */
        cq[2] = (q15_t)lrint(half[1] * 32768);
        cq[3] = (q15_t)lrint(half[2] * 32768);
        cq[4] = (q15_t)lrint(half[3] * 32768);
        cq[5] = (q15_t)lrint(half[4] * 32768);
        printf("biquad_q15 coeffs=%d,%d,%d,%d,%d,%d postShift=1 (-a1=%.6f > 1)\n", cq[0], cq[1], cq[2], cq[3], cq[4], cq[5], -c[3]);
    }
    for (unsigned k = 0; k < 4; k++) {
        q15_t st[4] = {0};
        arm_biquad_casd_df1_inst_q15 S;
        arm_biquad_cascade_df1_init_q15(&S, 1, cq, st, 1);
        make_tone(FREQ[k], 0.5);
        for (int i = 0; i < NSAMP; i += BLOCK)
            arm_biquad_cascade_df1_q15(&S, xq + i, yq + i, BLOCK);
        double g = amp_q(yq) / 0.5;
        printf("biquad_q15 f=%.0f gain=%.5f db=%.2f\n", FREQ[k], g, 20 * log10(g));
    }
    printf("done=1\n");
    return 0;
}

Дальше: раздел 81б «Алгоритм Гёрцеля: найти тон без БПФ» — одна частота без всего спектра: тоновый набор, окно и q31.