卡尔曼滤波实现目标跟踪

国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
滤滤滤滤滤滤 10
1次 滤 滤 滤 滤 1000次 滤 滤 滤 滤 真目目目真真
5
0
-5
-10
-15
-10
-5
0
5
10
15
X(米 )
图 2 真实运动轨迹和卡尔曼滤波后运动轨迹的比较
Y(米 )
Y(米)
下面对滤波器的误差进行相应的分析。图 3、图 4 所示分别为 X 方向和 Y 方向上的滤 波误差均值和滤波误差标准差。从图中可以很清楚地观测到初始的状态由于对真实目标状态 估计不佳，误差很大。然而，经过大约 20 个样品后，扩展的卡尔曼滤波器靠近真实运动轨 道。达到真实运动轨迹附近之后，误差水平保持在零均值附近，从而可以看出此扩展卡尔曼 滤波器对于机动目标运动轨迹的估计预测确实可以达到比较良好的效果。
按照一般形式，观测方程式为
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] ⎥⎦
(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]))
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 轴方向上的误差曲线
（10）
状态：
⎡ ⎢
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]⎦⎥
遗憾的是，测量矢量与信号参数是非线性的。为了估计信号矢量，我们将应用扩展卡尔
曼滤波。由于状态方程（12）是线性的，A[n]刚好是（12）给出的 A,我们只需要确定
H[n] = ∂h ∂s[n] s[n]=sˆ[n|n−1]
(18)
对观测方程求导， 我们得到雅克比矩阵：
⎡ ⎢
rx[n]
ry[n]
0
⎤ 0⎥
∂h ∂s[n]
=
⎢ ⎢ ⎢
R[n] −ry[n]
⎢ ⎢⎣
R2[n]
R[n]
rx[n] R2[n]
0
⎥ ⎥ ⎥ 0⎥ ⎥⎦
(19)
最后，我们需要指定驱动噪声和观测噪声的协方差。我们假设风、速度修正等在任何方
向以同样的幅度出现，那么给 ux[n]
和
u
y
[n]
赋予相同的方差（都用
σ
2 u
表示）并且假定
它们是独立的，这是合理的。那么，我们有
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 采采次滤
(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 数学建模与理论分析
即 sˆ[−1| −1] = (5,5, 0, 0)T ， 这 样 不 至 于 偏 离 扩 展 卡 尔 曼 滤 波 器 大 的 初 始 MSE 或 M [−1| −1] = 100I 。
对理论运动轨迹和实际运动轨迹进行仿真结果显示在图 1，从图中可以明显看出实际观 测运动的轨迹加入观测噪声之后就难以看出其运动的轨迹。
（8）
ry[n] = ry[n −1] + vy[n −1]Δ
（9）
其中 Δ 是样本之间的时间间隔。在这个运动方程的离散模型中，飞行器将按前一时刻的
速度运动，然后下一时刻突然改变，这是对真实连续行为的一种近似，。现在，我们选择的
信号矢量是由位置和速度矢量分量组成的。
s[n] = (rx[n], ry[n],vx[n],vy[n])T
图 4 Y 轴方向上的误差曲线
Y(米)
参考文献
[1] 刘福声 罗鹏飞，统计信号处理，国防科技大学出版社，1999 [2] 万建伟，信号处理实验，国防科技大学出版社
（6）
vy[n] = vy[n −1] + uy[n]
（7）
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
如果没有噪音 ux[n] 和 uy[n] 扰动，速度是不变的，因此，飞行器将被看作是直线运动。
根据运动方程，在时刻 n 的位置为，
rx[n] = rx[n −1] + vx[n −1]Δ
0 引言
卡尔曼滤波算法是基于线性无偏最小均方误差原则提出的一种递归算法。本文基于该算 法建立了飞行器动力学的数学模型，实现了机动目标跟踪的功能。
1 卡尔曼滤波原理
在统计中，卡尔曼滤波是以鲁道夫—卡尔曼命名的数学方法。它提供了一种有效的计 算（递归）算法来估计过程的状态，它最大限度地减少了均方误差。该滤波器在许多方面的 应用是非常强大：它可以对过去，现在，甚至未来状态进行估计，即使是模拟系统的确切性 不明的情况下。
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
卡尔曼滤波实现目标跟踪
殷加鹏 张斌 郭晨 赵萌 （国防科学技术大学电子科学与工程学院，长沙，410072）
摘要：卡尔曼滤波是一种数据处理方法，它是一种线性最小方差无偏估计准则，基于系统
状态估计和当前观测，通过引入状态空间而获得的新的状态估计.本篇论文陈述了卡尔曼滤 波的基本思路和算法；并通过仿真，显示卡尔曼滤波的功能，以及如何用它来跟踪方向确定、 速度恒定的飞行器。
假定误差估计
C[n]
=
C
=
⎡⎢σ
⎢
2 R
⎢⎣ 0
0
⎤ ⎥
σ
2 β
⎥ ⎥⎦
(21)
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
总之，对这个问题的扩展卡尔曼滤波器方程为(1)(2)(3)(4)(5).
3 仿真结果与分析
为了利用扩展卡尔曼滤波器，必须指定一个初始状态估计。在实际中，要有初始位置和 速度的知识是不大可能的。这样，为了说明的缘故，我们选择起始状态要远离真实的状态，
⎡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]的估计误差，我们通常
卡尔曼滤波器产生测量真值的估计值及其相关的计算值的预测值，估计预测值的不确定 性，并计算的预测值与实测值加权平均值。它给出不确定性最小的估计值。用该方法产生的 估计往往比原来测量真值更接近真实值。从理论上看，卡尔曼滤波是一种有效的实现非线性 动力学系统的算法，它是一个类似于隐马尔可夫模型的贝叶斯模型，但其潜在变量的状态空 间是连续的，所有潜在的和观察到的变量都服从高斯分布（通常是一个多元高斯分布）。
目目目目目目目目目
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 给出了一个近似的最优估计。该非线性系统的动力学过程由一个线性版本近似。
为了让该近似有效，这个线性化模型是非线性模型在状态估计不确定域的最佳近似 。
扩展卡尔曼滤波器方程如下： (1) 预测:
sˆ[n | n −1] = Asˆ[n −1| n −1]
(1)
sˆ[n | n −1]表示预测的信号， sˆ[n −1| n −1] 表示 n-2 次滤波后的信号
本实验中，我们应用扩展卡尔曼滤波器来跟踪飞行器的位置和速度，飞行器按计划以给
定的方向和速度运动，需测量距离和方位。在得出的飞行器的动力学模型中，我们假定恒定
速度，只受到由风、轻微的速度修正等产生的扰动，这些情况在飞机上是可能出现得，所以
在 n 时刻，在 X 和 Y 方向的速度分量为
vx[n] = vx[n −1] + ux[n]
如果所有的噪声为高斯噪声，卡尔曼滤波器最小化了参数估计的均方误差。鉴于只有噪
国防科学技术大学电子科学与工程学院《随机信号分析与处理》课程论文
声平均值和标准差，卡尔曼滤波器是最优的线性估计；并且，它结构优良，易于实现。
一个线性化当前均值和方差的卡尔曼滤波器被称为扩展卡尔曼滤波器（EKF）。类似于泰
勒级数的展开，我们可以利用偏导数将非线性方程和测量过程线性化。EKF 可将被预测点的
关键词：卡尔曼滤波 机动 目标跟踪
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
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]