clcclear
T=1;%觀測間隔時間
var1=0.0247;%相位差變化率數據精度
var=0.0175;%相位差數據精度
ro=4e+5;
ho=1e+4;

time=400;%偵察時間
N=time/T;
xt=107.15e+3;?yt=399.88e+3;%目標位置
v=300;%飛機速度
dx=10;%基線長度
f=3e+9;%載頻
c=3e+8;
k=2*pi*dx/c;
deta_phi=0.001;
lamda=T^2*deta_phi/var1;
afa=-（lamda^2+8*lamda-（lamda+4）*sqrt（lamda^2+8*lamda））/8;
beita=（lamda^2+4*lamda-lamda*sqrt（lamda^2+8*lamda））/4;

K=[afa;?beita/T];
num=100;%蒙特卡洛次數

%=======================================相位差真實值
for?i=1:N
????sinbeta（i）=（xt-v*i）/sqrt（（v*i-xt）^2+yt^2）;
????phix_true（i）=k*f*sinbeta（i）;
end
%=======================================================



%=========================================相位差變化率真實值
for?i=1:N
????beta（i）=atan（（xt-v*i）/sqrt（ro^2-ho^2））;
????beta1（i）=sqrt（ro^2-ho^2）*v/（ro^2-ho^2+（xt-v*i）^2）;
????phi（i）=-k*f*beta1（i）*sqrt（1-（beta（i）^2））;
end
%========================================================
%========================================先用差分法求變化率
for?i=1:N
????sinbeta（i）=（xt-v*i）/sqrt（（v*i-xt）^2+yt^2）;
????phix（i）=k*f*sinbeta（i）+var*randn（11）;
end

for?i=2:N
????fi1（i）=（phix（i）-phix（i-1））/（i*T-（i-1）*T）;
end
fi1（1）=fi1（2）;
%===================================================

F=[1?T;0?1];
C=[1?0];
B=[（T^2）/2;?T];
x_estimate=[phix（1）;?fi1（1）];%濾波初值
%==========================================濾波處理
for?i=1:num???%先用α-β濾波器處理
????for?j=1:N
????????X1=F*x_estimate;
????????
????????v=phix（j）-C*X1;
????????
????????x_estimate=X1+K*v;
????????rex（ij）=x_estimate（11）;
????????rey（ij）=x_estimate（21）;
????end
????x_estimate=[rex（i1）;rey（i1）];
end

x_estimate=[rex（num1）;rey（num1）];
p_estimate=[var?0;0?var1];
for?i=1:num????%再用卡爾曼濾波處理
????A=[1?T;0?1];
????C=[1?0];
????B=[T;?1];
????deta_phi=0.001;
????for?j=1:N
????????X1=A*x_estimate;
????????P1=A*p_estimate*A‘+B*deta_phi*B‘;
????????
????????K=P1*C‘*（C*P1*C‘+var）^（-1）;
????????
????????x_estimate=X1+K*（phix（j）-C*X1）;
????????p_estimate=（eye（2）-K*C）*P1;
????????rex（ij）=x_estimate（11）;%相位差
????????rey（ij）=x_estimate（21）;%相位差變化率
????????pex（ij）=p_estimate（11）;
????????pey（ij）=p_estimate（22）;
????end
????x_estimate=[rex（i1）;rey（i1）];
????p_estimate=[pex（i1）?0;0?pey（i1）];
end

%========================求相位差及其變化率的均方差
for?j=1:N
????ex=0;ey=0;
????for?i=1:num
????????ex=ex+（rex（ij）-phix_true（j））^2;
????????ey=ey+（rey（ij）-phi（j））^2;
????end
????sigma_pd（j）=sqrt（ex/num）;
????sigma_prc（j）=sqrt（ey/num）;
end
%=============================================================


t=1:N;
figure（1）
plot（tsigma_pd）
axis（[0?400?-0.0006?0.06]）
xlabel（‘time（s）‘）
ylabel（‘相位差誤差均方差rad‘）

figure（2）
plot（tsigma_prc）
axis（[0?400?-0.0006?0.03]）
xlabel（‘time（s）‘）
ylabel（‘相位差變化率誤差均方差rad/s‘）

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----
?????文件????????2878??2020-07-27?20:34??afa_beta_kalman_prc_var.m