图解卡尔曼滤波器，无需深厚的数学知识也易懂（第三部分：α−β−γ滤波器）

Tiger

案例1：称金子重量

现在我们已经有足够的基础看第一个简单的例子。在这个例子中，我们将估计静态系统的状态。静态系统是一个在合理时间内不会改变其状态的系统。例如，静态系统可以是一个塔，状态是它的高度。

在这个案例中，我们要估计金条的重量。我们有一个无偏差量度，也就是说，这个量度没有系统误差，但是在测量中依然会包含随机噪声。（误差分为内部外差/系统误差，外部误差/非系统误差，这就想说明的是非内部/系统误差）

在这个例子中，系统是金条，系统的状态是金条的重量。因为我们假设重量在短时间内不会改变，因此系统的动态模型是恒定的。

为了估计系统的状态（即金条重量），我们可以进行多次测量并取其平均值。

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

本例中的动态模型是恒定的，因此 ${\hat{x}}_{n, n} = {\hat{x}}_{n, n + 1}$ \hat{x}_{n,n}=\hat{x}_{n,n+1} 。

注意：在文献中，变量上方有符号，如 $\tilde{x}$ \tilde{x} 表明就是估计值。

为了估计 ${\hat{x}}_{N, N}$ \hat{x}_{N,N} 我们需要记住所有的历史测量值。假设我们没有笔和纸来记录测量值，我们也不能依靠记忆来记忆所有的历史测量值。我们只想使用之前的估计值并添加一个小的调整（在现实应用程序中，我们希望节省计算机内存）。我们可以用一个小的数学技巧来做到这一点：

${\hat{x}}_{n, n - 1}$ \hat{x}_{n,n-1} 表示在时间N时x的预测状态，而这个预测状态时基于在时间N-1时的测量。换句话说， $x_{n, n - 1}$ x_{n,n-1} 是上一次（相较于当前的上一次）的估计值。

上述方程是五个卡尔曼滤波器方程之一。它被称为状态更新方程State Update Equation（敲黑板：后面会一直用到这个方程，请大家务必熟练于心）。其含义如下：

系数 $\frac{1}{N}$ \frac{1}{N} 对于我们的示例是特定的。稍后我们将讨论这个系数的重要作用，不过现在我先指出，在卡尔曼滤波器中，这个因素被称为卡尔曼增益(Kalman Gain)。用 $K_{n}$ K_{n} 表示。下标n表示卡尔曼增益随每次迭代而变化。

$K_{n}$ K_{n} 的发现是Rudolf Kalman的主要贡献之一。

同时，在我们深入探讨卡尔曼滤波器之前，我们将使用希腊字母 $α_{n}$ \alpha_{n} 来表示这个系数而不是 $K_{n}$ K_{n} 。

因此，状态更新方程如下：

$y_{n} - {\hat{x}}_{n, n - 1}$ y_{n}-\hat{x}_{n,n-1} 是测量误差，也被称为innovation。innovation中也包含新信息。（注意， $y_{n}$ y_{n} 是系统在n时的测量值， ${\hat{x}}_{n, n - 1}$ \hat{x}_{n,n-1} 是对系统在n时的估计值）

在我们的案例中， $\frac{1}{N}$ \frac{1}{N} 随着N增加而减小。这意味着在开始时，我们没有足够的重量值信息，因此我们将估计值建立在测量的基础上。随着我们继续测量，由于 $\frac{1}{N}$ \frac{1}{N} 的减小，在估计过程中的每个连续测量的变化起的作用越来越小。直到某个点，新测量的贡献将变得微不足道。

让我们继续看这个例子。在进行第一次测量之前，我们可以通过读取金条上的图章来猜测（或粗略估计）金条的重量。这被称为初步猜测Initial Guess，这将是我们的第一次估计。

正如我们稍后将看到的，卡尔曼滤波器需要初始猜测作为预设，这个猜测可能非常粗略。

1.1 估计算法

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

现在我们准备开始测量和估计过程。

1.2 数值案例

1.2.1 第0次迭代

1）初始化

我们对金条重量的初步估计是1000g。滤波器初始化操作仅需一次，因此在下一次迭代中不需要它。

2）预测

金条的重量不会改变。因此，系统的动态模型是静态的。我们的下一个状态估计（预测）等于初始化：

1.2.2 第一次迭代

step 1

用天平测量重量得到：

step2

1）计算卡尔曼增益。在我们的例子中 $α_{n} = \frac{1}{n}$ \alpha_{n}=\frac{1}{n} ，因此：

2）使用状态更新方程计算当前估计值：

注意：在这个特定的例子中，最初的猜测可以是任何数字。由于 $α_{1} = 1$ \alpha_{1}=1 ，初始猜测在第一次迭代中被消除。

step3

系统的动态模型是静态的，所以金条的重量不应该改变。下一个状态估计（预测）等于当前状态估计：

1.2.3 第二次迭代

单位时间延迟后，上一次迭代的预测值predicted estimate将成为当前迭代的前一个估计值previous estimate：

step 1

对重量进行第二次测量得到：

step 2

1）计算卡尔曼收益Kalman gain:

2）计算当前估计值：

step 3

下面是第3至第10次迭代：

滤波器迭代现在可以停止了。每次测量卡尔曼增益都会减小。因此，每次连续测量的贡献都低于先前测量的贡献。我们的重量接近于1010克。如果我们进行更多的测量，我们会更接近真实值。

下表总结了我们的测量和估计，图表比较了测量值、估计值和真实值。

我们可以看到，我们的估计算法对测量有平滑的影响（意思是，估计算法会使得测量值变化减小），并且它收敛到真实值。

1.2.4案例总结

在这个例子中，我们为一个静态系统开发了一个简单的估计算法。我们还推导出了状态更新方程，这是五个卡尔曼滤波方程之一。

案例2 ：跟踪一维匀速飞行器

现在做一个随时间改变状态的动态系统。在这个例子中，我们将使用α-β filter在一维上跟踪等速飞行器。

让我们假设一个一维的世界。我们假设一架飞机正在朝向远离雷达（或朝向雷达）飞行。在一维世界中，雷达的角度是恒定的，飞机的高度是恒定的，如下图所示。

$x_{n}$ x_{n} 表示在时间n时飞行航程的变化。飞机速度定义为距离相对于时间的变化率，因此速度是距离的导数：

雷达以恒定速率向目标的方向发送跟踪光束。跟踪周期为 $Δ t$ \Delta{t}。

假设速度恒定，系统的动态模型可以用两个运动方程来描述：

根据方程可知，下一个追踪周期的飞机航程等于当前追踪周期的航程加上飞机速度乘以雷达追踪周期时间。在这个例子中，我们假设飞行速度是恒定的。因此，下一个周期的速度等于当前周期的速度。

上述方程组称为状态外推方程State Extrapolation Equation（也称为过渡方程Transition Equation或预测方程Prediction Equation），这是五个卡尔曼滤波方程之一。这个方程组将当前状态外推到下一个状态（预测）。

我们已经在前面的例子中使用了状态外推方程，我们假设下一个状态的重量等于当前状态的重量。

矩阵表示法中有一种状态外推方程的一般形式，我们稍后将学习。

在这个例子中，我们将使用上面的一组特定于我们的案例的方程。

现在，我们将在案例中修改状态更新方程State Update Equation。

2.1 $α - β$ \alpha-\beta filter

设置雷达跟踪周期间隔 $Δ t$ \Delta{t} 为5ms。假设在n−1时，无人机（无人飞行器）的预计航程为30000m，速度为40m/s。

使用状态外推方程，我们可以预测时间n时目标的位置：

时间n时的目标速度：

然而，在时间n，雷达测量的范围（ $y_{n}$ y_{n} ）为30110米，而不是预期的30200米。预期（预测）范围和测量范围之间有90米的间隙。造成这种差距的可能原因有两个：

雷达测量不精确
飞机速度改变了。新速度是： $\frac{30110 - 30000}{5} = 22 m / s$ \frac{30110-30000}{5}=22m/s

这两个说法中哪一个是正确的？

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

系数β的值取决于雷达的精确程度。假设雷达的1σ精度为20米，很可能是由于飞机速度的变化导致了预测距离和测量距离之间90米的差距。在这种情况下，我们将β因子设置为高值。如果我们将β设为0.9，则估计速度为：

假设雷达的1σ精度为150m，很可能是雷达测量误差造成的50米差距。在这种情况下，我们将低值设置为β因子。如果我们将β设为0.1，则估计速度为：

飞机位置的状态更新方程与前一个例子中推导的方程类似：

与以前的例子相反之处是，上一个例子中每次迭代需要重新计算α因子（ $α_{n} = \frac{1}{N}$ \alpha_{n} = \frac{1}{N} ），在这个例子中α因子是常数。

α因子的大小取决于雷达测量精度。对于高精度雷达，我们将选择高α，这将给测量带来很高的权重。如果α=1，则估计范围等于测量范围：

如果α=0，则测量没有意义：

因此，我们得到了一个方程组，它构成了雷达跟踪器的更新状态方程。它们也被称为α-β轨迹更新方程(α−β track update equations)或α-β轨迹滤波方程(α−βtrack filtering equations)。

注：在一些书籍中，α−β过滤器称为g-h过滤器，其中希腊字母α替换为英文字母g，希腊字母β替换为英文字母h。
注：在本例中，我们从距离测量（ $\dot{x} = \frac{Δ x}{Δ t}$ \dot{x}=\frac{\Delta{x}}{\Delta{t}} ）中得出飞机速度。现代雷达可以利用多普勒效应直接测量径向速度。然而，我的目标是解释卡尔曼滤波器而不是雷达操作。因此，为了一般性，在我们的例子中，我将继续从距离测量中推导速度。

2.2 估计算法

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

与上一个例子相反，这个例子给出了增益值α和β。在卡尔曼滤波器中，α和β被每次迭代计算的卡尔曼增益重新计算更新，但我们稍后将学习它。

现在我们准备开始这个数值例子。

2.3 数值案例

考虑飞机在一维世界中正径向远离雷达（或朝向雷达）飞行。

α−βfilter的参数是：

$α = 0.2$ \alpha=0.2
$β = 0.1$ \beta=0.1

雷达追踪周期每5秒更新一次。

注意：在这个例子中，我们将使用非常不精确的雷达和低速无人机（UAV）来更好地显示图形。在现实生活中，雷达通常更精确，目标也更快。

2.3.1 第0次迭代

初始化：

当时间n=0时，初始条件如下：

注：追踪初始化Track Initiation（或我们如何获得初始条件）是一个非常重要的主题，稍后将讨论。现在或者目标是了解基本的α-β过滤器操作，所以假设初始条件是由其他人给出的。

预测：

应使用状态外推方程将初始猜测外推到第一个周期（n=1）：

2.3.2 第一次迭代

在第一个周期（n=1），最初的猜测是上一次的估计值：

step 1

雷达测量飞机的距离：

step 2

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

step 3

使用状态外推方程计算下一个状态估计值：

以下是第2次至第10次迭代：

下表总结了我们的测量和估计。

下表比较了真实值、测量值和估计值。

我们可以看到，我们的估计算法对测量有平滑的影响（即，逐渐测量值的变化缩小）并且它收敛到真实值。

使用高α和β：

下图描述了α=8和β=0.5的真实值、测量值和估计值。

这个滤波器的“平滑度”要低得多。“当前估计”与实测值非常接近，预测误差较大。

那么我们应该一直选择α和β的低值吗？

答案是否定的。α和β的值取决于测量精度。如果我们使用非常精确的设备，比如激光雷达，我们会更喜欢高α和β的测量。在这种情况下，滤波器会对目标的速度变化作出快速响应。另一方面，如果测量精度较低，我们会选择较低的α和β。在这种情况下，滤波器将降低测量中的不确定度（误差）。然而，滤波器对目标速度变化的反应会慢得多。

由于α和β的计算是一个重要的课题，我们将在后面更详细地讨论它。

2.4 案例总结

在这个例子中，我们推导了α-β滤波器状态更新方程。我们还学习了状态外推方程。本文提出了一种基于α-β滤波器的一维动态系统估计算法，并求解了匀速目标的数值案例。