用α-β滤波器结合卡尔曼滤波器对相位差及其变化率数据进行处理

用α-β滤波器结合卡尔曼滤波器对相位差及其变化率数据进行处理，提高数据的测量精度，为机载单站无源定位提供高精度数据，给出了误差均方差作为精度指标。

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