简单的导航仿真源码程序

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');