www.pudn.com > UKFa.rar > UKFa.m, change:2010-01-22,size:2430b


%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 
%%%%%%%%平台A的UKF 
function [ Xa_gj Pa_gj ]=UKFa( NT,step,t,Fai_a,Ta,RTa,Xa,x0,p0,Za,QQa,D,Ra ) 
%%%%%UKF权重计算%%%%% 
Wx(1)=(3-4)/3; 
Wx(2:9)=1/6; 
Wz(1)=(3-2)/3; 
Wz(2:5)=1/6; 
for k=1:step/3 
    for i=1:NT 
        r(:,:,i)=chol(3*p0(:,:,i)); 
        x_yc(:,i)=[0;0;0;0]; 
        Px(:,:,i)=zeros(4,4); 
        z_yc(:,i)=[0;0]; 
        Pz(:,:,i)=zeros(2,2); 
        Pxz(:,:,i)=zeros(4,2); 
    end 
    for i=1:NT 
        if k==1 
            %%%%%%%%%%%%%%%%%%%%状态计算sigma点%%%%%%%%%%%%%%%%%% 
            for j=1:4 
                xgamaP1(:,j,i)=RTa*x0(:,i)+Ta+r(:,j,i);  %%%%%%对目标i 
                xgamaP2(:,j,i)=RTa*x0(:,i)+Ta-r(:,j,i); 
            end 
            Xsigma(:,:,i)=[RTa*x0(:,i)+Ta xgamaP1(:,:,i) xgamaP2(:,:,i)]; %%%%%%对目标i 
        else 
            for j=1:4 
                xgamaP1(:,j,i)=Xa(:,k-1,i,t)+r(:,j,i);  %%%%%%对目标i 
                xgamaP2(:,j,i)=Xa(:,k-1,i,t)-r(:,j,i);  %%%%%%对目标i 
            end 
            Xsigma(:,:,i)=[Xa(:,k-1,i,t) xgamaP1(:,:,i) xgamaP2(:,:,i)]; %%%%%%对目标i 
        end 
        X(:,:,i)=Fai_a*Xsigma(:,:,i); 
        for j=1:9 
            x_yc(:,i)=x_yc(:,i)+Wx(j)*X(:,j,i);    %%%%%%%%预测值%%% 
        end 
        for j=1:9 
            Px(:,:,i)=Px(:,:,i)+Wx(j)*(X(:,j,i)-x_yc(:,i))*(X(:,j,i)-x_yc(:,i))'; 
        end 
        %%%%%%%%%%%%%%%%%%%%量测采样计算sigma点%%%%%%%%%%%%%%%%%% 
        y(:,:,i)=chol(3*Px(:,:,i)); 
        for j=1:2 
            zgamaP1(:,j,i)=x_yc(:,i)+y(:,j,i);  %%%%%%对目标i 
            zgamaP2(:,j,i)=x_yc(:,i)-y(:,j,i);  %%%%%%对目标i 
        end 
        Zsigma(:,:,i)=[x_yc(:,i) zgamaP1(:,:,i) zgamaP2(:,:,i)]; 
        for j=1:5 
            Z(:,j,i)=[acos(Zsigma(1,j,i)/sqrt(Zsigma(1,j,i)^2+Zsigma(3,j,i)^2));acos((Zsigma(1,j,i)-D)/sqrt((Zsigma(1,j,i)-D)^2+Zsigma(3,j,i)^2))]; 
            z_yc(:,i)=z_yc(:,i)+Wz(j)*Z(:,j,i);  %%%%%%量测的一步预测 
        end 
        for j=1:5 
            Pz(:,:,i)=Pz(:,:,i)+Wz(j)*(Z(:,j,i)-z_yc(:,i))*(Z(:,j,i)-z_yc(:,i))'; 
            Pxz(:,:,i)=Pxz(:,:,i)+Wz(j)*(X(:,j,i)-x_yc(:,i))*(Z(:,j,i)-z_yc(:,i))'; 
        end 
        Px(:,:,i)=Px(:,:,i)+QQa(:,:,i); 
        Pz(:,:,i)=Pz(:,:,i)+Ra*Ra; 
        K(:,:,i)=Pxz(:,:,i)*inv(Pz(:,:,i)); 
        Xa_gj(:,k,i,t)=x_yc(:,i)+K(:,:,i)*(Za(:,k,i,t)-z_yc(:,i)); 
        Pa_gj(:,:,k,i,t)=Px(:,:,i)-K(:,:,i)*Pz(:,:,i)*K(:,:,i)'; 
    end 
end