Kalmanfilter仿真作业.docx
《Kalmanfilter仿真作业.docx》由会员分享,可在线阅读,更多相关《Kalmanfilter仿真作业.docx(11页珍藏版)》请在冰豆网上搜索。
![Kalmanfilter仿真作业.docx](https://file1.bdocx.com/fileroot1/2023-1/26/b416b087-87df-40d3-bc3e-281a03a7e0d9/b416b087-87df-40d3-bc3e-281a03a7e0d91.gif)
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;