\( \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 \) 次 \( x \) 的估计值(测量出 \( z_{n} \) 后进行估算)
\( \hat{x}_{n,n-1} \) 是 \( x \) 的先验估计值,在时间 \( n-1 \) 时计算出(测量出 \( z_{n-1} \) 后进行估算)
\( \hat{x}_{n+1,n} \) 是对未来 (\( n+1 \) ) 时 \( x \) 的估计。在第 \( n \)次测量出 \( z_{n} \) 后进行估算。换言之,\( \hat{x}_{n+1,n} \)是一个预测值
注:在文献中,顶部带有插入符号的变量为估计值。

本例中的动态模型是定值,所以 \( \hat{x}_{n+1,n}= \hat{x}_{n,n} \)。

想要估计 \( \hat{x}_{n,n} \) ,我们需要记住所有历史测量数据。假设我们没有笔和纸来记录,也不能凭的记忆记下所有的历史测量数据。但我们可以仅用上一次的估计值和一点小小的调整(在现实生活的应用中,我们想节省计算机内存),以及一个数学小技巧来做到这一点:

备注
\( \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{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} = \) 拆分 \( \frac{n-1}{n} \)
\( = \hat{x}_{n-1,n-1}+ \frac{1}{n} \left( z_{n}- \hat{x}_{n-1,n-1} \right) \) 重新排列

上述方程是卡尔曼滤波五个方程之一,名为状态更新 State Update Equation。其含义为:

State update

系数 \( \frac{1}{N} \) 对于这个例子是特定的。稍后我们将讨论这个系数的重要作用,但我想指出的是,在卡尔曼滤波中,这个系数叫做 卡尔曼增益 Kalman Gain,记作 \( K_{n} \) ,脚标 \( n \) 表示卡尔曼增益随每次迭代而变化。

推导出 \( K_{n} \) 是鲁道夫·卡尔曼的主要贡献之一。

在我们深入学习卡尔曼滤波器前,会使用希腊字母 \( \alpha _{n} \) 来代替 \( K_{n} \).

因此,状态更新方程为:

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

\( \left( z_{n}- x_{n,n-1} \right) \) 为观测残差,叫做更新innovation,更新包含了新的信息。

本例中, \( \frac{1}{N} \) 随着 \( N \) 的增加而减少。这意味着刚开始时权重值没有足够的信息,因此估计基于测量结果。随着迭代的继续,\( \frac{1}{N} \) 一直减小,每一次的测量值在估算中的权重越来越小。迭代足够多后,新的测量值对估计值的影响可以忽略不记。

让我们继续看这个例子,在进行第一次测量前,可以通过金条上的图章来猜测(或粗略估计)金条的重量。这个初始猜测 Initial Guess也就是第一个估计值。

后面会讲到,卡尔曼滤波器需要预设一个初始猜测值,这个值不用很精准。

估计算法

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

Estimation algorithm

准备好开始测量和估计了。

数值案例

第零次迭代

初始化

我们对金条重量的初步估计是1000克。滤波器初始化操作仅需一次,不会用在下一次迭代中。

\[ \hat{x}_{0,0}=1000g \]

预测

金条的重量是不变的,因此系统的动态模型是静态的,状态的下一个估计值(预测值)等于初始值:

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

第一次迭代

步骤1

第一次秤重:

\[ z_{1}= 1030g \]

步骤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( 1030-1000 \right) =1030g \]

注:在这个特定的例子中,最初的猜测可以是任何值,因为 \( \alpha _{1}= 1 \) ,初始猜测值在第一次迭代就被消去了。
步骤3

系统的动态模型是静态的,因此金条的重量不会改变,状态的下一个估计值(预测值)等于当前的估计值:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}=1030g \]

第二次迭代

经过一个单位时间后,上一次迭代的预测值predicted estimate将成为当前迭代的先验估计值previous estimate

\[ \hat{x}_{2,1}=1030g \]

步骤1

第二次称重:

\[ z_{2}= 989g \]

步骤2

计算增益:

\[ \alpha _{2}= \frac{1}{2} \]

计算当前估计值:

\[ \hat{x}_{2,2}= \hat{x}_{2,1}+ \alpha _{2} \left( z_{2}- \hat{x}_{2,1} \right) =1030+\frac{1}{2} \left( 989-1030 \right) =1009.5g \]

步骤3

\[ \hat{x}_{3,2}= \hat{x}_{2,2}=1009.5g \]

第三次迭代

\[ z_{3}= 1017g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{3}= \frac{1}{3} \] \[ \hat{x}_{3,3}=~ 1009.5+\frac{1}{3} \left( 1017-1009.5 \right) =1012g \] \[ \hat{x}_{4,3}=1012g \]

第四次迭代

\[ z_{4}= 1009g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{4}= \frac{1}{4} \] \[ \hat{x}_{4,4}= 1012+\frac{1}{4} \left( 1009-1012 \right) =1011.25g \] \[ \hat{x}_{5,4}=1011.25g \]

第五次迭代

\[ z_{5}= 1013g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{5}= \frac{1}{5} \] \[ \hat{x}_{5,5}= 1011.25+\frac{1}{5} \left( 1013-1011.25 \right) =1011.6g \] \[ \hat{x}_{6,5}=1011.6g \]

第六次迭代

\[ z_{6}= 979g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{6}= \frac{1}{6} \] \[ \hat{x}_{6,6}= 1011.6+\frac{1}{6} \left( 979-1011.6 \right) =1006.17g \] \[ \hat{x}_{7,6}=1006.17g \]

第七次迭代

\[ z_{7}=1008g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{7}= \frac{1}{7} \] \[ \hat{x}_{7,7}= 1006.17+\frac{1}{7} \left( 1008-1006.17 \right) =1006.43g \] \[ \hat{x}_{8,7}=1006.43g \]

第八次迭代

\[ z_{8}=1042g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{8}= \frac{1}{8} \] \[ \hat{x}_{8,8}= 1006.43+\frac{1}{8} \left( 1042-1006.43 \right) =1010.87g \] \[ \hat{x}_{9,8}=1010.87g \]

第九次迭代

\[ z_{9}=1012g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{9}= \frac{1}{9} \] \[ \hat{x}_{9,9}= 1010.87+\frac{1}{9} \left( 1012-1010.87 \right) =1011g \] \[ \hat{x}_{10,9}=1011g \]

第十次迭代

\[ z_{10}=1011g~~~~~~~~~~~~~~~~~~~~~~~~~~ \alpha _{10}= \frac{1}{10} \] \[ \hat{x}_{10,10}= 1011+\frac{1}{10} \left( 1011-1011 \right) =1011g \] \[ \hat{x}_{11,10}=1011g \]

差不多可以了,增益随每次测量而减小,因此,每一个测量值的影响都比前一个测量值小。1010克很接近真实的重量了。如果进行更多的测量,就会更趋近真实值。

下表总结了测量值和估计值,图表比较了测量值、估计值和真实值。

\( 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} \) 1030 989 1017 1009 1013 979 1008 1042 1012 1011
\( \hat{x}_{n,n} \) 1030 1009.5 1012 1011.25 1011.6 1006.17 1006.43 1010.87 1011 1011
\( \hat{x}_{n+1,n} \) 1030 1009.5 1012 1011.25 1011.6 1006.17 1006.43 1010.87 1011 1011
Measurements vs. True value vs. Estimates

如图所示,估计算法对测量值有平滑效果,估计值会趋近真实值。

总结

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

例子2 – 在一维空间中追踪匀速飞行器

现在,来看一个随时间改变状态的动态系统吧。在这个例子中,我们用α-β 滤波器在一维空间中追踪匀速飞行的飞行器。

假设在一个一维空间,有一架飞行器正在向远离雷达(或朝向雷达)的方向飞行。在一维空间中,雷达的角度不变,飞行器的高度不变,如下图所示。

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

根据方程可知,飞行器在下一个追踪周期的航程等于当前航程加上飞行器速度速度乘以追踪周期。在这个例子中,假设速度恒定,因此飞行器下一个周期的速度等于当前周期的速度。

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

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

状态外推方程一般用矩阵形式表示,我们稍后将学习。

在这个例子中,我们会使用上面这组特定于我们的例子的方程。

注:我们已经学习了卡尔曼滤波五个方程中的两个:
  • 状态更新方程
  • 状态外推方程

现在,我们将在本例中修改状态更新方程。

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

设置雷达跟踪周期 ( \( \Delta t \) ) 为5秒。假设在 \( n -1 \) 时,无人机(UAV)的估计航程为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\ast40=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 \]

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

\[ \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) = x_{n,n-1} \]

于是,我们得到了雷达追踪器的状态更新方程组,它们也被称为α-β轨迹更新方程\( \alpha - \beta \) track update equationsα-β轨迹滤波方程\( \alpha - \beta \) track filtering equations

位置(航程)状态更新方程:
\[ \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滤波,其中希腊字母 \( \alpha \) 替换为英文字母g,希腊字母 \( \beta \) 替换为英文字母h。
注:在本例中,我们用测量航程得出飞机速度 ( \( \dot{x}= \frac{ \Delta x}{ \Delta t} \) ),现代雷达可以利用多普勒效应直接测量径向速度,但我的目的是解释卡尔曼滤波器而不是雷达操作。为了提高通用性,在我们的例子中,会一直用测量航程来推导飞行速度。

估计算法

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

Estimation Algorithm

与上一个例子不同的是,这个例子给出了增益值 \( \alpha \) 和 \( \beta \) ,在卡尔曼滤波器中,取代\( \alpha \) 和 \( \beta \) 的,是每次迭代都会重新计算的卡尔曼增益,稍后我们会学到。

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

数值案例

飞行器在一维空间中,正在径向远离雷达(或朝向雷达)飞行。

\( \alpha - \beta \) 滤波的参数是:

  • \( \alpha =0.2 \)
  • \( \beta =0.1 \)

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

注意:在这个例子中,为了得到便于理解的结果,我们使用的是非常不精确的雷达和低速无人机(UAV)。在现实生活中,雷达通常更精确,被追踪目标的速度也更快。

第零次迭代

初始化

当时间 \( n=0 \) 时,初始条件如下:

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

注:追踪初始化Track Initiation (如何获得初始条件)是一个非常重要的主题,稍后将讨论。现在我们的的目标是了解基本的 \( \alpha - \beta \) 滤波器操作,假设初始条件是由其他人给出的。
预测

用状态外推方程将初始猜测外推到第一个周期( \( n=1 \) ):

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

第一次迭代

在第一个周期( \( 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}= 30110m \]

步骤2

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

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30200+0.2 \left( 30110-30200 \right) =30182m \] \[ \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{30110-30200}{5} \right) =38.2m/s \]

步骤3

使用状态外推方程计算下一次的状态预测值:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \Delta t\hat{\dot{x}}_{1,1} =30182+5 \times 38.2=30373m \] \[ \hat{\dot{x}}_{2,1}= \hat{\dot{x}}_{1,1} =38.2m/s \]

第二次迭代

一个周期后,上一次迭代的预测值变成当前迭代的先验估计值:

\[ \hat{x}_{2,1}=30373m \] \[ \hat{\dot{x}}_{2,1}= 38.2m/s \]

步骤1

雷达测量飞行器的距离:

\[ z_{2}= 30265m \]

步骤2

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

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30373+0.2 \left( 30265-30373 \right) =30351.4m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 38.2+0.1 \left( \frac{30265-30373}{5} \right) =36m/s \]

步骤3

使用状态外推方程计算下一次的状态预测值:

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \Delta t\hat{\dot{x}}_{2,2} =30351.4+5 \times 36=30531.6m \] \[ \hat{\dot{x}}_{3,2}= \hat{\dot{x}}_{2,2} =36m/s \]

第三次迭代

\[ z_{3}= 30740 \] \[ \hat{x}_{3,3}=~ 30531.6+0.2 \left( 30740~ -30531.6 \right) =30573.3m \] \[ \hat{\dot{x}}_{3,3}= 36+0.1 \left( \frac{30740~ -30531.6}{5} \right) =40.2m/s \] \[ \hat{x}_{4,3}= 30573.3+5 \times 40.2=30774.3m \] \[ \hat{\dot{x}}_{4,3}= 40.2m/s \]

第四次迭代

\[ z_{4}= 30750 \] \[ \hat{x}_{4,4}=~ 30774.3+0.2 \left( 30750-30774.3 \right) =30769.5m \] \[ \hat{\dot{x}}_{4,4}= 40.2+0.1 \left( \frac{30750-30774.3}{5} \right) =39.7m/s \] \[ \hat{x}_{5,4}= 30769.5+5 \times 39.7=30968.1m \] \[ \hat{\dot{x}}_{5,4}= 39.7m/s \]

第五次迭代

\[ z_{5}= 31135 \] \[ \hat{x}_{5,5}=~ 30968.1+0.2 \left( 31135 -30968.1 \right) =31001.5m \] \[ \hat{\dot{x}}_{5,5}= 39.7+0.1 \left( \frac{31135 -30968.1}{5} \right) =43.1m/s \] \[ \hat{x}_{6,5}= 31001.5+5 \times 43.1=31216.8m \] \[ \hat{\dot{x}}_{6,5}= 43.1m/s \]

第六次迭代

\[ z_{6}= 31015 \] \[ \hat{x}_{6,6}=~ 31216.8+0.2 \left( 31015 -31216.8 \right) =31176.4m \] \[ \hat{\dot{x}}_{6,6}= 43.1+0.1 \left( \frac{31015 -31216.8}{5} \right) =39m/s \] \[ \hat{x}_{7,6}= 31176.4+5 \times 39=31371.5m \] \[ \hat{\dot{x}}_{7,6}= 39m/s \]

第七次迭代

\[ z_{7}= 31180 \] \[ \hat{x}_{7,7}=~ 31371.5+0.2 \left( 31180 -31371.5 \right) =31333.2m \] \[ \hat{\dot{x}}_{7,7}= 39+0.1 \left( \frac{31180 -31371.5}{5} \right) =35.2m/s \] \[ \hat{x}_{8,7}= 31333.2+5 \times 35.2=31509.2m \] \[ \hat{\dot{x}}_{8,7}= 35.2m/s \]

第八次迭代

\[ z_{8}= 31610 \] \[ \hat{x}_{8,8}=~ 31509.2+0.2 \left( 31610 -31509.2 \right) =31529.4m \] \[ \hat{\dot{x}}_{8,8}= 35.2+0.1 \left( \frac{31610 -31509.2}{5} \right) =37.2m/s \] \[ \hat{x}_{9,8}= 31529.4+5 \times 37.2=31715.4m \] \[ \hat{\dot{x}}_{9,8}= 37.2m/s \]

第九次迭代

\[ z_{9}= 31960 \] \[ \hat{x}_{9,9}=~ 31715.4+0.2 \left( 31960 -31715.4 \right) =31764.3m \] \[ \hat{\dot{x}}_{9,9}= 37.2+0.1 \left( \frac{31960 -31715.4}{5} \right) =42.1m/s \] \[ \hat{x}_{10,9}= 31764.3+5 \times 42.1=31974.8m \] \[ \hat{\dot{x}}_{10,9}=42.1m/s \]

第十次迭代

\[ z_{10}= 31865 \] \[ \hat{x}_{10,10}=~ 31974.8+0.2 \left( 31865 -31974.8 \right) =31952.9m \] \[ \hat{\dot{x}}_{10,10}= 42.1+0.1 \left( \frac{31865 -31974.8}{5} \right) =39.9m/s \] \[ \hat{x}_{11,10}= 31952.9+5 \times 39.9=32152.4m \] \[ \hat{\dot{x}}_{11,10}= 39.9m/s \]

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

\( n \) 1 2 3 4 5 6 7 8 9 10
\( z_{n} \) 30110 30265 30740 30750 31135 31015 31180 31610 31960 31865
\( \hat{x}_{n,n} \) 30182 30351.4 30573.3 30769.5 31001.5 31176.4 31333.2 31529.4 31764.3 31952.9
\( \dot{\hat{x}}_{n,n} \) 38.2 36 40.2 39.7 43.1 39 35.2 37.2 42.1 39.9
\( \hat{x}_{n+1,n} \) 30373 30531.6 30774.3 30968.1 31216.8 31371.5 31509.2 31715.4 31974.8 32152.4
\( \dot{\hat{x}}_{n+1,n} \) 38.2 36.0 40.2 39.7 43.1 39 35.2 37.2 42.1 39.9

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

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 \) 滤波器状态更新方程,还学习了状态外推方程。并且设计了一种基于 \( \alpha - \beta \) 滤波器的一维动态系统估计算法,求解了匀速目标的一个数值案例。

例子3 – 在一维空间中追踪加速飞行器

在这个例子中,我们将用在上一个例子中学习过的 \( \alpha - \beta \) 滤波器追踪以恒定加速度运动的飞行器。

在前一个例子中,我们追踪以40m/s的恒定速度运动的无人机(UAV),下图描述了无人机航程和速度与时间的关系。

Constant Velocity Movement

如图,距离函数是线性的。

现在来看一架战斗机,它先以50m/s的恒定速度飞行15秒,然后以8m/s2的加速度匀加速飞行35秒。

下图显示了目标航程、速度、加速度随时间的变化。

Accelerated Movement

从这张图中可以看出,前15秒内飞行器的速度是不变的,在15秒之后呈线性增长。前15秒内距离的增长是线性的,随后呈平方增长。

我们将使用在上一个例子中用的α-β滤波来追踪这个飞行器。

数值案例

假设飞行器在一维空间中,正在径向朝向雷达(或远离雷达)运动。

\( \alpha - \beta \) 滤波器的参数分别是:

  • \( \alpha =0.2 \)
  • \( \beta =0.1 \)

雷达追踪周期为5秒。

第零次迭代

初始化

时间 \( n=0 \) 时的初始条件是:

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

注:追踪初始化(如何获得初始条件)是一个非常重要的主题,稍后将讨论。现在我们的的目标是了解基本的 \( \alpha - \beta \) 滤波器操作,假设初始条件是由其他人给出的。
预测

使用状态外推方程将初始猜测外推到第一个周期( \( n=1 \) ):

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

下表汇总了滤波器的每次迭代:

\( n \) \( z_{n} \) 当前状态估计值 ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \) ) 预测值 ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \) )
1 \( 30160m \) \[ \hat{x}_{1,1}=~ 30250+0.2 \left( 30160- 30250 \right) =30232m \] \[ \hat{\dot{x}}_{1,1}= 50+0.1 \left( \frac{30160 -30250}{5} \right) =48.2m/s \] \[ \hat{x}_{2,1}= 30232+5 \times 48.2=30473m \] \[ \hat{\dot{x}}_{2,1}= 48.2m/s \]
2 \( 30365m \) \[ \hat{x}_{2,2}=~ 30473+0.2 \left( 30365 -30473 \right) =30451.1m \] \[ \hat{\dot{x}}_{2,2}= 48.2+0.1 \left( \frac{30365 -30473}{5} \right) =46m/s \] \[ \hat{x}_{3,2}= 30451.1+5 \times 46=30681.6m \] \[ \hat{\dot{x}}_{3,2}= 46m/s \]
3 \( 30890m \) \[ \hat{x}_{3,3}=~ 30681.6+0.2 \left( 30890 -30681.6 \right) =30723.3m \] \[ \hat{\dot{x}}_{3,3}= 46+0.1 \left( \frac{30890 -30681.6}{5} \right) =50.2m/s \] \[ \hat{x}_{4,3}= 30723.3+5 \times 50.2=30974.3m \] \[ \hat{\dot{x}}_{4,3}= 50.2m/s \]
4 \( 31050m \) \[ \hat{x}_{4,4}=~ 30974.3+0.2 \left( 31050 -30974.3 \right) =30989.5m \] \[ \hat{\dot{x}}_{4,4}= 50.2+0.1 \left( \frac{31050 -30974.3}{5} \right) =51.7m/s \] \[ \hat{x}_{5,4}= 30989.5+5 \times 51.7=31248.1m \] \[ \hat{\dot{x}}_{5,4}= 51.7m/s \]
5 \( 31785m \) \[ \hat{x}_{5,5}=~ 31248.1+0.2 \left( 31785 -31248.1 \right) =31355.5m \] \[ \hat{\dot{x}}_{5,5}= 51.7+0.1 \left( \frac{31785 -31248.1}{5} \right) =62.5m/s \] \[ \hat{x}_{6,5}= 31355.5+5 \times 62.5=31667.8m \] \[ \hat{\dot{x}}_{6,5}= 62.5m/s \]
6 \( 32215m \) \[ \hat{x}_{6,6}=~ 31667.8+0.2 \left( 32215 -31667.8 \right) =31777.2m \] \[ \hat{\dot{x}}_{6,6}= 62.5+0.1 \left( \frac{32215 -31667.8}{5} \right) =73.4m/s \] \[ \hat{x}_{7,6}= 31777.2+5 \times 73.4=32144.2m \] \[ \hat{\dot{x}}_{7,6}= 73.4m/s \]
7 \( 33130m \) \[ \hat{x}_{7,7}=~ 32144.2+0.2 \left( 33130 -32144.2 \right) =32341.4m \] \[ \hat{\dot{x}}_{7,7}= 73.4+0.1 \left( \frac{33130 -32144.2}{5} \right) =93.1m/s \] \[ \hat{x}_{8,7}= 32341.4+5 \times 93.1=32807m \] \[ \hat{\dot{x}}_{8,7}= 93.1m/s \]
8 \( 34510m \) \[ \hat{x}_{8,8}=~ 32807+0.2 \left( 34510 -32807 \right) =33147.6m \] \[ \hat{\dot{x}}_{8,8}= 93.1+0.1 \left( \frac{34510 -32807}{5} \right) =127.2m/s \] \[ \hat{x}_{9,8}= 33147.6+5 \times 127.2=33783.5m \] \[ \hat{\dot{x}}_{9,8}= 127.2m/s \]
9 \( 36010m \) \[ \hat{x}_{9,9}=~ 33783.5+0.2 \left( 36010 -33783.5 \right) =34228.8m \] \[ \hat{\dot{x}}_{9,9}= 127.2+0.1 \left( \frac{36010 -33783.5}{5} \right) =171.7m/s \] \[ \hat{x}_{10,9}= 34228.8+5 \times 171.7=35087.4m \] \[ \hat{\dot{x}}_{10,9}= 171.7m/s \]
10 \( 37265m \) \[ \hat{x}_{10,10}=~ 35087.4+0.2 \left( 37265 -35087.4 \right) =35522.9m \] \[ \hat{\dot{x}}_{10,10}= 171.7+0.1 \left( \frac{37265 -35087.4}{5} \right) =215.3m/s \] \[ \hat{x}_{11,10}= 35522.9+5 \times 215.3=36559.2m \] \[ \hat{\dot{x}}_{11,10}= 215.3m/s \]

以下图表比较了飞行器航程与速度前75秒的真实值、测量值和估计值。

Range vs. Time
Velocity vs. Time

可以看到真实值或测量值与估计值之间存在一个固定的差值,这个差值被称为滞后误差lag error。滞后误差的其他常见名称有:

  • 动态误差 Dynamic error
  • 系统误差 Systematic error
  • 偏移误差 Bias error
  • 截断误差 Truncation error

案例总结

在这个例子中,我们发现恒定加速度会导致滞后误差。

例子4 – 用 \( \alpha -\beta -\gamma \) 滤波器追踪加速飞行器

在这个例子中,我们将使用 \( \alpha -\beta -\gamma \) 滤波器追踪匀加速运动的飞行器。

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

The \( \alpha -\beta -\gamma \) 滤波器 (也叫做g-h-k滤波器)会将目标的加速度考虑进去,因此状态外推方程为:

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \hat{\dot{x}}_{n,n} \Delta t+ \hat{\ddot{x}}_{n,n}\frac{ \Delta t^{2}}{2} \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}+ \hat{\ddot{x}}_{n,n} \Delta t \] \[ \hat{\ddot{x}}_{n+1,n}= \hat{\ddot{x}}_{n,n} \]
加速度为 \( \ddot{x}_{n} \) ( \( x \) 的二阶导数)

状态更新方程变成:

\[ \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) \] \[ \hat{\ddot{x}}_{n,n}= \hat{\ddot{x}}_{n,n-1}+ \gamma \left( \frac{z_{n}-\hat{x}_{n,n-1}}{0.5 \Delta t^{2}} \right) \]

数值案例

以上一个例子中的场景为例:飞行器先以50m/s的速度匀速飞行15秒,再以8m/s2的加速度匀加速飞行35秒。

\( \alpha \) - \( \beta \) 滤波器的参数分别是:

  • \( \alpha =0.5 \)
  • \( \beta =0.4 \)
  • \( \gamma =0.1 \)

雷达追踪周期为5秒。

注意:在这个例子中,为了得到便于理解的结果,我们使用的是非常不精确的雷达和低速飞行器。在现实生活中,雷达通常更精确,被追踪目标的速度也更快。

第零次迭代

初始化

时间 \( n=0 \) ,初始条件为:

\[ \hat{x}_{0,0}=30000m \] \[ \hat{\dot{x}}_{0,0}=50m/s \] \[ \hat{\ddot{x}}_{0,0}=0m/s^{2} \]

预测

使用状态外推方程将初始猜测外推到第一个周期( \( n=1 \) ):

\[ \hat{x}_{n+1,n}= \hat{x}_{n,n}+ \hat{\dot{x}}_{n,n} \Delta t+ \hat{\ddot{x}}_{n,n}\frac{ \Delta t^{2}}{2}~~ \rightarrow \hat{x}_{1,0}= \hat{x}_{0,0}+ \hat{\dot{x}}_{0,0} \Delta t+ \hat{\ddot{x}}_{0,0}\frac{ \Delta t^{2}}{2}= 30000+ 50 \times 5+ 0 \times \frac{5^{2}}{2}=30250m \] \[ \hat{\dot{x}}_{n+1,n}= \hat{\dot{x}}_{n,n}+ \hat{\ddot{x}}_{n,n} \Delta t~ \rightarrow \hat{\dot{x}}_{1,0}= \hat{\dot{x}}_{0,0} + \hat{\ddot{x}}_{0,0} \Delta t =50+0 \times 5=50m/s \] \[ \hat{\ddot{x}}_{n+1,n}= \hat{\ddot{x}}_{n,n} \rightarrow ~ \hat{\ddot{x}}_{1,0}= \hat{\ddot{x}}_{0,0}= 0m/s^{2} \]

第一次迭代

在第一个周期( \( n=1 \) ),初始的猜测是上一次的估计值:

\[ \hat{x}_{n,n-1}=~ \hat{x}_{1,0}=30250m \] \[ \hat{\dot{x}}_{n,n-1}=~ \hat{\dot{x}}_{1,0}=50m/s \] \[ \hat{\ddot{x}}_{n,n-1}= \hat{\ddot{x}}_{1,0}= 0m/s^{2} \]

步骤1

雷达测量飞行器的距离:

\[ z_{1}= 30160m \]

步骤2

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

\[ \hat{x}_{1,1}=~ \hat{x}_{1,0}+ \alpha \left( z_{1}- \hat{x}_{1,0} \right) =30250+0.5 \left( 30160-30250 \right) =30205m \] \[ \hat{\dot{x}}_{1,1}= \hat{\dot{x}}_{1,0}+ \beta \left( \frac{z_{1}-\hat{x}_{1,0}}{ \Delta t} \right) = 50+0.4 \left( \frac{30160-30250}{5} \right) =42.8m/s \] \[ \hat{\ddot{x}}_{1,1}= \hat{\ddot{x}}_{1,0}+ \gamma \left( \frac{z_{1}-\hat{x}_{1,0}}{0.5 \Delta t^{2}} \right) = 0+0.1 \left( \frac{30160-30250}{0.5 \times 5^{2}} \right) =-0.7m/s^{2} \]

步骤3

使用状态外推方程计算下一次的状态预测值:

\[ \hat{x}_{2,1}= \hat{x}_{1,1}+ \hat{\dot{x}}_{1,1} \Delta t+ \hat{\ddot{x}}_{1,1}\frac{ \Delta t^{2}}{2}= 30205+ 42.8 \times 5+ \left( -0.7 \right) \times \frac{5^{2}}{2}=30410m \] \[ \hat{\dot{x}}_{2,1}=\hat{\dot{x}}_{1,1} + \hat{\ddot{x}}_{1,1} \Delta t =42.8+ \left( -0.7 \right) \times 5=39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= \hat{\ddot{x}}_{1,1}= -0.7m/s^{2} \]

第二次迭代

一个周期后,上一次迭代的预测值变成当前迭代的先验估计值:

\[ \hat{x}_{2,1}=30410m \] \[ \hat{\dot{x}}_{2,1}= 39.2m/s \] \[ \hat{\ddot{x}}_{2,1}= -0.7m/s^{2} \]

步骤1

雷达测量飞行器的距离:

\[ z_{2}= 30365m \]

步骤2

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

\[ \hat{x}_{2,2}=~ \hat{x}_{2,1}+ \alpha \left( z_{2}- \hat{x}_{2,1} \right) =30410+0.5 \left( 30365-30410 \right) =30387.5m \] \[ \hat{\dot{x}}_{2,2}= \hat{\dot{x}}_{2,1}+ \beta \left( \frac{z_{2}-\hat{x}_{2,1}}{ \Delta t} \right) = 39.2+0.4 \left( \frac{30365-30410}{5} \right) =35.6m/s \] \[ \hat{\ddot{x}}_{2,2}= \hat{\ddot{x}}_{2,1}+ \gamma \left( \frac{z_{2}-\hat{x}_{2,1}}{0.5 \Delta t^{2}} \right) = -0.7+0.1 \left( \frac{30365-30410}{0.5 \times 5^{2}} \right) =-1.1m/s^{2} \]

步骤3

使用状态外推方程计算下一次的状态预测值:

\[ \hat{x}_{3,2}= \hat{x}_{2,2}+ \hat{\dot{x}}_{2,2} \Delta t+ \hat{\ddot{x}}_{2,2}\frac{ \Delta t^{2}}{2}= 30387.5+ 35.6 \times 5+ \left( -1.1 \right) \times \frac{5^{2}}{2}=30552m \] \[ \hat{\dot{x}}_{3,2}=\hat{\dot{x}}_{2,2} + \hat{\ddot{x}}_{2,2} \Delta t =35.6+ \left( -1.1 \right) \times 5=30.2m/s \] \[ \hat{\ddot{x}}_{3,2}= \hat{\ddot{x}}_{2,2}= -1.1m/s^{2} \]

第3-9次迭代

下表汇总了滤波器的每次迭代:

\( n \) \( z_{n} \) 当前状态估计值 ( \( \hat{x}_{n,n} \), \( \hat{\dot{x}}_{n,n} \), \( \hat{\ddot{x}}_{n,n} \) ) 预测值 ( \( \hat{x}_{n+1,n} \), \( \hat{\dot{x}}_{n+1,n} \), \( \hat{\ddot{x}}_{n+1,n} \) )
3 \( 30890m \) \[ \hat{x}_{3,3}=~ 30552+0.5 \left( 30890 -30552 \right) =30721m \] \[ \hat{\dot{x}}_{3,3}= 35.6+0.4 \left( \frac{30890 -30552}{5} \right) =57.2m/s \] \[ \hat{\ddot{x}}_{3,3}=~ -1.1+0.1 \left( \frac{30890 -30552}{0.5 \times 5^{2}} \right) =1.6m/s^{2} \] \[ \hat{x}_{4,3}= 30721+5 \times 57.2 + 1.6 \times \frac{5^{2}}{2}=31027.5m \] \[ \hat{\dot{x}}_{4,3}= 57.2 + 1.6 \times 5=65.4m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.6m/s^{2} \]
4 \( 31050m \) \[ \hat{x}_{4,4}=~ 31027.5+0.5 \left( 31050 -31027.5 \right) = 31038.8m \] \[ \hat{\dot{x}}_{4,4}= 65.4+0.4 \left( \frac{31050 -31027.5}{5} \right) = 67.2m/s \] \[ \hat{\ddot{x}}_{4,4}=~ 1.6+0.1 \left( \frac{31050 -31027.5}{0.5 \times 5^{2}} \right) = 1.8m/s^{2} \] \[ \hat{x}_{5,4}= 31038.8+5 \times 67.2 + 1.8 \times \frac{5^{2}}{2}=31397.1m \] \[ \hat{\dot{x}}_{5,4}= 67.2 + 1.8 \times 5=76.2m/s \] \[ \hat{\ddot{x}}_{4,3}= 1.8m/s^{2} \]
5 \( 31785m \) \[ \hat{x}_{5,5}=~ 31397.1+0.5 \left( 31785 -31397.1 \right) = 31591.1m \] \[ \hat{\dot{x}}_{5,5}= 76.2+0.4 \left( \frac{31785 -31397.1}{5} \right) = 107.2m/s \] \[ \hat{\ddot{x}}_{5,5}=~ 1.8+0.1 \left( \frac{31785 -31397.1}{0.5 \times 5^{2}} \right) = 4.9m/s^{2} \] \[ \hat{x}_{6,5}= 31591.1+5 \times 107.2 + 4.9 \times \frac{5^{2}}{2} = 32188.5m \] \[ \hat{\dot{x}}_{6,5}= 107.2 + 4.9 \times 5 = 131.7m/s \] \[ \hat{\ddot{x}}_{6,5}= 4.9m/s^{2} \]
6 \( 32215m \) \[ \hat{x}_{6,6}=~ 32188.5+0.5 \left( 32215 -32188.5 \right) =32201.7m \] \[ \hat{\dot{x}}_{6,6}= 131.7+0.4 \left( \frac{32215 -32188.5}{5} \right) = 133.9m/s \] \[ \hat{\ddot{x}}_{6,6}=~ 4.9+0.1 \left( \frac{32215 -32188.5}{0.5 \times 5^{2}} \right) =5.1m/s^{2} \] \[ \hat{x}_{7,6}= 32201.7+5 \times 133.9 + 5.1 \times \frac{5^{2}}{2}=32935.1m \] \[ \hat{\dot{x}}_{7,6}= 133.9 + 5.1 \times 5=159.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 5.1m/s^{2} \]
7 \( 33130m \) \[ \hat{x}_{7,7}=~ 32935.1+0.5 \left( 33130 -32935.1 \right) =33032.5m \] \[ \hat{\dot{x}}_{7,7}= 159.5+0.4 \left( \frac{33130 -32935.1}{5} \right) =175.1m/s \] \[ \hat{\ddot{x}}_{7,7}=~ 5.1+0.1 \left( \frac{33130 -32935.1}{0.5 \times 5^{2}} \right) =6.7m/s^{2} \] \[ \hat{x}_{7,6}= 33032.5m+5 \times 175.1 + 6.7 \times \frac{5^{2}}{2}=33991.3m \] \[ \hat{\dot{x}}_{7,6}= 175.1 + 6.7 \times 5=208.5m/s \] \[ \hat{\ddot{x}}_{7,6}= 6.7m/s^{2} \]
8 \( 34510m \) \[ \hat{x}_{8,8}= 33991.3+0.5 \left( 34510 -33991.3 \right) =34250.7m \] \[ \hat{\dot{x}}_{8,8}=208.5+0.4 \left( \frac{34510 -33991.3}{5} \right) =250m/s \] \[ \hat{\ddot{x}}_{8,8}=~ 6.7+0.1 \left( \frac{34510 -33991.3}{0.5 \times 5^{2}} \right) =10.8m/s^{2} \] \[ \hat{x}_{9,8}= 34250.7+5 \times 250 + 10.8 \times \frac{5^{2}}{2}=35635.8m \] \[ \hat{\dot{x}}_{9,8}= 250 + 10.8 \times 5=304.1m/s \] \[ \hat{\ddot{x}}_{9,8}= 10.8m/s^{2} \]
9 \( 36010m \) \[ \hat{x}_{9,9}=~ 35635.8+0.5 \left( 36010 -35635.8 \right) =35822.9m \] \[ \hat{\dot{x}}_{9,9}= 304.1+0.4 \left( \frac{36010 -35635.8}{5} \right) =334m/s \] \[ \hat{\ddot{x}}_{9,9}=~ 10.8+0.1 \left( \frac{36010 -35635.8}{0.5 \times 5^{2}} \right) =13.8m/s^{2} \] \[ \hat{x}_{10,9}= 35822.9+5 \times 334+ 13.8 \times \frac{5^{2}}{2}=37665.8m \] \[ \hat{\dot{x}}_{10,9}= 334 + 13.8 \times 5=403.1m/s \] \[ \hat{\ddot{x}}_{10,9}= 13.8m/s^{2} \]
10 \( 37265m \) \[ \hat{x}_{10,10}=~ 37665.8+0.5 \left( 37265 -37665.8 \right) =37465.4m \] \[ \hat{\dot{x}}_{10,10}= 403.1+0.4 \left( \frac{37265 -37665.8}{5} \right) =371.1m/s \] \[ \hat{\ddot{x}}_{10,10}=~ 13.8+0.1 \left( \frac{37265 -37665.8}{0.5 \times 5^{2}} \right) =10.6m/s^{2} \] \[ \hat{x}_{11,10}= 37465.4+5 \times 371.1 +10.6 \times \frac{5^{2}}{2}=39453.5m \] \[ \hat{\dot{x}}_{11,10}= 371.1 + 10.6 \times 5=424.2m/s \] \[ \hat{\ddot{x}}_{11,10}= 10.6m/s^{2} \]

以下图表比较了飞行器航程,速度,加速度前50秒的真实值、测量值和估计值。

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

如图所见,包含加速度动态模型的 \( \alpha -\beta -\gamma \) 滤波器可以追踪匀加速运动的目标,并且消除滞后误差。

但是如果目标的运动状态是高度变化的呢?目标可以通过转弯突然改变飞行方向,真实的目标动态模型可能包括一个突然加速(改变加速度),在这些情况下,具有恒定 \( \alpha -\beta -\gamma \) 系数的 \( \alpha -\beta -\gamma \) 滤波器会产生估计误差,在某些情况下甚至会追踪失效。

卡尔曼滤波器可以处理动态模型中的不确定性,在总结后马上就会学到。

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

\( \alpha -\beta -(\gamma) \) 滤波器有许多类型,它们基于相同的原理:

  • 当前状态估计基于状态更新方程。
  • 状态的下一个估计值(预测值)基于动态模型方程。

这些滤波器之间的主要区别在于加权系数 \( \alpha -\beta -(\gamma) \)的选择。有些滤波器使用恒定的加权系数,有些则需要在每次迭代(循环)时计算加权系数。

\( \alpha \), \( \beta \) 和 \( \gamma \) 的选择对于估计算法的功能性至关重要。另一个重要事项是滤波器的初始化,即滤波器第一次迭代的初始值。

以下是最常用的 \( \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 粒子滤波器

我希望将来写一些关于以上波滤器的教程。但本教程是关于卡尔曼滤波器的,下一个例子就来介绍卡尔曼滤波。

上一节 下一节