基于扩展卡尔曼滤波算法的matlab程序

扩展卡尔曼滤波原理:在原有卡尔曼滤波的基础上,为了解决多目标值的跟踪与估量，形成了扩展卡尔曼滤波。起 matlab 主要程序如下：clear allv=150； ％％目标速度v_sensor=0;%%传感器速度t=1； ％％扫描周期xradarpositon=0； ％％传感器坐标yradarpositon=0; ％%ppred=zeros（4,4)；Pzz=zeros(2，2)；Pxx=zeros(4,2);xpred=zeros(4,1）；ypred=zeros（2,1）；sumx=0；sumy=0；sumxukf=0;sumyukf=0;sumxekf=0;sumyekf=0； ％%%统计的初值％滤波算法描述:L=4;alpha=1；kalpha=0;belta=2；ramda=3-L;azimutherror=0.015； ％％方位均方误差rangeerror=100; ％％距离均方误差processnoise=1； %％过程噪声均方差tao=［t^3/3 t^2/2 0 0;t^2/2 t 0 0；0 0 t^3/3 t^2/2;0 0 t^2/2 t]; ％% the input matrix of process G=［t^2/2 0 t 0 0 t^2/20 t ］;a=35＊pi/180；a_v=5/100;a_sensor=45＊pi/180；x（1）=8000； ％％初始位置y(1)=12000;for i=1：200x(i+1)=x(i）+v＊cos（a)*t；y(i+1)=y(i)+v＊sin（a)＊t; endfor i=1：200 xradarpositon=0；yradarpositon=0;Zmeasure（1，i)=atan（(y(i）—yradarpositon)/（x(i）—xradarpositon)）+random（’Normal’,0，azimutherror,1，1）; Zmeasure（2,i)=sqrt（（y（i）-yradarpositon)^2+(x（i)—xradarpositon）^2)+random（’Normal',0，rangeerror,1,1); xx(i)=Zmeasure(2,i）＊cos(Zmeasure(1,i)）;％％观测值yy(i)=Zmeasure(2,i)＊sin(Zmeasure（1,i)); measureerror=［azimutherror^2 0；0 rangeerror^2］；processerror=tao＊processnoise；vNoise = size（processerror,1)；wNoise = size(measureerror,1);A=［1 t 0 0；0 1 0 0；0 0 1 t;0 0 0 1］;Anoise=size（A,1）；for j=1:2*L+1Wm（j)=1/(2＊(L+ramda））;Wc（j)=1/(2＊(L+ramda））;endWm(1)=ramda/(L+ramda)；Wc（1）=ramda/（L+ramda)；％+1—alpha^2+belta; %%％权值if i==1 xerror=rangeerror^2*cos(Zmeasure（1，i）)^2+Zmeasure（2，i)^2＊azimutherror^2*sin(Zmeasure(1，i）)^2；yerror=rangeerror^2＊sin(Zmeasure（1，i)）^2+Zmeasure（2，i)^2*azimutherror^2＊cos（Zmeasure(1,i)）^2;xyerror=（rangeerror^2-Zmeasure(2,i)^2＊azimutherror^2）＊sin（Zmeasure（1，i))＊cos（Zmeasure（1，i))；P=[xerror xerror/t xyerror xyerror/t;xerror/t 2＊xer...