卡尔曼滤波实现目标跟踪

卡尔曼滤波实现目标跟踪

殷加鹏 张斌 郭晨 赵萌

（国防科学技术大学电子科学与工程学院，长沙，410072）

摘要：卡尔曼滤波是一种数据处理方法，它是一种线性最小方差无偏估计准则，基于系统

状态估计和当前观测，通过引入状态空间而获得的新的状态估计.本篇论文陈述了卡尔曼滤波的基本思路和算法；并通过仿真，显示卡尔曼滤波的功能，以及如何用它来跟踪方向确定、速度恒定的飞行器。

关键词：卡尔曼滤波 机动 目标跟踪

Abstract ：Kalman filter is a data-processing method which is based On linear unbiased minimal variance estimate principle．By virtue of a systematic state estimate and current observation, a new state estimation is obtained by introducing a concept of state space．This paper present the basic ideas and algorithm of Kalman filter. Through the simulation; and show the features of Kalman filter and how it is used to track the vehicle, which is moving in a nominal given direction and at a nominal speed.

KeywordS ：Kalman filter maneuver orbit tracking

0 引言

卡尔曼滤波算法是基于线性无偏最小均方误差原则提出的一种递归算法。本文基于该算法建立了飞行器动力学的数学模型，实现了机动目标跟踪的功能。

1卡尔曼滤波原理

在统计中，卡尔曼滤波是以鲁道夫—卡尔曼命名的数学方法。它提供了一种有效的计

算（递归）算法来估计过程的状态，它最大限度地减少了均方误差。该滤波器在许多方面的应用是非常强大：它可以对过去，现在，甚至未来状态进行估计，即使是模拟系统的确切性不明的情况下。

卡尔曼滤波器产生测量真值的估计值及其相关的计算值的预测值，估计预测值的不确定性，并计算的预测值与实测值加权平均值。它给出不确定性最小的估计值。用该方法产生的估计往往比原来测量真值更接近真实值。从理论上看，卡尔曼滤波是一种有效的实现非线性动力学系统的算法，它是一个类似于隐马尔可夫模型的贝叶斯模型，但其潜在变量的状态空间是连续的，所有潜在的和观察到的变量都服从高斯分布（通常是一个多元高斯分布）。

如果所有的噪声为高斯噪声，卡尔曼滤波器最小化了参数估计的均方误差。鉴于只有噪

声平均值和标准差，卡尔曼滤波器是最优的线性估计；并且，它结构优良，易于实现。

一个线性化当前均值和方差的卡尔曼滤波器被称为扩展卡尔曼滤波器（EKF）。类似于泰勒级数的展开，我们可以利用偏导数将非线性方程和测量过程线性化。EKF可将被预测点的状态和测量方程线性化。

该EKF 给出了一个近似的最优估计。该非线性系统的动力学过程由一个线性版本近似。为了让该近似有效，这个线性化模型是非线性模型在状态估计不确定域的最佳近似 。

扩展卡尔曼滤波器方程如下：

(1) 预测:

ˆ[n |n −1]=As ˆ[n −1|n −1] s

(1)

ˆ[n |n −1]表示预测的信号， s ˆ[n −1|n −1] 表示n-2次滤波后的信号 s

(2) 预测最小均方误差方差阵:

M [n |n −1]=AM [n −1|n −1]A T +Q

(3) 卡尔曼增益矩阵:

(2)

K [n ]=M [n |n −1]H T [n ](C +H [n ]M [n |n −1]H T [n ])−1

(4) 滤波:

(3)

ˆ[n |n ]=s ˆ[n |n −1]+K [n ](x [n ]−h (s ˆ[n |n −1])) s

(5) 滤波误差方差阵:

(4)

其中

M [n |n ]=(I −K [n ]H [n ])M [n |n −1]

(5)

H [n ]=

∂h

s [n ]=s

ˆ[n |n −1]

A 是状态转移矩阵，Q代表了噪声协方差。h表示从状态变量的到理想观测（无噪音）的转变，H[n]是测量矩阵。

2数学建模与理论分析

本实验中，我们应用扩展卡尔曼滤波器来跟踪飞行器的位置和速度，飞行器按计划以给定的方向和速度运动，需测量距离和方位。在得出的飞行器的动力学模型中，我们假定恒定速度，只受到由风、轻微的速度修正等产生的扰动，这些情况在飞机上是可能出现得，所以

在n 时刻，在X 和Y 方向的速度分量为

v [n ]=v [n −1]+u [n ] x x x （6）

v y [n ]=v y [n −1]+u y [n ] （7）

u [n ]u y [n ]

如果没有噪音x 和扰动，速度是不变的，因此，飞行器将被看作是直线运动。

根据运动方程，在时刻n 的位置为，

r [n ]=r [n −1]+v [n −1]Δ （8） x x x

r [n ]=r [n −1]+v [n −1]Δ

y y y

（9）

其中Δ是样本之间的时间间隔。在这个运动方程的离散模型中，飞行器将按前一时刻的

速度运动，然后下一时刻突然改变，这是对真实连续行为的一种近似，。现在，我们选择的

信号矢量是由位置和速度矢量分量组成的。

状态：

s [n ]=(r [n ],r [n ],v [n ],v [n ])T x y x y

（10）

⎡r [n ]⎤⎡r [n −1]⎤⎡0⎤

x ⎢⎥⎡10Δ0⎤⎢x ⎥⎢

⎥⎢r [n −1]⎥⎢0⎥⎢r y [n ]⎥⎢⎥⎢⎥=⎢010Δ⎥⎢y ⎥+⎢

⎢v [n ]⎥⎢0010⎥⎢v [n −1]⎥⎢u x [n ]⎥⎥⎥⎢x ⎢x ⎥⎢⎥⎢

u [n ]⎥01⎦⎢v [n −1]⎥⎣00244⎢v [n ]⎥144y ⎦3y ⎣y 4243⎣424⎦⎣⎦11314243A

（11）

s [n ]s [n −1]

u [n ]

测量的是距离和方位：

或者

R [n ]=

r [n ]

y

β[n ]=arctan

r [n ]x ˆ[n ]=R [n ]+ω[n ] R

R

ˆ[n ]=β[n ]+ω[n ] β

（12） （13）

（14） （15）

β

按照一般形式，观测方程式为

x [n ]=h (s [n ])+w [n ] (16)

其中h 是函数：

h (s [n ])=⎢r [n ]⎥ (17)

⎢arctan y ⎥

r [n ]⎥⎢

x ⎣⎦

遗憾的是，测量矢量与信号参数是非线性的。为了估计信号矢量，我们将应用扩展卡尔曼滤波。由于状态方程（12）是线性的，A[n]刚好是（12）给出的A,我们只需要确定

H [n ]=

∂h

∂s [n ]s [n ]=s ˆ[n |n −1]

(18)

对观测方程求导， 我们得到雅克比矩阵：

⎡r [n ]r [n ]⎤y ⎢00⎥

⎢R [n ]R [n ]⎥∂h =⎢⎥ (19) ∂s [n ]⎢−r y [n ]r [n ]⎥00⎥⎢⎢⎥⎣R [n ]R [n ]⎦

最后，我们需要指定驱动噪声和观测噪声的协方差。我们假设风、速度修正等在任何方

2u [n ]σu [n ]

和 y 赋予相同的方差（都用u 表示）并且假定向以同样的幅度出现，那么给x

它们是独立的，这是合理的。那么，我们有

⎡0⎢⎢0Q =⎢0

⎢⎢⎢0⎣

00

000σ2

u 0

0⎤0⎥⎥

⎥2σ⎥u ⎦

(20) 0⎥⎥

σ2u [n ]=v [n ]−v [n −1]

x x 由于x ，要使用精确的u 取决于从样本到样本之间速度分量

的变化，这刚好是加速度的Δ倍，并且可以从飞行物理学推到出来。在指定测量噪声的方差时，我们注意到由（15）（16）测量误差可以看做为R [n ]和β[n ]的估计误差，我们通常假定误差估计

ˆˆ

⎡σ20⎤

⎢R ⎥

C [n ]=C =⎢⎥ (21) 2

⎢0σβ⎥⎣⎦

总之，对这个问题的扩展卡尔曼滤波器方程为(1)(2)(3)(4)(5).

3仿真结果与分析

为了利用扩展卡尔曼滤波器，必须指定一个初始状态估计。在实际中，要有初始位置和

速度的知识是不大可能的。这样，为了说明的缘故，我们选择起始状态要远离真实的状态，

T ˆs [−1|−1]=(5,5,0,0)即，这样不至于偏离扩展卡尔曼滤波器大的初始MSE 或M [−1|−1]=100I 。

对理论运动轨迹和实际运动轨迹进行仿真结果显示在图1，从图中可以明显看出实际观测运动的轨迹加入观测噪声之后就难以看出其运动的轨迹。

目目目目目目目目目

10

50-5-10

目目目目目目目目目

100

-10

-20-15

-10-5051015

图1 卡尔曼滤波器对于运动轨迹的估计

再利用扩展卡尔曼滤波器对观测样本进行处理，图2显示的是对观测样本进行1次滤波处理和1000次蒙特卡洛仿真的滤波处理的结果。由于一次滤波处理存在随机性比较大，可能会偏离真实轨迹较大，但经过1000次蒙特卡洛仿真之后可以看到获得比较平滑的运动轨迹用于逼近真实运动轨迹。

滤滤滤滤滤滤

Y (米)

X (米)

图2 真实运动轨迹和卡尔曼滤波后运动轨迹的比较

下面对滤波器的误差进行相应的分析。图3、图4所示分别为X 方向和Y 方向上的滤

波误差均值和滤波误差标准差。从图中可以很清楚地观测到初始的状态由于对真实目标状态估计不佳，误差很大。然而，经过大约20个样品后，扩展的卡尔曼滤波器靠近真实运动轨道。达到真实运动轨迹附近之后，误差水平保持在零均值附近，从而可以看出此扩展卡尔曼

滤波器对于机动目标运动轨迹的估计预测确实可以达到比较良好的效果。

X 滤滤滤滤滤滤滤滤

20Y (米

)

-2-4-60

20

40

60

100120140

采采次滤

x 滤滤滤滤目滤滤滤滤

80

160

180

200

21.5Y (米)

10.50采采次滤

图3 X轴方向上的误差曲线

y 滤滤滤滤滤滤滤滤

50

Y (米)

-5-10

0204060

100120140采采次滤

y 滤滤滤滤目滤滤滤滤

[1**********]

43

Y (米)

2100

20

40

60

80

100120采采次滤

140

160

180

200

图4 Y轴方向上的误差曲线

参考文献

[1] 刘福声 罗鹏飞，统计信号处理，国防科技大学出版社，1999 [2] 万建伟，信号处理实验，国防科技大学出版社