bistaticpleunbias.m

纯方位定位算法

function bistaticPLEunbias
T=1;
times=50;
sum=zeros(300,1);
sum1=zeros(300,1);

z=zeros(600,1);
for m=1:times
    xri1=zeros(4,1);
    xri2=zeros(4,1);
    alpha=120*pi/180;%%%%target direction
    alpha1=330*pi/180;
    xri1=[-1000 0 3.09*sin(alpha1) 3.09*cos(alpha1)]';
    xri2=[-500 0 3.09*sin(alpha1) 3.09*cos(alpha1)]';
    xrt=[0 20000 10.29*sin(alpha) 10.29*cos(alpha)]';
    for i=1:300
        pha=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];  %%%state transfer matrix
        Oistate1=pha*xri1;
        Oistate2=pha*xri2;
        Tistate=pha*xrt;
        V1=(2*pi/180)*randn;
        V2=(2*pi/180)*randn;
        beta1=atan((Tistate(1)-Oistate1(1))/(Tistate(2)-Oistate1(2)));
        beta2=atan((Tistate(1)-Oistate2(1))/(Tistate(2)-Oistate2(2)));
        betam1(i,:)=beta1+V1;%%%measure
        betam2(i,:)=beta2+V2;%%%measure
        H=[cos(betam1(i,:)) -sin(betam1(i,:)) 0 0;cos(betam2(i,:)) -sin(betam2(i,:)) 0 0];
        Ak=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];
        z(2*i-1,:)=H(1,:)*Oistate1;% mathematic measure
        z(2*i,:)=H(2,:)*Oistate2;% mathematic measure
        %A*iK=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];
        A([2*i-1,2*i],:)=H*Ak;
        Au=[A([1:2*i],:),-z([1:2*i],:)];
        w=zeros(5,5);
        for k=1:i
            ui1=[sin(betam1(k,:)),cos(betam1(k,:)),k*sin(betam1(k,:)),k*cos(betam1(k,:)),-Oistate1(1)*sin(betam1(k,:))-Oistate1(2)*cos(betam1(k,:))]';
            ui2=[sin(betam2(k,:)),cos(betam2(k,:)),k*sin(betam2(k,:)),k*cos(betam2(k,:)),-Oistate2(1)*sin(betam2(k,:))-Oistate2(2)*cos(betam2(k,:))]';
            u1=ui1*ui1';
            u2=ui2*ui2';
            w=w+u1+u2;
        end
        AAu=Au'*Au;
        
        [V,D]=eig(AAu,w);
        [C,I]=max(D);
        [C1,I1]=min(C);
        VV=V(:,I1);
        Xe0=[VV(1),VV(2),VV(3),VV(4)]'/VV(5);
        %Xe0=pinv(A([1:2*i],:))*z([1:2*i],:);
        Ak1=[1 0 i*T 0;0 1 0 i*T;0 0 1 0;0 0 0 1];
            Xe=Ak1*Xe0;
            mmse(i,:)=sqrt((Xe(1)-Tistate(1))^2+(Xe(2)-Tistate(2))^2);
             %xrt1=sqrt(Tistate(1)^2+Tistate(2)^2);
        %mmse(i,:)=abs(xet-xrt1);
        %%%Fisher matrix computation
        TT(:,i)=Tistate;
        O1(:,i)=Oistate1;
        O2(:,i)=Oistate2;
        r1(i,1)=(Tistate(1)-Oistate1(1))^2+(Tistate(2)-Oistate1(2))^2;
        r2(i,1)=(Tistate(1)-Oistate2(1))^2+(Tistate(2)-Oistate2(2))^2;
        for l=1:i
             b1(l,:)=[(TT(2,l)-O1(2,l))/r1(l),-(TT(1,l)-O1(1,l))/r1(l),-(i-l)*(TT(2,l)-O1(2,l))/r1(l),(i-l)*(TT(1,l)-O1(1,l))/r1(l)];
             b2(l,:)=[(TT(2,l)-O2(2,l))/r2(l),-(TT(1,l)-O2(1,l))/r2(l),-(i-l)*(TT(2,l)-O2(2,l))/r2(l),(i-l)*(TT(1,l)-O2(1,l))/r2(l)];

           
        end
        FIM=b1'*b1/(2*pi/180)^2+b2'*b2/(2*pi/180)^2;
        FF=inv(FIM);
        mse(i,:)=sqrt(FF(1,1)+FF(2,2));
                
    end
     sum=sum+mmse;
     sum1=sum1+mse;
end


re=sum/times;
re1=sum1/times;
t=1:300;
plot(t,re,t,re1,'r');
iii=20;