Kalmanfilter仿真作业Word下载.docx
- 文档编号:13980586
- 上传时间:2022-10-16
- 格式:DOCX
- 页数:12
- 大小:138.16KB
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
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- Kalmanfilter 仿真 作业