最近在看一个云台开源项目SimpleBGC，研究了一下里面的滤波算法，发现实现方式很有意思，但有些地方写得比较草率，值得仔细分析一下。

1. firstOrderFilter 一阶低通滤波

1.1 代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
void initFirstOrderFilter(void)
{
    float a;
    a = 2.0f * eepromConfig.accelX500HzLowPassTau * 500.0f;
    firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx1 = 1.0f / (1.0f + a);
    firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx2 = 1.0f / (1.0f + a);
    firstOrderFilters[ACCEL_X_500HZ_LOWPASS].gx3 = (1.0f - a) / (1.0f + a);
    firstOrderFilters[ACCEL_X_500HZ_LOWPASS].previousInput  = 0.0f;
    firstOrderFilters[ACCEL_X_500HZ_LOWPASS].previousOutput = 0.0f;
    ...
}

float firstOrderFilter(float input, struct firstOrderFilterData *filterParameters)
{
    float output;

    output = filterParameters->gx1 * input +
             filterParameters->gx2 * filterParameters->previousInput -
             filterParameters->gx3 * filterParameters->previousOutput;

    filterParameters->previousInput  = input;
    filterParameters->previousOutput = output;

    return output;
}

1.2 公式推导

结论：这是一个标准的一阶低通滤波，采用双线性变换进行离散化。

定义 $f_s$ 为采样频率，$\tau$ 为时间常数（即 eepromConfig.accelX500HzLowPassTau）：

$$g_1 = g_2 = \frac{1}{1+2f_s\tau}$$ $$g_3 = \frac{1-2f_s\tau}{1+2f_s\tau}$$

差分方程：

$$y = g_1x+g_2xz^{-1}-g_3yz^{-1}$$

传递函数：

$$\frac{y}{x} = \frac{g_1+g_2z^{-1}}{1+g_3z^{-1}}$$

代入 $g_1=g_2$：

$$\frac{y}{x} = \frac{1+z^{-1}}{1+z^{-1}+2f_s\tau(1-z^{-1})}$$

分子分母同除以 $(1+z^{-1})$：

$$\frac{y}{x} = \frac{1}{1+2f_s\tau(\frac{1-z^{-1}}{1+z^{-1}})}$$

由双线性变换公式：

$$s=\frac{2}{T_s} (\frac{1-z^{-1}}{1+z^{-1}})$$

其中 $T_s = \frac{1}{f_s}$，代入化简最终得到：

$$\frac{1}{\tau s+1}$$

这正是标准的一阶低通滤波，时间常数由 eepromConfig.accelX500HzLowPassTau 确定。

2. 低通滤波（后向差分形式）

2.1 代码

1
smoothAcc[ROLL]  = ((smoothAcc[ROLL ] * 99.0f) + accAngle[ROLL ]) / 100.0f;

2.2 公式推导

这也是一个一阶低通滤波，但采用的是后向差分法进行离散化。

对等式两边同乘 100：

$$100y = 99yz^{-1}+x$$

传递函数：

$$\frac{y}{x} = \frac{1}{100-99z^{-1}}$$

整理：

$$\frac{y}{x} = \frac{1}{1+99(1-z^{-1})}$$

由于后向差分法的离散化关系：

$$s=\frac{1-z^{-1}}{T_s}$$

代入可得：

$$\frac{y}{x} = \frac{1}{1+99T_ss}$$

因此，这是一个时间常数为 $99T_s$ 的一阶低通滤波器。

这里 $T_s$ 为采样周期，假设代码运行频率为 500Hz，即 $T_s=2\text{ms}$，那么时间常数 $\tau=99\times2\text{ms}=198\text{ms}$，截止频率约为 $f_c=\frac{1}{2\pi\tau}\approx0.8\text{Hz}$。

3. 互补滤波

3.1 代码

1
orient[PITCH]   = (orient[PITCH] + gyroRate[PITCH] * dt) + 0.0002f * (smoothAcc[PITCH] - orient[PITCH]);

其中：

  • gyroRate[PITCH]：陀螺仪测得的角速度（$x_1$，信号自带高频分量）
  • smoothAcc[PITCH]：加速度计测得的位姿（$x_2$，信号经过平滑，低频准确）

3.2 公式推导

这是一个互补滤波器（complementary filter），陀螺仪信号通过高通，加速度计信号通过低通，两者融合。

离散化方程：

$$y = yz^{-1}+x_1T_s+0.0002x_2-0.0002yz^{-1}$$

整理：

$$y(1 - z^{-1} + 0.0002z^{-1}) = x_1T_s + 0.0002x_2$$ $$y = \frac{T_s}{1+0.9998(1-z^{-1})}x_1s + \frac{0.0002}{1+0.9998(1-z^{-1})}x_2$$

由后向差分 $s=\frac{1-z^{-1}}{T_s}$，代入后可得到连续域形式：

$$y = \frac{4999T_ss}{1+4999T_ss}x_1 + \frac{1}{1+4999T_ss}x_2$$

刚好是一个高通 + 低通的形式，两路相加增益为1。

3.3 代码中存在的问题

原代码中陀螺项的系数有一点点偏差。如果按照严格的互补滤波（两项相加的直流增益为1，高频增益也为1），陀螺项的系数应当比 0.0002 的倒数小1。

正确的写法应该是：

1
orient[PITCH]   = (orient[PITCH] + 0.9998f * gyroRate[PITCH] * dt) + 0.0002f * (smoothAcc[PITCH] - orient[PITCH]);

这样两项的互补关系更严格——高频由陀螺仪主导，低频由加速度计主导，过渡处的幅值特性更平滑。

不过在实际工程中，这点小偏差往往被噪声和模型不确定性掩盖，效果上几乎感知不到区别。