卡尔曼滤波实现目标跟踪

按照一般形式，观测方程式为
x[n] = h(s[n]) + w[n]
其中 h 是函数：
（13）
（14） （15）
(16)
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
⎡
⎢
h(s[n])
=
⎢ ⎢
⎢
rx2[n] + ry2[n]⎤⎥⎥
arctan ry[n]
⎥ ⎥
(17)
⎢⎣
rx[n] ⎥⎦
(4)
(5) 滤波误差方差阵:
M [n | n] = (I − K[n]H[n])M [n | n −1]
(5)
其中
H[n]
=
∂h ∂s[n]
s[n]=sˆ[n|n−1]
A 是状态转移矩阵，Q 代表了噪声协方差。h 表示从状态变量的到理想观测（无噪音）的转
变，H[n]是测量矩阵。
2 数学建模与理论分析
（10）状态：Fra bibliotek⎡ ⎢rx
[n
]
⎤ ⎥
⎢ ⎢
ry
[n]
⎥ ⎥
⎢ ⎢
vx
[
n]
⎥ ⎥
1⎣⎢4v y2[n43]⎦⎥
s[n]
=
⎡1 0 Δ 0 ⎤
⎢ ⎢
0
1
0
Δ
⎥ ⎥
⎢0 0 1 0 ⎥
1⎢⎣0440 204413⎥⎦
A
⎡ ⎢
rx
[n
−
1]
⎤ ⎥
⎢ ⎢
ry
[
n
−
1]
⎥ ⎥
⎢ ⎢
v
x
[n
−
1]⎥
⎥
1⎣⎢v4y [2n −413]⎦⎥
(2) 预测最小均方误差方差阵:
M[n | n −1] = AM[n −1| n −1]AT + Q
(2)
(3) 卡尔曼增益矩阵:
K[n] = M[n | n −1]H T [n](C + H[n]M[n | n −1]H T [n])−1
(3)
(4) 滤波:
sˆ[n | n] = sˆ[n | n −1]+ K[n](x[n]− h(sˆ[n | n −1]))
（6）
vy[n] = vy[n −1] + uy[n]
（7）
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
如果没有噪音 ux[n] 和 uy[n] 扰动，速度是不变的，因此，飞行器将被看作是直线运动。
根据运动方程，在时刻 n 的位置为，
rx[n] = rx[n −1] + vx[n −1]Δ
目目目目目目目目目
10
5
0
-5
-10
-8
-6
-4
-2
0
2
4
6
8
10
目目目目目目目目目
10
0
-10
-20
-15
-10
-5
0
5
10
15
图 1 卡尔曼滤波器对于运动轨迹的估计
再利用扩展卡尔曼滤波器对观测样本进行处理，图 2 显示的是对观测样本进行 1 次滤波 处理和 1000 次蒙特卡洛仿真的滤波处理的结果。由于一次滤波处理存在随机性比较大，可 能会偏离真实轨迹较大，但经过 1000 次蒙特卡洛仿真之后可以看到获得比较平滑的运动轨 迹用于逼近真实运动轨迹。
如果所有的噪声为高斯噪声，卡尔曼滤波器最小化了参数估计的均方误差。鉴于只有噪
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
声平均值和标准差，卡尔曼滤波器是最优的线性估计；并且，它结构优良，易于实现。
一个线性化当前均值和方差的卡尔曼滤波器被称为扩展卡尔曼滤波器（EKF）。类似于泰
勒级数的展开，我们可以利用偏导数将非线性方程和测量过程线性化。EKF 可将被预测点的
s[n − 1]
+
⎡0⎤
⎢ ⎢
0
⎥ ⎥
⎢ ⎢
u
x
[
n
]
⎥ ⎥
1⎢⎣4u y2[n43]⎥⎦
u[n]
（11）
测量的是距离和方位：
R[n] = rx2[n] + ry2[n]
（12）
β[n] = arctan ry[n] rx[n]
或者
Rˆ[n] = R[n] + ωR[n] βˆ[n] = β[n]+ωβ [n]
本实验中，我们应用扩展卡尔曼滤波器来跟踪飞行器的位置和速度，飞行器按计划以给
定的方向和速度运动，需测量距离和方位。在得出的飞行器的动力学模型中，我们假定恒定
速度，只受到由风、轻微的速度修正等产生的扰动，这些情况在飞机上是可能出现得，所以
在 n 时刻，在 X 和 Y 方向的速度分量为
vx[n] = vx[n −1] + ux[n]
关键词：卡尔曼滤波 机动 目标跟踪
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
Y(米)
Y(米)
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
y滤 滤 滤 滤 滤 滤 滤 滤 5
0
-5
-10 0
4
20 40
60 80 100 120 140 160 180 200 采采次滤
y滤 滤 滤 滤 目 滤 滤 滤 滤
3
2
1
0 0 20 40 60 80 100 120 140 160 180 200 采采次滤
即 sˆ[−1| −1] = (5,5, 0, 0)T ， 这 样 不 至 于 偏 离 扩 展 卡 尔 曼 滤 波 器 大 的 初 始 MSE 或 M [−1| −1] = 100I 。
对理论运动轨迹和实际运动轨迹进行仿真结果显示在图 1，从图中可以明显看出实际观 测运动的轨迹加入观测噪声之后就难以看出其运动的轨迹。
0 引言
卡尔曼滤波算法是基于线性无偏最小均方误差原则提出的一种递归算法。本文基于该算 法建立了飞行器动力学的数学模型，实现了机动目标跟踪的功能。
1 卡尔曼滤波原理
在统计中，卡尔曼滤波是以鲁道夫—卡尔曼命名的数学方法。它提供了一种有效的计 算（递归）算法来估计过程的状态，它最大限度地减少了均方误差。该滤波器在许多方面的 应用是非常强大：它可以对过去，现在，甚至未来状态进行估计，即使是模拟系统的确切性 不明的情况下。
X滤 滤 滤 滤 滤 滤 滤 滤 2
0
-2
-4
-6 0 20 40 60 80 100 120 140 160 180 200 采采次滤 x滤 滤 滤 滤 目 滤 滤 滤 滤
2
1.5
1
0.5
0 0 20 40 60 80 100 120 140 160 180 200 采采次滤
图 3 X 轴方向上的误差曲线
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
卡尔曼滤波实现目标跟踪
殷加鹏 张斌 郭晨 赵萌 （国防科学技术大学电子科学与工程学院，长沙，410072）
摘要：卡尔曼滤波是一种数据处理方法，它是一种线性最小方差无偏估计准则，基于系统
状态估计和当前观测，通过引入状态空间而获得的新的状态估计.本篇论文陈述了卡尔曼滤 波的基本思路和算法；并通过仿真，显示卡尔曼滤波的功能，以及如何用它来跟踪方向确定、 速度恒定的飞行器。
遗憾的是，测量矢量与信号参数是非线性的。为了估计信号矢量，我们将应用扩展卡尔
曼滤波。由于状态方程（12）是线性的，A[n]刚好是（12）给出的 A,我们只需要确定
H[n] = ∂h ∂s[n] s[n]=sˆ[n|n−1]
(18)
对观测方程求导， 我们得到雅克比矩阵：
⎡ ⎢
rx[n]
ry[n]
0
⎤ 0⎥
⎡0 0 0 0 ⎤
⎢⎢0 0 0
0
⎥ ⎥
Q = ⎢⎢0
0
σ
2 u
0
⎥ ⎥
(20)
⎢ ⎢⎣0 0
0
σ
2 u
⎥ ⎥⎦
由于
ux
[n]
=
vx[n]
−
vx[n
−1]
，要使用精确的
σ
2 u
取决于从样本到样本之间速度分量
的变化，这刚好是加速度的 Δ 倍，并且可以从飞行物理学推到出来。在指定测量噪声的方
差时，我们注意到由（15）（16）测量误差可以看做为 Rˆ[n] 和 βˆ[n]的估计误差，我们通常
图 4 Y 轴方向上的误差曲线