卡尔曼滤波应用实例-目标跟踪滤波

卡尔曼滤波应用实例-目标跟踪滤波
目标模型
滤波算法
仿真分析
1.目标模型
雷达数据处理是卡尔曼滤波重要的一个应用领域。
雷达数据处理是指雷达在检测到目标、并录取目标的位置数据以后，对测量
到的目标数据（称为点迹）进行处理，自动形成航迹，并对下一时刻的目标
位置进行预测。如下图
录 取 的 点 迹
自适应 波门 数据 关联 航迹起始 与终止 跟踪 滤波
2 p33 E ( w2 [2]) y

2
T
2
2 a
/ 4 2 / T
2
2
p34 E w y [2] (T / 2)a y [1] ( w y [1] w y [2]) / T / T
2


p44 E (T / 2)a y [1] ( w y [1] w y [2]) / T
x[k 1] Φx[k ] Γn[k ]
z x [k ] x[k ] wx [k ] z y [ k ] y[ k ] w y [ k ]
z[k ] Hx[k ] w[k ]
小结： 2. 跟踪算法 标准形式就是卡尔曼滤波算法的五个基本公式 算法起始：两点起始法
2 2 T Px [2 / 2] 0 0 2 T
滤波误差方差阵： Px [k / k ] ( I K[k ]H)Px [k / k 1]
滤波起始：采用两点起始法，
即利用前两个观测数据 z[1], z[2] 进行起始。
z x [2] ( z [2] z [1]) / T x x ˆ x[2 / 2] z y [2] ( z y [2] z y [1]) / T

2

2 T 2a / 4 22 / T 2
所以，
2 2 T Px [2 / 2] 0 0
2 T T 2 2 4 T
2 2 a 2
0 0
2
0 0
2 T
0 0 2 T 2 2 2 T a 2 2 4 T
2 T 22 2 a 2 4 T
0 0 2 2 T
z x [2] ( z [2] z [1]) / T x x ˆ [2 / 2] x z y [2] ( z y [2] z y [1]) / T
0 0
0 0 2 T 2 2 2 T a 2 2 4 T
2 预测误差方差阵： Px [k / k 1] ΦPx [k 1 / k 1]ΦT a ΓΓT
增益： 滤波：
K[k ] Px [k / k 1]HT ( HPx [k / k 1]HT 2I) 1
ˆ [k / k ] x ˆ [k / k 1] K[k ]( z[k ] Hx ˆ [k / k 1]) x
假定

2 ax 2 ay
2 a
a x , a y 不相关
2 零均值白噪声，方差为 a y
令
x[k ] x[k ] x[k ] y[k ] y[k ]
1 0 a [ k ] x Φ n[k ] 0 a [ k ] y 0
则，测量方程可表示为
z[k ] Hx[k ] w[k ]
其中 w[k ] 为零均值白噪声，其方差阵为
R E ( w[k ]w [k ]) 0
T
2
0 2 I 2
2. 滤波算法
预测：
ˆ [k / k 1] Φx ˆ [k 1 / k 1] x
x[1] T x[1] T a x [1] / 2 x[1] x[1] T a x [1] (T / 2)a x [1] T
2
所以， x[2] ( z x [2] z x [1]) / T (T / 2)a x [1] ( wx [1] wx [2]) / T 同理， y[2] ( z y [2] z y [1]) / T (T / 2)a y [1] ( w y [1] w y [2]) / T
T 1
0 0 T 2 / 2 0 0 0 0 Γ T 0 1 T 0 T 2 / 2 0 0 1 T 0
则 x[k 1] Φx[k ] Γn[k ]
2 T 其中 n[k ] 为零均值白噪声，方差阵为 Q E ( n[k ]n [k ]) a 0
y[2] z y [2] y[2] y[2] w y [2] w y [2]
x[2] wx [2] x[1] wx [1] z x [2] z x [1] x[2] x[2] T T x[2] x[1] wx [1] wx [2] x[2] T T
所以，
wx [2] 1 wx [1] wx [2] T a x [1] 2 T x[2 / 2] w y [2] 1 T a [1] w y [1] w y [2] y T 2
p11 p Px [2 / 2] 21 0 0 p12 p22 0 0 0 0 p33 p43 0 0 p34 p44
2 p11 E ( wx [2]) 2
p12 E wx [2] (T / 2) a x [1] ( wx [1] wx [2]) / T / T
2
Fra Baidu bibliotek
p22 E (T / 2)a x [1] ( wx [1] wx [2]) / T
点迹 预处理
航迹 文件
如何用卡尔曼滤波实现目标的跟踪滤波？ 假定目标在平面上做匀速直线运动，但在运动中受气象条件的轻微扰动，
这种扰动使速度产生变化（加速度），这种速度的变化可以用噪声来建
模。所以，目标的运动模型可以表示为 零均值白噪声，方差为
2 ax
x[k 1] x[k ] Tx[k ] (T 2 / 2)a x [k ] x[k 1] x[k ] Ta x [k ] y[k 1] y[k ] Ty[k ] (T 2 / 2)a y [k ] y[k 1] y[k ] Ta y [k ]
测量：虚线 滤波：实线
1900 1800 1700 1600 1500-10000 -5000
0
110 100 标准差 90 80 70 60 50 40 30 5000 20 0 10 20 30 40 50 60 70 80 90 100 x( 米) 采样次数
(a) 真实轨迹、测量及滤波估计
(b) x方向位置估计误差标准差
2 2 2 假定 x y
wx [k ] 和 w y [k ] 统计独立。
令
z x [ k ] z[k ] z [ k ] y
wx [ k ] w[k ] w [ k ] y
1 0 0 0 H 0 0 1 0
3. 仿真分析 假定目标在平面上沿水平方向做恒速运动， 起始位置为(-10000米,2000米)，运动的速度为15米/秒，雷达扫描周期为
T=10秒， a 0, 100 米。采用蒙特卡洛仿真来分析跟踪滤波器的性能，
仿真次数为50次。
y( 米) 2500 2400 2300 2200 2100 2000
小结： 3. 仿真分析 采用蒙特卡洛仿真方法对平面上匀速运动目标
跟踪的性能进行了仿真分析。
小结： 1. 目标模型
x[k 1] x[k ] Tx[k ] (T 2 / 2)a x [k ] x[k 1] x[k ] Ta x [k ] y[k 1] y[k ] Ty[k ] (T / 2)a y [k ]
2
y[k 1] y[k ] Ta y [k ]
z x [2] x[2] x[2] ( z [2] z [1]) / T x x x[2 / 2] z y [2] y[2] ( z [2] z [1]) / T y y[2] y
x[2] z x [2] x[2] x[2] wx [2] wx [2]
0 2 a I 2 a
雷达的测量一般是观测目标的斜距和方位角，
为了分析方便起见，这里假定对x和y分别独立地进行观测， 因此，观测模型可以表示为
z x [k ] x[k ] wx [k ] z y [ k ] y[ k ] w y [ k ]
零均值白噪声，方差为 2 x 零均值白噪声，方差为 2 y