\( \alpha -\beta -\gamma \) 滤波器

本章介绍\(alpha -\beta\) 滤波器和 \(\alpha -\beta -\gamma\) 滤波器,这类滤波器常用来对时间序列数据进行平滑。\(alpha -\beta\) 滤波器和 \(\alpha -\beta -\gamma\)滤波器在原理上和卡尔曼滤波高度相关。

示例 1 – 给金条称重

现在介绍第一个简单示例。本例对一个静态系统的状态进行估计。所谓静态系统,是指在合理时间范围内系统状态不会自发改变的系统。例如一座塔便是一个静态系统,高度便是其状态之一,它不随时间改变而变化。

本例中,我们估计一根金条的重量。假定我们用来称金条的秤是无偏的,即称重结果没有系统性偏差,但是有随机噪声。

Gold bars

金条就是我们所关心的系统,金条的重量就是该系统的状态。该系统的动态模型是恒定的,因为我们假定金条的重量(在短时间内)不会发生变化。

为了估计出该系统的状态(金条重量),我们可以对其进行多次称重,然后取多次测量结果的平均值。

Measurements vs. True value

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

\[ \hat{x}_{n,n}= \frac{1}{n} \left( z_{1}+ z_{2}+ \ldots + z_{n-1}+ z_{n} \right) = \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right) \]
注解:
\( x \) 是金条重量的真值
\( z_{n} \) 是 \( n \) 时刻对金条重量的测量值
\( \hat{x}_{n,n} \) 是在 \( n \) 时刻,使用了 \( n \) 时刻的测量值 \( z_{n} \),对 \( x \) 的估计值
\( \hat{x}_{n+1,n} \) 是在 \( n \) 时刻对未来状态(\( n+1 \) 时刻)的预测,记为 \( \hat{x}_{n+1,n} \) ,或者说外插
\( \hat{x}_{n-1,n-1} \) 是在 \( n-1 \) 时刻,使用了 \( n-1 \) 时刻的测量值 \( z_{n-1} \),对 \( x \) 的估计值
\( \hat{x}_{n,n-1} \) 是一个先验估计 - 在 \( n-1 \) 时刻对 \( n \) 时刻的系统状态所进行的预测(译注:对第n个时刻而言, \( \hat{x}_{n,n-1} \) 是先验估计, \( \hat{x}_{n+1,n} \) 是预测)
注:在本教程中,变量上的尖号符号(或者叫hat)代表这是一个对该变量的估计值。

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

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

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

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

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

注解
\( \hat{x}_{n,n}= \frac{1}{n} \sum _{i=1}^{n} \left( z_{i} \right) \) 求平均公式:\( n \) 个测量的和再除以 \( n \)
\( = \frac{1}{n} \left( \sum _{i=1}^{n-1} \left( z_{i} \right) + z_{n} \right) \) 前 \( n - 1 \) 个测量的和再加上最近一次的测量值整体除以 \( n \)
\( = \frac{1}{n} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} \) \( \frac{1}{n} \) 乘进去展开
\( = \frac{1}{n}\frac{n-1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right) + \frac{1}{n} z_{n} \) 给求和项同时乘以并除以 \( n - 1 \)
\( = \frac{n-1}{n}\color{#FF8C00}{\frac{1}{n-1} \sum _{i=1}^{n-1} \left( z_{i} \right)} + \frac{1}{n} z_{n} \) 调整顺序
橘黄色的项就是上一时刻的估计值
\( = \frac{n-1}{n}\color{#FF8C00}{\hat{x}_{n-1,n-1}} + \frac{1}{n} z_{n} \) 把求和项用上一时刻的估计值替换掉
\( = \hat{x}_{n-1,n-1}- \frac{1}{n}\hat{x}_{n-1,n-1}+ \frac{1}{n} z_{n} \) 把 \(\hat{x}_{n-1,n-1}\) 乘进 \( \frac{n-1}{n} \) 的分子,并拆项
\( = \hat{x}_{n-1,n-1}+ \frac{1}{n} \left( z_{n}- \hat{x}_{n-1,n-1} \right) \) 提出 \( \frac{1}{n} \)

\( \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} \) 的估计可以写成:

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

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

State update

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

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

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

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

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

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

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

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

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

估计算法

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

Example 1 estimation algorithm

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

数值示例

第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 \]

注:初始估计可以是任何值,由于 \( \alpha _{1}= 1 \),初始值在第1次迭代的时候将被抵消。
第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)。如果做更多次的称重,我们会和真值更加接近。

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

\( n \) 1 2 3 4 5 6 7 8 9 10
\( \alpha _{n} \) \( 1 \) \( \frac{1}{2} \) \( \frac{1}{3} \) \( \frac{1}{4} \) \( \frac{1}{5} \) \( \frac{1}{6} \) \( \frac{1}{7} \) \( \frac{1}{8} \) \( \frac{1}{9} \) \( \frac{1}{10} \)
\( z_{n} \) 996 994 1021 1000 1002 1010 983 971 993 1023
\( \hat{x}_{n,n} \) 996 995 1003.67 1002.75 1002.6 1003.83 1000.86 997.125 996.67 999.3
\( \hat{x}_{n+1,n} \) 996 995 1003.67 1002.75 1002.6 1003.83 1000.86 997.125 996.67 999.3

结果分析

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

Measurements vs. True value vs. Estimates

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

示例小结

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

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

是时候考虑一下状态随时间变化的动态系统了。本例中,我们尝试用 \( \alpha - \beta \) 滤波器 对一个直线匀速飞行中的飞行器进行跟踪。

假设一个只有一个维度的世界中,有这样一个飞行器在往远离雷达的方向飞行(或者靠近雷达)。因为是一维空间,飞行器到雷达的角度是恒定的,其高度也是恒定的。

1D scenario

\( x_{n} \) 表示 \( n \) 时刻飞行器的距离。飞行器速度可以近似用距离差分法得到 - 计算距离随时间的变化率。

因此,速度是距离的导数:

\[ \dot{x}= v= \frac{dx}{dt} \]

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

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

\[ x_{n+1}= x_{n}+ \Delta t\dot{x}_{n} \] \[ \dot{x}_{n+1}= \dot{x}_{n} \]

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

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

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

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

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

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

注:我们已经学到了两个卡尔曼滤波方程:
  • 状态更新方程
  • 状态外插方程

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

\( \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 \)

哪个原因是正确的呢?

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

\[ \hat{\dot{x}}_{n,n}= \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) \]

系数 \( \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左右不变。

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

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

和上例不同的是,上例里 \( \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 \) 跟踪滤波方程

位置的状态更新方程为:
\[ \hat{x}_{n,n} = \hat{x}_{n,n-1}+ \alpha \left( z_{n}- \hat{x}_{n,n-1} \right) \]

速度的状态更新方程为:
\[ \hat{\dot{x}}_{n,n} = \hat{\dot{x}}_{n,n-1}+ \beta \left( \frac{z_{n}-\hat{x}_{n,n-1}}{ \Delta t} \right) \]
注:在其他的书中,\( \alpha - \beta \) 滤波器又叫 g-h 滤波器,字母g代替了希腊字母 \( \alpha \),字母h代替了希腊字母 \( \beta \).
注:本例中,我们是根据雷达距离测量来间接估计的飞行器速度的 ( \( \dot{x}= \frac{ \Delta x}{ \Delta t} \) ),实际上现代雷达可以直接用多普勒效应测量径向速度。但目前我们的目的是解释卡尔曼滤波而非雷达工作原理,所以为了简单起见,我们的示例中将全部用距离观测来间接测量速度。

估计算法

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

Estimation Algorithm

与上一个示例不同,增益(\( \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 \]

注:跟踪问题的初始化(或者说如何获得初始状态)是个十分重要的工作,后面会讲到。目前我们的目标是理解 \( \alpha - \beta \) 滤波器的基本原理,所以我们假设初始值已经合理获取了。
预测

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

\[ \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 \]

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

\( n \) 1 2 3 4 5 6 7 8 9 10
\( z_{n} \) 30171 30353 30756 30799 31018 31278 31276 31379 31748 32175
\( \hat{x}_{n,n} \) 30194.2 30383.64 30612.73 30818.93 31025.7 31242.3 31418.8 31566.3 31739.4 31964.1
\( \dot{\hat{x}}_{n,n} \) 39.42 38.65 42.2 41.7 41.55 42.44 38.9 34.2 34.4 39.67
\( \hat{x}_{n+1,n} \) 30391.3 30576.9 30823.9 31027.6 31233.4 31454.5 31613.15 31737.24 31911.4 32162.45
\( \dot{\hat{x}}_{n+1,n} \) 39.42 38.65 42.2 41.7 41.55 42.44 38.9 34.2 34.4 39.67

结果分析

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

Measurements vs. True value vs. Estimates - low Alpha and Beta

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

用较大的 \( \alpha \) 和 \( \beta \)

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

Measurements vs. True value vs. Estimates - high Alpha and Beta

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

那么我们应该一直选取较低的 \( \alpha \) 和 \( \beta \) 吗?

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

示例小结

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

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

本例中,我们用 \( \alpha - \beta \) 来跟踪匀加速运动的飞行器。

上个示例中,我们跟踪了一个匀速40m/s飞行的飞行器,下图给出了目标距离和速度随时间的变化。

Constant Velocity Movement

可以看到,距离随时间的图像是一条直线。

考虑一架战斗机。战斗机先以50m/s匀速飞行20秒,再以8m/s2匀加速35秒。

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

Accelerated Movement

如图所示,飞机速度前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 \]

注:跟踪问题的初始化(或者说如何获得初始状态)是个十分重要的工作,后面会讲到。目前我们的目标是理解 \( \alpha - \beta \) 滤波器的基本原理,所以我们假设初始值已经合理获取了。
预测

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

\[ \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次迭代

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

\( n \) \( z_{n} \) 当前状态估计 ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \) ) 状态预测 ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \) )
1 \( 30221m \) \[ \hat{x}_{1,1} = 30250+0.2 \left( 30221- 30250 \right) = 30244.2m \] \[ \hat{\dot{x}}_{1,1}= 50+0.1 \left( \frac{30221 -30250}{5} \right) = 49.42m/s \] \[ \hat{x}_{2,1} = 30244.2 + 5 \times 49.42 = 30491.3m \] \[ \hat{\dot{x}}_{2,1}= 49.42m/s \]
2 \( 30453m \) \[ \hat{x}_{2,2} = 30491.3 + 0.2 \left( 30453 - 30491.3 \right) = 30483.64m \] \[ \hat{\dot{x}}_{2,2} = 49.42 + 0.1 \left( \frac{30453 - 30491.3}{5} \right) = 48.65m/s \] \[ \hat{x}_{3,2} = 30483.64 + 5 \times 48.65 = 30726.9m \] \[ \hat{\dot{x}}_{3,2} = 48.65m/s \]
3 \( 30906m \) \[ \hat{x}_{3,3} = 30726.9 + 0.2 \left( 30906 - 30726.9 \right) = 30762.7m \] \[ \hat{\dot{x}}_{3,3}= 48.65 + 0.1 \left( \frac{30906 - 30726.9}{5} \right) = 52.24m/s \] \[ \hat{x}_{4,3} = 30762.7 + 5 \times 52.24 = 31023.9m \] \[ \hat{\dot{x}}_{4,3} = 52.24m/s \]
4 \( 30999m \) \[ \hat{x}_{4,4} = 31023.9 + 0.2 \left( 30999 - 31023.9 \right) = 31018.93m \] \[ \hat{\dot{x}}_{4,4}= 52.24 + 0.1 \left( \frac{30999 - 31023.9}{5} \right) = 51.74m/s \] \[ \hat{x}_{5,4} = 31018.93 + 5 \times 51.74 = 31277.6m \] \[ \hat{\dot{x}}_{5,4} = 51.74m/s \]
5 \( 31368m \) \[ \hat{x}_{5,5} = 31277.6 + 0.2 \left( 31368 - 31277.6 \right) = 31295.7m \] \[ \hat{\dot{x}}_{5,5}= 51.74 + 0.1 \left( \frac{31368 - 31277.6}{5} \right) = 53.55m/s \] \[ \hat{x}_{6,5} = 31295.7 + 5 \times 53.55 = 31563.4m \] \[ \hat{\dot{x}}_{6,5} = 53.55m/s \]
6 \( 31978m \) \[ \hat{x}_{6,6} = 31563.4 + 0.2 \left( 31978 - 31563.4 \right) = 31646.3m \] \[ \hat{\dot{x}}_{6,6}= 53.55 + 0.1 \left( \frac{31978 - 31563.4}{5} \right) = 61.84m/s \] \[ \hat{x}_{7,6} = 31646.3 + 5 \times 61.84 = 31955.5m \] \[ \hat{\dot{x}}_{7,6}= 61.84m/s \]
7 \( 32526m \) \[ \hat{x}_{7,7} = 31955.5 + 0.2 \left( 32526 - 31955.5 \right) = 32069.6m \] \[ \hat{\dot{x}}_{7,7}= 61.84 + 0.1 \left( \frac{32526 - 31955.5}{5} \right) = 73.25m/s \] \[ \hat{x}_{8,7} = 32069.6 + 5 \times 73.25 = 32435.85m \] \[ \hat{\dot{x}}_{8,7} = 73.25m/s \]
8 \( 33379m \) \[ \hat{x}_{8,8} = 32435.85 + 0.2 \left( 33379 - 32435.85 \right) = 32624.5m \] \[ \hat{\dot{x}}_{8,8}= 73.25 + 0.1 \left( \frac{33379 - 32435.85}{5} \right) = 92.1m/s \] \[ \hat{x}_{9,8} = 32624.5 + 5 \times 92.1 = 33085m \] \[ \hat{\dot{x}}_{9,8} = 92.1m/s \]
9 \( 34698m \) \[ \hat{x}_{9,9} = 33085 + 0.2 \left( 34698 - 33085 \right) = 33407.6m \] \[ \hat{\dot{x}}_{9,9}= 92.1 + 0.1 \left( \frac{34698 - 33085}{5} \right) = 124.37m/s \] \[ \hat{x}_{10,9} = 33407.6 + 5 \times 124.37 = 34029.5m \] \[ \hat{\dot{x}}_{10,9} = 124.37m/s \]
10 \( 36275m \) \[ \hat{x}_{10,10} = 34029.5 + 0.2 \left( 36275 - 34029.5 \right) = 34478.6m \] \[ \hat{\dot{x}}_{10,10} = 124.37 + 0.1 \left( \frac{36275 - 34029.5}{5} \right) = 169.28m/s \] \[ \hat{x}_{11,10} = 34478.6 + 5 \times 169.28 = 35325m \] \[ \hat{\dot{x}}_{11,10} = 169.28m/s \]

结果分析

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

Range vs. Time
Velocity vs. Time

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

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

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

示例小结

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

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

本例中,我们用 \( \alpha -\beta -\gamma \) 滤波器来跟踪一架匀加速运动的飞行器。

The \( \alpha -\beta -\gamma \) filter

\( \alpha -\beta -\gamma \) 滤波器 (又叫 g-h-k 滤波器) 能够处理加速中的目标。新的状态外插方程为:

结果分析

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

Range vs. Time
Velocity vs. Time
Acceleration vs. Time

示例小结

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

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

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

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

有许多种 \( \alpha -\beta -\gamma \) 滤波器,这些滤波器基本上都遵循同样的原理:

  • 当前状态估计是基于状态更新方程的。
  • 下一个时刻的状态预测是基于系统动态模型的。

主要区别在这些滤波器对权重系数(\( \alpha -\beta -\gamma \))的选择上。有些滤波器使用恒定权重,有些每次迭代会重新计算权重。

对 \( \alpha \)、\( \beta \) 和 \( \gamma \) 参数的选择对估计算法的性能至关重要。

那么应该怎样选择这些参数呢?

\( \alpha -\beta -(\gamma) \) 滤波器在这里只是作为卡尔曼滤波的切入点,所以我不计划展开讨论参数选择的策略。有兴趣的读者可以在许多文献或书籍中找到相关的信息,例如下面列出的材料:

Dirk Tenne, Tarunraj Singh. “Optimal Design of \( \alpha -\beta -(\gamma) \) Filters”. State University of New York at Buffalo.

另一个重要的问题是滤波器的初始化,例如提供第一次迭代的起始值。

下面列出了一些常见的 \( \alpha -\beta -(\gamma) \) 滤波器(译注:有些滤波器未找到通用的中文译名,故保留原英文名):

  • 维纳滤波 Wiener Filter
  • 贝叶斯滤波 Bayes Filter
  • Fading-memory polynomial Filter
  • Expanding-memory (or growing-memory) polynomial Filter
  • 最小二乘滤波 Least-squares Filter
  • Benedict–Bordner Filter
  • Lumped Filter
  • Discounted least-squares \( \alpha -\beta \) Filter
  • Critically damped \( \alpha -\beta \) Filter
  • Growing-memory Filter
  • 卡尔曼滤波 Kalman Filter
  • 扩展卡尔曼滤波 Extended Kalman Filter
  • 无迹卡尔曼滤波 Unscented Kalman Filter
  • Extended Complex Kalman Filter
  • Gauss-Hermite Kalman Filter
  • Cubature Kalman Filter
  • 粒子滤波 Particle Filter

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

Previous Next