Kalmanfilter仿真作业Word下载.docx
《Kalmanfilter仿真作业Word下载.docx》由会员分享，可在线阅读，更多相关《Kalmanfilter仿真作业Word下载.docx（12页珍藏版）》请在冰豆网上搜索。
而误差协方差矩阵为，
3.仿真计算与结果
首先给出理论航迹，
3.1理论航迹图
由图3.1可以看出目标运动的理论航迹，即沿y轴负方向直线运动。
假定观察噪声的标准差均为100m，可以给出目标的观测数据和理论航迹的对比图，见图3.2。
3.2观测数据和理论航迹对比图
接下来经过Kalman滤波后，x轴方向的航迹图，见图3.3。
3.3滤波输出x轴方向航迹图
y轴方向的航迹图，见图3.4。
3.4滤波输出y轴方向航迹图
将输出的x和y方向的航迹估计显示在直角坐标系下，见图3.5。
3.5滤波输出xoy方向航迹图
为了真实地反映出Kalman滤波的效果，采用了Monte-Carlo方法，采用多次实验取均值的方法进行研究，可以计算出估计的误差均值和方差，其表达式为：
（3.1）
而误差的标准差可以表示为：
（3.2）
在（3.1）和（3.2）中，就是进行Monte-Carlo仿真的次数，而为取样点数。
当仿真的次数越多时，实验的效果越接近于实际，但是计算的速度会明显变慢。
在仿真时，需要根据实际适当选取。
在本程序中，取。
下面对滤波估计值的误差均值和方差进行简单分析，对x和y方向的航迹估计进行分别讨论，假定Monte-Carlo仿真的次数＝50。
由（3.1）和（3.2）式就可以求出实际滤波输出每时刻误差的均值和标准差。
3.6估计误差均值和标准差曲线
经过上面的仿真分析，可以看出Kalman滤波算法对于动态目标的跟踪有着比较好的效果，而且可以较好地抑止环境中的噪声影响。
并且观测次数越多，滤波估计值的误差均值和方差越接近零，即是与理论航迹越接近。
程序附录：
1.trajectory.m（产生理论航迹图）
function[X,Y]=trajectory（Ts,offtime）
%产生真实航迹[X,Y]，并在直角坐标系下显示出
%Ts为雷达扫描周期，每隔Ts秒取一个观测数据
%做匀速运动
ifnargin>
2
error（'
输入的变量过多，请检查'
）;
end
ifofftime<
400
仿真时间必须大于400s，请重新输入'
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:
401
x（t）=x0+vx*（t-1）;
y（t）=y0+vy*（t-1）;
%得到雷达的观测数据
forn=0:
Ts:
offtime
X（n/Ts+1）=x（n+1）;
Y（n/Ts+1）=y（n+1）;
%显示真实轨迹
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噪声的标准差值
4
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:
N
vx（i）=d*randn
（1）;
%观测噪声，两者独立
vy（i）=d*randn
（1）;
zx（i）=x（i）+vx（i）;
%实际观测值
zy（i）=y（i）+vy（i）;
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:
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）;
XE
（1）=zx
（1）;
XE
（2）=zx
（2）;
figure
plot（x,'
2）;
holdon
plot（zx,'
g'
plot（XE）;
holdoff
gridon;
legend（'
真实航迹'
观测数据'
滤波输出'
title（'
X轴方向航迹图'
）
Yfli=[zy
（2）（zy
（2）-zy
（1））/Ts]'
Yef=[-vy
（2）（vy
（1）-vy
（2））/Ts]'
Py=[Pv,Pv/Ts;
Yest=Phi*Yfli;
Yes=Phi*Yef;
Pye=Phi*Py*Phi'
K=Pye*C'
*inv（C*Pye*C'
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）;
YE
（1）=zy
（1）;
YE
（2）=zy
（2）;
plot（y,'
plot（zy,'
plot（YE）;
Y轴方向航迹图'
plot（x,y,'
holdon;
plot（zx,zy,'
plot（XE,YE）;
holdoff;
axis（[15002500200012000]）,gridon;
理论轨迹'
title（'
XOY方向航迹图'
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;
3
error（'
Toomanyinputarguments.'
offtime=400;
%产生理论的航迹
randn（'
state'
sum（100*clock））;
%设置随机数发生器
%产生观测数据
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:
%滤波误差的均值
XERB=mean（XER,2）;
YERB=mean（YER,2）;
%滤波误差的标准差
XSTD=std（XER,1,2）;
%计算有偏的估计值，flag='
1'
YSTD=std（YER,1,2）;
subplot（2,2,1）
plot（XERB）
axis（[050-5050]）
xlabel（'
观测次数'
ylabel（'
X方向滤波误差均值'
）,gridon;
subplot（2,2,2）
plot（YERB）
Y方向滤波误差均值'
subplot（2,2,3）
plot（XSTD）
axis（[0
升级会员 