$\alpha -\beta -\gamma$ 滤波器

示例 1 – 给金条称重

在时刻 $n$，估计值 $\hat{x}_{n,n}$ 便是所有之前测量的平均值：

由于金条的重量不随时间改变而改变，系统动态模型在本例中是静态的（恒定），因此有 $\hat{x}_{n+1,n}= \hat{x}_{n,n}$ .

上面求平均的表达式虽然在数学层面是正确的，但是它不具备可实现性。这是因为根据平均值的定义，为了估计 $\hat{x}_{n,n}$ ，我们需要存储下所有的历史测量值，这对内存开销巨大。并且每次获得了新的测量值后都需要完全重新从第一次测量开始计算，这对CPU算力也消耗巨大。

现实一点的考虑是，最好只需存储上一时刻的估计值 $\hat{x}_{n-1,n-1}$ ，并在新的测量完成后更新它即可。下图描述了这个思路：

• 根据当前的测量和先验估计，估计当前的状态。
• 根据当前的状态估计以及系统动态模型，进行下一时刻的预测。

把上述求平均的表达式在数学上等效变换一下，可以得到：

$\hat{x}_{n-1,n-1}$ 就是在 $n - 1$ 时刻使用 $n-1$ 时刻的测量值，对 $x$ 的状态估计。

接下来使用 $n-1$ 时刻的估计值 $\hat{x}_{n-1,n-1}$ 计算 $\hat{x}_{n,n-1}$ （对 $n$ 时刻 $x$ 的预测）。即把 $\hat{x}_{n-1,n-1}$ 外插至 $n$ 时刻。

由于系统模型是静态的，当前时刻对 $x$ 的预测就等于上一时刻对 $x$ 的估计：$\hat{x}_{n,n-1} = \hat{x}_{n-1,n-1}$.

基于上述推导，对当前时刻状态 $\hat{x}_{n,n}$ 的估计可以写成：

上式即为卡尔曼滤波的五个方程之一。称为状态更新方程。其意为：

系数 $\frac{1}{n}$ 是本例特定的。后面会具体谈到这个系数的重要性，但此刻可以先指出，在卡尔曼滤波的语境中，这个系数被称作卡尔曼增益，符号为 $K_{n}$. 其具有下标 $n$ 意味着卡尔曼增益随着每次迭代都会改变。

$K_{n}$ 的提出是Rudolf Kalman重要的贡献之一。

在进展到卡尔曼滤波之前，我们先不用 $K_{n}$，而是用希腊字母 $\alpha _{n}$ 来表示这个系数。

所以状态更新方程可以写作：

$\left( z_{n}- \hat{x}_{n,n-1} \right)$ 这一项被称为“测量残差”，也叫更新量。更新量包含新的信息。

本例中，随着 $n$ 的增加，$\frac{1}{n}$ 会下降。在一开始，因为没有足够的信息，第一次估计完全是基于第一次的测量值的（$\frac{1}{n}|_{n=1} = 1$）。随着迭代进行，每次后续测量的权重都在下降，并且会逐渐变得可以忽略不计。

继续讲示例。在进行第一次测量之前，我们可以根据金条上的钢印来猜测（或粗略估计）金条的重量，这是 初始估计，是算法的第一个估计值。

卡尔曼滤波需要一个初始估计作为初始值，这个值可以非常粗略。

估计算法

下图描述了本例中所使用的算法。

现在，可以开始具体测量和估计的过程了。

数值示例

第0次迭代

初始化

金条重量的初始估计是1000g，这个估计仅在滤波器初始化时使用一次，后续迭代不再需要这个值。

$$\hat{x}_{0,0}=1000g$$

预测

金条的重量不会改变，系统模型是静态的，下一个时刻的预测就等于此时的初始化估计值。

$$\hat{x}_{1,0} = \hat{x}_{0,0}=1000g$$

第1次迭代

第1步

用秤称重：

$$z_{1}= 996g$$

第2步

计算增益。本例中 $\alpha_{n}= \frac{1}{n}$，故：

$$\alpha_{1}= \frac{1}{1}=1$$

用状态更新方程计算当前的估计值：

$$\hat{x}_{1,1}= \hat{x}_{1,0}+ \alpha _{1} \left( z_{1}- \hat{x}_{1,0} \right) = 1000+1 \left( 996-1000 \right) = 996g$$

第3步

系统模型是静态的，金条的重量不应该改变，下一时刻的预测应该等于此时的估计：

$$\hat{x}_{2,1}= \hat{x}_{1,1}=996g$$

第2次迭代

在一个采样周期过后，上一时刻的预测值成为了这一时刻的先验估计。

$$\hat{x}_{2,1}=996g$$

第1步

做第二次称重：

$$z_{2}= 994g$$

第2步

计算增益：

$$\alpha _{2}= \frac{1}{2}$$

计算估计值：

$$\hat{x}_{2,2}= \hat{x}_{2,1}+ \alpha_{2} \left( z_{2}- \hat{x}_{2,1} \right) =996+\frac{1}{2} \left( 994-996 \right) = 995g$$

第3步

$$\hat{x}_{3,2}= \hat{x}_{2,2}=995g$$

第3次迭代

$$z_{3}= 1021g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{3}= \frac{1}{3}$$ $$\hat{x}_{3,3}=~ 995+\frac{1}{3} \left( 1021-995 \right) =1003.67g$$ $$\hat{x}_{4,3}=1003.67g$$

第4次迭代

$$z_{4}= 1000g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{4}= \frac{1}{4}$$ $$\hat{x}_{4,4}= 1003.67+\frac{1}{4} \left( 1000-1003.67 \right) =1002.75g$$ $$\hat{x}_{5,4}=1002.75g$$

第5次迭代

$$z_{5}= 1002g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{5}= \frac{1}{5}$$ $$\hat{x}_{5,5}= 1002.75+\frac{1}{5} \left( 1002-1002.75 \right) =1002.6g$$ $$\hat{x}_{6,5}=1002.6g$$

第6次迭代

$$z_{6}= 1010g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{6}= \frac{1}{6}$$ $$\hat{x}_{6,6}= 1002.6+\frac{1}{6} \left( 1010-1002.6 \right) =1003.83$$ $$\hat{x}_{7,6}=1003.83g$$

第7次迭代

$$z_{7}=983g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{7}= \frac{1}{7}$$ $$\hat{x}_{7,7}= 1003.83+\frac{1}{7} \left( 983-1003.83 \right) =1000.86g$$ $$\hat{x}_{8,7}=1000.86g$$

第8次迭代

$$z_{8}=971g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{8}= \frac{1}{8}$$ $$\hat{x}_{8,8}= 1000.86+\frac{1}{8} \left( 971-1000.86 \right) =997.125g$$ $$\hat{x}_{9,8}=997.125g$$

第9次迭代

$$z_{9}=993g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{9}= \frac{1}{9}$$ $$\hat{x}_{9,9}= 997.125+\frac{1}{9} \left( 993-997.125 \right) =996.67g$$ $$\hat{x}_{10,9}=996.67g$$

第10次迭代

$$z_{10}=1023g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha_{10}= \frac{1}{10}$$ $$\hat{x}_{10,10}= 996.67+\frac{1}{10} \left( 1023-996.67 \right) =999.3g$$ $$\hat{x}_{11,10}=999.3g$$

至此先告一段落。增益随着每次测量而减小，故后面的测量对估计值的贡献总小于前面的测量。我们已经很接近真实的金条重量了（1000g）。如果做更多次的称重，我们会和真值更加接近。

下表汇总并比较了上面的测量值、估计值以及真值。

结果分析

下图中列出了测量值、估计值以及真值。

这个估计算法对测量值有平滑的效果，并且能够收敛到真值。

示例小结

本例中，我们设计了一个针对静态系统的简单估计算法。我们还推导出了五个卡尔曼滤波方程之一的状态更新方程。下一章中我们会重新审视这个状态更新方程。

示例 2 - 跟踪直线匀速运动的飞行器

因此，速度是距离的导数：

雷达向目标的方向以固定频率发射跟踪波束，两次跟踪测量之间的时间间隔为 $\Delta t$.

则匀速运动的动力学模型可以由下面的运动方程给出：

根据这些方程，下一个采样周期时的飞行器距离等于当前飞行器距离加上目标速度乘以采样间隔时间。由于我们假设飞行器的速度不变，下一时刻的速度等于当前时刻的速度。

上述方程称为 状态外插方程 (也叫 转移方程 或 预测方程)。

在上个示例中我们已经用过状态外插方程了，只不过上个例子中这个方程是个恒等式，即下一时刻的状态等于当前时刻的状态。

状态外插方程依赖系统动态模型，因此不同的示例中这个方程也不同。

这个方程有一个以矩阵形式给出的更加一般的形式，后续会讲到。

上述方程的形式是本例特有的。

现在我们来把状态更新方程改一改。

$\alpha - \beta$ 滤波器

假设雷达的跟踪间隔 ( $\Delta t$ ) 为5秒，假设 $n - 1$ 时刻飞行器的距离为30,000m，其速度为40m/s.

使用上述状态外插方程，我们能够预测 $n$ 时刻的目标位置为：

$$\hat{x}_{n,n-1}= \hat{x}_{n-1,n-1}+ \Delta t\hat{\dot{x}}_{n-1,n-1}=30000+5\times40=30200m$$

预测目标 $n$ 时刻的速度为：

$$\hat{\dot{x}}_{n,n-1}= \hat{\dot{x}}_{n-1,n-1}=40m/s$$

然而 $n$ 时刻雷达测量的目标距离 ( $z_{n}$ ) 为30,110m而非30,200m。预测和实际测量的距离之间相差了90m. 这个差有可能是两个原因导致的：

• 雷达测量不够精准
• 飞行器速度变化了。新的速度是 $\frac{30,110-30,000}{5}=22m/s$

哪个原因是正确的呢？

我们把速度的状态更新方程写下来：

系数 $\beta$ 的值和雷达的测量精度等级有关。假设雷达的 $1 \sigma$ 精度是20m，那么90m的误差大概率是飞行器速度改变了，我们应该把 $\beta$ 的值调高一些。如果 $\beta$ 设为0.9，那么此时估计的速度就应该是：

$$\hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.9 \left( \frac{30110-30200}{5} \right) =23.8m/s$$

而另一方面，假如雷达的 $1 \sigma$ 精度是150m，那90m的误差大概率是雷达测得不准，我们应该把 $\beta$ 的值降低一些。如果 $\beta$ 的值降为0.1，那么估计的速度就应该是：

$$\hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) =40+0.1 \left( \frac{30110-30200}{5} \right) =38.2m/s$$

如果是飞行器真实速度从40m/s降到了22m/s，可以看到在10个测量周期之后（把上面的公式以 $\beta$ = 0.1 带入10次），飞行器速度的估计也逐渐降到了22m/s. 而如果是因为雷达测量不准，则后续测量的位置将会大致均匀散布在真实位置前后，整体上计算出来的平均速度会保持在40m/s左右不变。

飞行器位置的状态更新方程与上一个例子里的方程类似：

和上例不同的是，上例里 $\alpha$ 系数每周期都在重新计算 ( $\alpha _{n}= \frac{1}{n}$ )，而本例中 $\alpha$ 则是恒定的。

$\alpha$ 系数的大小和雷达精度有关。对高精度雷达，应该选用高的 $\alpha$，以给测量值分配更高的权重。如果 $\alpha = 1$，则估计的飞行器距离会等于测量值。

$$\hat{x}_{n,n}= \hat{x}_{n,n-1}+ 1 \left( z_{n}- \hat{x}_{n,n-1} \right) = z_{n}$$

如果 $\alpha =0$，则测量值完全起不到任何作用：

$$\hat{x}_{n,n} = \hat{x}_{n,n-1}+ 0 \left( z_{n}- \hat{x}_{n,n-1} \right) = \hat{x}_{n,n-1}$$

于是我们推导出了雷达跟踪问题的状态更新方程。这又叫$\alpha - \beta$ 跟踪更新方程 或 $\alpha - \beta$ 跟踪滤波方程。

估计算法

下图描述了本例所使用的估计算法。

与上一个示例不同，增益（$\alpha$ 和 $\beta$）的值在本例里是给定的。在卡尔曼滤波里，$\alpha$ 和 $\beta$ 会被卡尔曼增益代替，并且每个采样周期会重新计算，后面会讲到。

现在来看一个数值示例。

数值示例

考虑一个在一维世界里正在向雷达靠近（或远离）的飞行器。

$\alpha - \beta$ 滤波器的参数为：

• $\alpha =0.2$.
• $\beta =0.1$.

测量周期是5秒。

第0次迭代

初始化

$n=0$ 时刻的初始条件给定为：

$$\hat{x}_{0,0}=30000m$$ $$\hat{\dot{x}}_{0,0}=40m/s$$

预测

初始值需要外插至第一个测量周期：

$$\hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 40=30200m$$ $$\hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =40m/s$$

第1次迭代

第一个周期 ( $n=1$ )，初始值就是先验估计：

$$\hat{x}_{n,n-1} = \hat{x}_{1,0}=30200m$$ $$\hat{\dot{x}}_{n,n-1} = \hat{\dot{x}}_{1,0}=40m/s$$

第1步

雷达进行一次测距：

$$z_{1}= 30171m$$

第2步

用状态更新方程计算当前的状态估计：

$$\hat{x}_{1,1} = \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30200+0.2 \left( 30171-30200 \right) = 30194.2m$$ $$\hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 40+0.1 \left( \frac{30171-30200}{5} \right) = 39.42m/s$$

第3步

用状态外插方程预测下一周期的状态：

$$\hat{x}_{2,1}= \hat{x}_{1,1}+ \Delta t\hat{\dot{x}}_{1,1} =30194.2+5 \times 39.42=30391.3m$$ $$\hat{\dot{x}}_{2,1}= \hat{\dot{x}}_{1,1} =39.42m/s$$

第2次迭代

一个采样周期过后，状态预测变为先验估计：

$$\hat{x}_{2,1}=30391.3m$$ $$\hat{\dot{x}}_{2,1}= 39.42m/s$$

第1步

雷达进行一次测距：

$$z_{2}= 30353m$$

第2步

用状态更新方程计算当前的状态估计：

$$\hat{x}_{2,2} = \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30391.3+0.2 \left( 30353-30391.3 \right) = 30383.64m$$ $$\hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 39.42 + 0.1 \left( \frac{30353-30391.3}{5} \right) = 38.65m/s$$

第3步

用状态外插方程预测下一周期的状态：

$$\hat{x}_{3,2}= \hat{x}_{2,2}+ \Delta t\hat{\dot{x}}_{2,2} =30383.64+5 \times 38.65=30576.9m$$ $$\hat{\dot{x}}_{3,2}= \hat{\dot{x}}_{2,2} =38.65m/s$$

第3次迭代

$$z_{3}= 30756m$$ $$\hat{x}_{3,3}= 30576.9+0.2 \left( 30756 -30576.9 \right) = 30612.73m$$ $$\hat{\dot{x}}_{3,3}= 38.65+0.1 \left( \frac{30756 - 30576.9}{5} \right) = 42.2m/s$$ $$\hat{x}_{4,3}= 30612.73+5 \times 42.2=30823.9m$$ $$\hat{\dot{x}}_{4,3}= 42.2m/s$$

第4次迭代

$$z_{4}= 30799m$$ $$\hat{x}_{4,4}= 30823.9+0.2 \left( 30799-30823.9 \right) = 30818.93m$$ $$\hat{\dot{x}}_{4,4}= 42.2+0.1 \left( \frac{30799-30823.9}{5} \right) = 41.7m/s$$ $$\hat{x}_{5,4}= 30818.93+5 \times 41.7=31027.6m$$ $$\hat{\dot{x}}_{5,4}= 41.7m/s$$

第5次迭代

$$z_{5}= 31018m$$ $$\hat{x}_{5,5}= 31027.6+0.2 \left( 31018 -31027.6 \right) = 31025.7m$$ $$\hat{\dot{x}}_{5,5}= 41.7+0.1 \left( \frac{31018 -31027.6}{5} \right) = 41.55m/s$$ $$\hat{x}_{6,5}= 31025.7+5 \times 41.55 = 31233.4m$$ $$\hat{\dot{x}}_{6,5}= 41.55m/s$$

第6次迭代

$$z_{6}= 31278m$$ $$\hat{x}_{6,6}= 31233.4+0.2 \left( 31278 -31233.4 \right) = 31242.3m$$ $$\hat{\dot{x}}_{6,6}= 41.55+0.1 \left( \frac{31278 -31233.4}{5} \right) = 42.44m/s$$ $$\hat{x}_{7,6}= 31242.3+5 \times 42.44 = 31454.5m$$ $$\hat{\dot{x}}_{7,6}= 42.44m/s$$

第7次迭代

$$z_{7}= 31276m$$ $$\hat{x}_{7,7}= 31454.5+0.2 \left( 31276 -31454.5 \right) = 31418.8m$$ $$\hat{\dot{x}}_{7,7}= 42.44+0.1 \left( \frac{31276 -31454.5}{5} \right) = 38.9m/s$$ $$\hat{x}_{8,7}= 31418.8+5 \times 38.9 = 31613.15m$$ $$\hat{\dot{x}}_{8,7}= 38.9m/s$$

第8次迭代

$$z_{8}= 31379m$$ $$\hat{x}_{8,8}= 31613.15+0.2 \left( 31379 -31613.15 \right) = 31566.3m$$ $$\hat{\dot{x}}_{8,8}= 38.9+0.1 \left( \frac{31379 -31613.15}{5} \right) = 34.2m/s$$ $$\hat{x}_{9,8}= 31566.3 + 5 \times 34.2 = 31737.24m$$ $$\hat{\dot{x}}_{9,8}= 34.2m/s$$

第9次迭代

$$z_{9}= 31748m$$ $$\hat{x}_{9,9}= 31737.24+0.2 \left( 31748 -31737.24 \right) = 31739.4m$$ $$\hat{\dot{x}}_{9,9}= 34.2+0.1 \left( \frac{31748 -31737.24}{5} \right) = 34.4m/s$$ $$\hat{x}_{10,9}= 31739.4+5 \times 34.4=31911.4m$$ $$\hat{\dot{x}}_{10,9}=34.4m/s$$

第10次迭代

$$z_{10}= 32175m$$ $$\hat{x}_{10,10}= 31911.4+0.2 \left( 32175 -31911.4 \right) = 31964.1m$$ $$\hat{\dot{x}}_{10,10}= 34.4+0.1 \left( \frac{32175 -31911.4}{5} \right) = 39.67m/s$$ $$\hat{x}_{11,10}= 31964.1+5 \times 39.67 = 32162.45m$$ $$\hat{\dot{x}}_{11,10}= 39.67m/s$$

下表汇总了测量值和估计值。

结果分析

下图中列出了测量值、估计值以及真值。

我们的估计算法对测量值有平滑效果，并且能收敛到真值。

用较大的 $\alpha$ 和 $\beta$

下图描述了 $\alpha = 0.8$、$\beta = 0.5$ 时的真值、测量值和估计值。

滤波器的平滑效果降低了很多。当前估计值非常接近测量值，预测的误差也相对较高。

那么我们应该一直选取较低的 $\alpha$ 和 $\beta$ 吗？

答案是否定的。$\alpha$ 和 $\beta$ 的值应该依测量精度而定。如果测量设备精度很高，例如激光雷达，较高的 $\alpha$ 和 $\beta$ 会比较好，会更信赖测量值，对于本例则滤波器对飞行器速度的变化响应更快；如果测量设备精度很低，应该用较低的 $\alpha$ 和 $\beta$，会更信赖预测值，对于本例则滤波器对测量不确定性（误差）的平滑效果会更好，但滤波器对飞行器速度的变化响应就会慢得多。

示例小结

我们推导了 $\alpha - \beta$ 滤波器的状态更新方程，并且还学到了状态外插方程。我们用 $\alpha - \beta$ 滤波器设计了一个一维动态系统估计算法，并用它数值求解了一个匀速运动目标的状态估计问题。

示例 3 - 跟踪直线加速运动的飞行器

下图描述了目标距离、速度和加速度与时间的关系。

如图所示，飞机速度前20秒恒定，随后线性增加。距离则前20秒线性增加，随后以二次曲线增加。

我们现在用上一例的 $\alpha - \beta$ 滤波器来跟踪这架飞行器。

数值示例

假设一维世界中，飞行器径向靠近（或远离）雷达。

$\alpha - \beta$ 滤波器参数为：

• $\alpha =0.2$
• $\beta =0.1$

跟踪周期是5秒。

第0次迭代

初始化

$n=0$ 时刻的初始条件给定为：

$$\hat{x}_{0,0}=30000m$$ $$\hat{\dot{x}}_{0,0}=50m/s$$

预测

初始值需要外插至第一个测量周期：

$$\hat{x}_{n+1,n}= \hat{x}_{n,n}+ \Delta t\hat{\dot{x}}_{n,n} \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \Delta t\hat{\dot{x}}_{0,0} =30000+5 \times 50=30250m$$ $$\hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n} \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} =50m/s$$

第1-10次迭代

所有滤波器的迭代过程归纳在下面的表里：

结果分析

下图中列出了飞行器距离和速度在前75秒的测量值、估计值以及真值。

可以看到在真值和估计值之间有一个稳态误差。这个稳态误差又叫 滞后误差，也叫：

• 动态误差
• 系统误差
• 偏差
• 截断误差

滞后误差在加速阶段开始出现。在加速阶段结束后，滤波器会追上这个误差并收敛到真值。然而，如此大的滞后误差可能直接导致目标跟丢，这是一些应用场景里是无法接受的，例如导弹制导或防空。

示例小结

我们分析了目标加速带来的滞后误差。

示例 4 - 用 $\alpha -\beta -\gamma$ 滤波器跟踪直线加速运动的飞行器

结果分析

下图中列出了飞行器距离、速度和加速度在前50秒的测量值、估计值以及真值。

示例小结

可见，$\alpha -\beta -\gamma$ 滤波器配合包含加速度的动态模型能够跟踪匀加速运动的目标并消除滞后误差。

但是如果目标正在进行机动怎么办？目标可能突然改变飞行方向，此时动态模型可能要包含jerk项（加速度的变化率），这种情况下，参数恒定的 $\alpha -\beta -\gamma$ 滤波器仍然会产生估计误差，并且有可能丢失目标。

卡尔曼滤波可以处理动态模型中的不确定性，这是下一个话题，小结后即将展开讨论。

$\alpha -\beta -(\gamma)$ 滤波器小结

关于这些滤波器也许后面会挑一些做类似的教程，但本教程是针对卡尔曼滤波的，后续示例也均围绕卡尔曼滤波展开。