miv.m

一个交叉定位算法的matlab程序

function bistaticPLEunbias
clc
clear
T=1;
times=1;
sum=zeros(100,1);
sum1=zeros(100,1);
sum2=zeros(100,1);

z=zeros(200,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 0 0]';%浮标1状态向量
    xri2=[0 0 0 0]';
    xrt=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';%目标状态向量
    for i=1:100
        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;%浮标1运动状态向量
        Oistate2=pha*xri2;
        Tistate=pha*xrt;%目标运动状态向量
%%%%%%%%%%%%%CRLBCRLBCRLB%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        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/(0.1*pi/180)^2+b2'*b2/(0.1*pi/180)^2;
        FF=inv(FIM);
        mse(i,:)=sqrt(FF(1,1)+FF(2,2));%%%%%%误差CRLB
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        
        V1=(0.1*pi/180)*randn;%噪声
        V2=(0.1*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(t)
        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;

        theta(:,i)=inv(A([1:2*i],:)'*A([1:2*i],:))*A([1:2*i],:)'*z([1:2*i],:);
        r11=sqrt((Tistate(1)-Oistate1(1,1))^2+(Tistate(2)-Oistate1(2,1))^2);
        r22=sqrt((Tistate(1)-Oistate2(1,1))^2+(Tistate(2)-Oistate2(2,1))^2);
        w(2*i-1,2*i-1)=r11^2*(0.1*pi/180)^2;
        w(2*i,2*i)=r22^2*(0.1*pi/180)^2;
        betam11(i,:)=beta1+V1;%atan((Tistate(1)+i*T*theta(3,1)-Oistate1(1))/(Tistate(2)+i*T*theta(4,1)-Oistate1(2)));
        betam22(i,:)=beta2+V2;%atan((Tistate(1)+i*T*theta(3,1)-Oistate2(1))/(Tistate(2)+i*T*theta(4,1)-Oistate2(2)));
        mmse(i,:)=sqrt((theta(1,i)-Tistate(1))^2+(theta(2,i)-Tistate(2))^2);%%%%%%误差
        mmmse(i,:)=sqrt((theta(1,i)-Tistate(1))^2+(theta(2,i)-Tistate(2))^2);%%%%%%误差
        A1([2*i-1,2*i],:)=H*Ak;
        if i>1
            u=0.01;%调节系数
        theta(:,1)=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';    
            %if i<4
        theta1(:,i)=theta(:,i-1)+inv(A([1:2*i-2],:)'*A([1:2*i-2],:)+u*eye(size(A([1:2*i-2],:)'*A([1:2*i-2],:))))*A([1:2*i-2],:)'*z([1:2*i-2],:);
         theta1(:,1)=[1500 15000 10.29*sin(alpha) 10.29*cos(alpha)]';
        %   else
         %   theta1=TH+inv(A1([1:2*i-2],:)'*A1([1:2*i-2],:)+u*eye(size(A1([1:2*i-2],:)'*A1([1:2*i-2],:))))*A1([1:2*i-2],:)'*z([1:2*i-2],:);
         %   end
        betam11(i,:)=atan((theta(1,1)+(i)*T*theta1(3,i)-Oistate1(1))/(theta(2,1)+(i)*T*theta1(4,i)-Oistate1(2)));
        betam22(i,:)=atan((theta(1,1)+(i)*T*theta1(3,i)-Oistate2(1))/(theta(2,1)+(i)*T*theta1(4,i)-Oistate2(2)));
        H1=[cos(betam11(i,:)) -sin(betam11(i,:)) 0 0;cos(betam22(i,:)) -sin(betam22(i,:)) 0 0];
        A1([2*i-1,2*i],:)=H1*Ak;
        r1t=sqrt((theta(1,1)+i*theta1(3,i)*T-Oistate1(1,1))^2+(theta(2,1)+i*theta1(4,i)*T-Oistate1(2,1))^2);
        r2t=sqrt((theta(1,1)+i*theta1(3,i)*T-Oistate2(1,1))^2+(theta(2,1)+i*theta1(4,i)*T-Oistate2(2,1))^2);
        w(2*i-1,2*i-1)=r1t^2*(0.1*pi/180)^2;
        w(2*i,2*i)=r2t^2*(0.1*pi/180)^2;
        TH=inv((inv(w([1:2*i],[1:2*i]))*A1([1:2*i],:))'*A([1:2*i],:))*(inv(w([1:2*i],[1:2*i]))*A1([1:2*i],:))'*z([1:2*i],:);
        mmse(i,:)=sqrt((TH(1)-Tistate(1))^2+(TH(2)-Tistate(2))^2);%%%%%%误差
        end
        
    end
    %theta=0;A=0;z=0;
     sum=sum+mmse;
     sum1=sum1+mse;
     sum2=sum2+mmmse;
end
re=sum/times;
re1=sum1/times;
re11=sum2/times;
t=1:100;
plot(t,re,t,re1,'r',t,re11,'g');