MATLAB实现 之 扩展卡尔曼滤波算法

clear all
v=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/2
0 t ];

a=35*pi/180;
a_v=5/100;

a_sensor=45*pi/180;
x(1)=8000; %%初始位置
y(1)=12000;

for i=1:200
x(i+1)=x(i)+v*cos(a)*t;
y(i+1)=y(i)+v*sin(a)*t; 
end


for i=1:200 
xradarpositon=0;
yradarpositon=0;

Zmeasure(1,i)=atan((y(i)-yradarpositon)/(x(i)-xradarpositon))+random('Normal',0,azimutherror,1,1); 
Zme