卡尔曼滤波在目标跟踪应用仿真研究Word格式.docx
《卡尔曼滤波在目标跟踪应用仿真研究Word格式.docx》由会员分享，可在线阅读，更多相关《卡尔曼滤波在目标跟踪应用仿真研究Word格式.docx（8页珍藏版）》请在冰豆网上搜索。
1．建立模型
当目标做匀速直线运动，取状态变量为：
X=yxVx（3-3）
状态方程为：
Xk+1=ΦX（k）（3-4）
观测方程为：
Zk=HXk+U（k）（3-5）
其中：
Z=ZyZxU=UyUx
H=100010Φ=10001T001
对目标位置和速度的最佳滤波和最佳预测如下：
预测：
Xkk-1=ΦX（k-1|k-1）（3-6）
预测误差协方差：
Pkk-1=ΦP（k-1|k-1）ΦT（3-7）
卡尔曼增益：
Kk=Pkk-1HTHPkk-1HT+R-1（3-8）
滤波：
Xkk=Xkk-1+KkZk-HXkk-1（3-9）
滤波协方差为：
Pkk=I-KkHP（k|k-1）（3-10）
R=σy200σx2
2.初始化
实际上我们常常无法得知目标的初始状态，这时我们可以利用前几个测量值建立状态的起始估计。
现在我们用两点起始法：
X22=Zy
（2）Zx
（2）Zx2-Zx
（1）TP22=σy2000σx2σx2T0σx2T2σx2T
滤波误差均值：
exk=1Ni=1N[xik-xi（k|k）]
滤波误差标准差：
σx=1Ni=1N[xik-xi（k|k）]2-exk2
四、仿真分析-
我们已经把卡尔曼滤波的算法叙述的很清楚，由3-6到3-10的5个公式就很容易实现卡尔曼滤波算法。
在计算机仿真中，我们采用Matlab编写程序，利用蒙特卡洛的方法对跟踪滤波器进行仿真分析，次数为1000次。
以下给出仿真图和结果分析。
图4-1
图4-1是目标的真实轨迹和测量轨迹，测量轨迹是真实轨迹数据添加方差和均值固定的随机测量噪声得到的，目标沿y=2000米做匀速直线运动。
给出的测量轨迹用于滤波后与滤波轨迹作比较分析。
从图中可以看出，测量轨迹围绕真实轨迹作上下浮动。
图4-2
图4-2是滤波轨迹和滤波均值轨迹。
从图中可以看出，滤波开始时误差较大，但是随着时间的推移，滤波误差降低，估计值逐步逼近真实轨迹。
采用蒙特卡洛方法，多次观测取均值，滤波更为接近真实曲线。
图4-3
图4-3是y方向滤波估计误差均值及误差标准差。
滤波开始时误差较大，随着采样点的增加，误差逐渐减小，误差的标准差也具有同样的特性。
达到了滤波误差标准差的方差压缩系数为0.5的要求。
图4-4
图4-4是x方向滤波估计误差均值及误差标准差。
与y方向的估计误差均值相比，x方向的估计误差均值波动较大，这是由于在x方向上有速度分量的缘故，同时其方向上滤波估计误差也有一定波动。
也达到了其压缩系数为0.5的要求。
图4-5
X方向速度估计曲线见图4-5。
单次滤波速度与实际值有差距，但是1000次滤波取均值后速度滤波已经于实际值，但是滤波开始时仍有很大偏差，这随着采样点的增加，误差逐渐减小。
五、参考文献
[1]刘福声罗鹏飞，统计信号处理，国防科技大学出版社，1999
[2]万建伟，信号处理实验，国防科技大学出版社
六、附录
实验源程序：
clearall;
clc;
%===============================仿真场景===================================
sigma=10000;
T=2;
t=300;
Vx=15;
H=[100;
010];
Q=[100;
01T;
001];
eXk（:
t）=[000]'
;
eXz（:
eeXz（:
t）=[00]'
N=10;
%蒙特卡洛次数
fori=1:
N
forj=1:
t
Zk（:
j）=[2000+wgn（1,1,40）;
-10000+Vx*T*（j-1）+wgn（1,1,40）];
end
300
ifj==1
Xk（:
1）=[Zk（1,1）,Zk（2,1）,0]'
Xk1（:
1）=Xk（:
1）;
2）=[Zk（1,2）,Zk（2,2）,（Zk（2,2）-Zk（2,1））/T]'
2）=Xk（:
2）;
Pk=[sigma,0,0;
0,sigma,sigma/T;
0,sigma/T,2*sigma/T];
else
ifj>
2
Xk1（:
j）=Q*Xk（:
j-1）;
%预测
Pk1=Q*Pk*Q'
%预测误差协方差
Kk=Pk1*H'
*inv（H*Pk1*H'
+sigma*eye
（2））;
%kalman增益
Xk（:
j）=Xk1（:
j）+Kk*（Zk（:
j）-H*Xk1（:
j））;
%滤波
Pk=（eye（3）-Kk*H）*Pk1;
%滤波协方差
end
end
%%
%1000次求平均
eXk（:
j）=eXk（:
j）+Xk（:
j）/N;
eXz（:
j）=eXz（:
j）+（[2000;
-10000+Vx*（j-1）*T;
0]-Xk（:
j））/N;
%滤波误差均值
eeXz（:
j）=eeXz（:
j）+[（2000-Xk（1,j））^2;
（-10000+Vx*（j-1）*T-Xk（2,j））^2]/N;
%滤波误差标准差
end
%===========================绘图====================================
%真实轨迹和测量轨迹
subplot（2,1,1）;
j=0:
0.1:
t;
plot（-10000+Vx*（j-1）*T,2000）;
title（'
目标真实轨迹'
）;
xlabel（'
X（米）'
ylabel（'
Y（米）'
subplot（2,1,2）;
plot（Zk（2,:
）,Zk（1,:
））;
测量轨迹'
%滤波单次仿真和蒙特卡洛仿真
figure;
%subplot（2,1,1）;
plot（Xk（2,:
）,Xk（1,:
）,'
g'
单次滤波数据曲线'
holdon
%subplot（2,1,2）;
plot（eXk（2,:
）,eXk（1,:
1000次滤波数据曲线'
%
j=1:
subplot（211）;
plot（j,eXz（2,:
X滤波误差均值曲线'
采样次数'
subplot（212）;
forj=1:
eeXz（1,j）=sqrt（eeXz（1,j）-eXz（1,j）^2）;
eeXz（2,j）=sqrt（eeXz（2,j）-eXz（2,j）^2）;
plot（j,eeXz（2,:
x滤波误差标准差曲线'
plot（j,eXz（1,:
y滤波误差均值曲线'
plot（j,eeXz（1,:
y滤波误差标准差曲线'
plot（j,Xk（3,:
单次滤波速度估计'
plot（j,eXk（3,:
1000次滤波速度估计'
升级会员 