Kalmanfilter仿真作业.docx
《Kalmanfilter仿真作业.docx》由会员分享，可在线阅读，更多相关《Kalmanfilter仿真作业.docx（11页珍藏版）》请在冰豆网上搜索。
Kalmanfilter仿真作业
研究生上机作业
姓名：
**学号:
********
1•情景假设
（学号末尾2,6）假设有一个二坐标雷达对一平面上运动目标的进行观察，目标在t=0〜400秒沿y轴作恒速直线运功，运动速度为-15m/s,目标的起点为
（2000m10000m）,雷达扫描周期为2秒，x和y独立地进行观察，观察噪声的标准差均为100m试建立雷达对目标的跟踪算法，并进行仿真分析，给出仿真结果，画出目标真实轨迹、对目标的观察和滤波曲线。
2.Kalman滤波算法分析
为了简单起见，仅对X轴方向进行考虑。
首先，目标运动沿X轴方向的运动可以用下面的状态方程描述：
x（k1）x（k）T*（k）（T2/2）Ux（k）（21）
X（k1）X（k）Tux（k）.
W（k）
用矩阵的形式表述为，
X（k1）X（k）
在上式中，X（k）
景中ux（k）=0。
考虑雷达的观测，得出观测方程为：
Z（k）C（k）X（k）V（k）（2.3）
在（2.3）中，C（k）10，V（k）为零均值的噪声序列，方差已知。
对目标进行预测，由相关理论可得到下面的迭代式：
X\k/k1）X\k1/k1）（2.4）
在（2.4）中，X?
（k/k1）E[X（k）|Zk1]，反映了由前k1各观测值对目前状态的估计。
而预测的误差协方差可由下式表出，
PX（k/k1）FX'（k1/k1）TQ（k1）T（2.5）
对于最佳滤波，迭代表达式为：
刃（k/k）X?
（k/k1）K（k）[Z（k）C（k）X\k/k1）]（2.6）
在式（2.6）中，K（k）为Kalman增益。
而滤波误差的协方差为,
Px\（k/k）[IK（k）C（k）]P*（k/k1）（2.7）
在应用上面的公式进行Kalman滤波时，需要指定初值。
由于实际中通常无法得到目标的初始状态，我们可以利用前几个观测值建立状态的初始估计，比如
采用前两个观测值，
刃（2/2）乙
（2）Zx
（2）Zx
（1）/TT（2.8）
此时，估计误差为
T
X（2/2）vx
（2）T_ux
（1）如
（1）丁如
（2）（2.9）
而误差协方差矩阵为,
3.仿真计算与结果
首先给出理论航迹,
3.1理论航迹图
由图3.1可以看出目标运动的理论航迹，即沿y轴负方向直线运动。
假定观察噪声的标准差均为100m，可以给出目标的观测数据和理论航迹的
对比图，见图3.2。
12000
2000
11000
10000
8000
7000
6000
5000
4000
3000
15001600170018001900200021002200230024002500
-理论航迹
观测数据-
.-■
9000
3.2观测数据和理论航迹对比图
接下来经过Kalman滤波后，x轴方向的航迹图，见图3.3
X轴方向航迹图
3.3滤波输出x轴方向航迹图
y轴方向的航迹图，见图3.4。
Y轴方向航迹图
3.5。
3.4滤波输出y轴方向航迹图
将输出的x和y方向的航迹估计显示在直角坐标系下，见图
ex（k）
[x/k）XKk|k）]（3.1）
i1
XOY方向航迹图
3.5滤波输出xoy方向航迹图
为了真实地反映出Kalman滤波的效果，采用了Monte-Carlo方法，采用多次实验取均值的方法进行研究，可以计算出估计的误差均值和方差，其表达式为:
而误差的标准差可以表示为:
在（3.1）和（3.2）中，M就是进行Monte-Carlo仿真的次数，而k为取样点数。
当仿真的次数越多时，实验的效果越接近于实际，但是计算的速度会明显变慢。
在仿真时，需要根据实际适当选取。
在本程序中，取M50。
下面对滤波估计值的误差均值和方差进行简单分析，对x和y方向的航迹估
计进行分别讨论，假定Monte-Carlo仿真的次数M=50。
由（3.1）和（3.2式就可以求出实际滤波输出每时刻误差的均值和标准差。
3.6估计误差均值和标准差曲线
经过上面的仿真分析，可以看出Kalman滤波算法对于动态目标的跟踪有着
比较好的效果，而且可以较好地抑止环境中的噪声影响。
并且观测次数越多，滤
波估计值的误差均值和方差越接近零，即是与理论航迹越接近。
程序附录：
1.Majectory.m（产生理论航迹图）
function[X,Y]=trajectory（Ts,offtime）
%产生真实航迹[X,Y]，并在直角坐标系下显示出%Ts为雷达扫描周期，每隔Ts秒取一个观测数据%做匀速运动
ifnargin>2
error（'输入的变量过多，请检查'）;end
ifofftime<400
error（'仿真时间必须大于400s,请重新输入'）;end
x=zeros（offtime+1,1）;
y=zeros（offtime+1,1）;
X=zeros（ceil（offtime/Ts+1）,1）;
Y=zeros（ceil（offtime/Ts+1）,1）;
x0=2000;%起始点坐标y0=10000;
vx=0;
vy=-15;%沿-y方向
fort=1:
401x（t）=x0+vx*（t-1）;y（t）=y0+vy*（t-1）;
end%得到雷达的观测数据forn=0:
Ts:
offtimeX（n/Ts+1）=x（n+1）;Y（n/Ts+1）=y（n+1）;
end%显示真实轨迹
plot（X,Y,'r','LineWidth',2）,axis（[15002500200012000]）,gridon;legend（理论航迹'）;
2.Kalman_filter1.m（卡尔曼滤波估计和输出）
function[XEYE]=Kalman_filter1（Ts,offtime,d）
%Kalman_filter采用Kalman滤波方法，从观测数值中得到航迹的估计
%XE输出x轴方向上的误差
%Ts采样时间，即雷达工作周期
%offtime仿真截止时间
%d噪声的标准差值
ifnargin>4
error（'输入的变量过多，请检查'）;
end
ifofftime<400
error（'仿真时间必须大于400s,请重新输入'）;end
Pv=d*d;%噪声的功率
N=ceil（offtime/Ts）+1;%采样点数%定义系统的状态方程
Phi=[1,Ts;0,1];
C=[10];
R=Pv;
Xest=zeros（2,1）;%用前k-1时刻的输出值估计k时刻的预测值
Xfli=zeros（2,1）;%k时刻Kalman滤波器的输出值
Xes=zeros（2,1）;%预测输出误差
Xef=zeros（2,1）;%滤波后输出的误差
Pxe=zeros（2,1）;%预测输出误差均方差矩阵
Px=zeros（2,1）;%滤波输出误差均方差矩阵
XE=zeros（1,N）;%得到最终的滤波输出值,仅仅考虑距离分量
YE=zeros（1,N）;%得到最终的滤波输出值,仅仅考虑距离分量
[x,y]=trajectory（Ts,offtime）;%产生理论的航迹
fori=1:
Nvx（i）=d*randn
（1）;%观测噪声,两者独立vy（i）=d*randn
（1）;
zx（i）=x（i）+vx（i）;%实际观测值zy（i）=y（i）+vy（i）;
end
Xfli=[zx
（2）（zx
（2）-zx
（1））/Ts]';%利用前两个观测值来对初始条件进行估计Xef=[-vx
（2）（vx
（1）-vx
（2））/Ts]';
Px=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts];
fork=3:
N
Xest=Phi*Xfli;%更新该时刻的预测值
Xes=Phi*Xef;%预测输出误差
Pxe=Phi*Px*Phi';%预测误差的协方差阵
K=Pxe*C'*inv（C*Pxe*C'+R）;%Kalman滤波增益
Xfli=Xest+K*（zx（k）-C*Xest）;
Xef=（eye
（2）-K*C）*Xes-K*vx（k）;
Px=（eye
（2）-K*C）*Pxe;
XE（k）=Xfli（1,1）;
end
XE
（1）=zx
（1）;XE
（2）=zx
（2）;
figureplot（x,'r','LineWidth',2）;holdonplot（zx,'g'）;holdonplot（XE）;holdoffgridon;
legend（真实航迹'，'观测数据','滤波输出'）;
title（'X轴方向航迹图'）
Yfli=[zy
（2）（zy
（2）-zy
（1））/Ts]';%利用前两个观测值来对初始条件进行估计Yef=[-vy
（2）（vy
（1）-vy
（2））/Ts]';
Py=[Pv,Pv/Ts;Pv/Ts,2*Pv/Ts];
fork=3:
N
Yest=Phi*Yfli;%更新该时刻的预测值Yes=Phi*Yef;%预测输出误差
Pye=Phi*Py*Phi';%预测误差的协方差阵K=Pye*C'*inv（C*Pye*C'+R）;%Kalman滤波增益
Yfli=Yest+K*（zy（k）-C*Yest）;Yef=（eye
（2）-K*C）*Yes-K*vy（k）;
Py=（eye
（2）-K*C）*Pye;
YE（k）=Yfli（1,1）;
end
YE
（1）=zy
（1）;YE
（2）=zy
（2）;figure
plot（y,'r','LineWidth',2）;holdonplot（zy,'g'）;holdonplot（YE）;holdoffgridon;
legend（理论航迹'，观测数据','滤波输出'）;
title（'Y轴方向航迹图'）
figure
plot（x,y,'r','LineWidth',2）;holdon;plot（zx,zy,'g'）;holdon;plot（XE,YE）;holdoff;
axis（[15002500200012000]）,gridon;legend（理论轨迹','观测数据','滤波输出'）;title（'XOY方向航迹图'）
end
3.filter_result.m（误差分析）
function[XER,YER]=filter_result（Ts,mon,d）
%filter_result对观测数据进行卡尔曼滤波，得到预测的航迹以及估计误差的均值和标准差
%Ts采样时间，即雷达的工作周期
%mon进行Monte-Carlo仿真的次数
%d测量的误差,单位m
%返回值包括滤波预测后的估计航迹,以及均值和误差协方差
Ts=2;
mon=50;
d=100;
ifnargin>3
error（'Toomanyinputarguments.'）;endofftime=400;
%产生理论的航迹
[x,y]=trajectory（Ts,offtime）;
Pv=d*d;
N=ceil（offtime/Ts）+1;
randn（'state',sum（100*clock））;%设置随机数发生器
fori=1:
N
vx（i）=d*randn
（1）;%观测噪声，两者独立
vy（i）=d*randn
（1）;
zx（i）=x（i）+vx（i）;%实际观测值
zy（i）=y（i）+vy（i）;
end
%产生观测数据
forn=1:
mon
%用卡尔曼滤波得到估计的航迹
[XEYE]=Kalman_filter1（Ts,offtime,d）;%误差矩阵
XER（1:
N,n）=x（1:
N）-（XE（1:
N））';
YER（1:
N,n）=y（1:
N）-（YE（1:
N））';end
%滤波误差的均值XERB=mean（XER,2）;YERB=mean（YER,2）;
%滤波误差的标准差
flag='1'
XSTD=std（XER,1,2）;%计算有偏的估计值，YSTD=std（YER,1,2）;
figuresubplot（2,2,1）plot（XERB）axis（[050-5050]）
xlabel（'观测次数'）
ylabel（'X方向滤波误差均值'）,gridon;subplot（2,2,2）plot（YERB）axis（[050-5050]）
xlabel（'观测次数'）
ylabel（'Y方向滤波误差均值'）,gridon;subplot（2,2,3）plot（XSTD）axis（[0500150]）
xlabel（'观测次数'）
ylabel（'X方向滤波误差标准值'）,gridon;subplot（2,2,4）plot（YSTD）axis（[0500150]）
xlabel（'观测次数'）
ylabel（'Y方向滤波误差标准值'）,gridon;
X=XER;Y=YER;
升级会员 