ukf.m

本程序是用matlab编写的UKF的滤波程序,并且运行通过.

function [ee1,ee2,ee3,ee4]=ukf
[B,F,G,X,Xt]=track;
xc(:,1)=[200 1.3 1500 -0.3];   %状态初值
%mn1 = mean(xc);                                   %FIRST STEP
%CV1 = (xc-mn1)*(xc-mn1)';
%CV1 = (Xo)*(Xo)';
%p=CV1;
p=[0.005 0 0 0;0 0.005 0 0;0 0 0.005 0;0 0 0 0.005];    %状态误差协方差初值
n=4;
%-------------------------
alpha=1;                                    %default, tunable
ki=-1;                                      %default, tunable
beta=2;                                     %default, tunable
lambda=alpha^2*(n+ki)-n;                    %scaling factor
cc=n+lambda;                                %scaling factor
Wm=[lambda/cc 0.5/cc+zeros(1,2*n)];         %weights for means
Wc=Wm;
Wc(1)=Wc(1)+(1-alpha^2+beta);               %weights for covariance
cc=sqrt(cc);
%-------------------
sxk=0;pp=0;szk=0;pzz=0;pxz=0;
%-------------------
for j=1:999
     pk=cc*chol(p);% B=chol(A);meant:A'*A=B;
     sigma=[xc(:,j) xc(:,j)+pk(:,1) xc(:,j)+pk(:,2) xc(:,j)+pk(:,3) xc(:,j)+pk(:,4)... 
            xc(:,j)-pk(:,1) xc(:,j)-pk(:,2) xc(:,j)-pk(:,3) xc(:,j)-pk(:,4)];
    %---------------------
    for ks=1:2*n+1
       sigma(:,ks)=F*sigma(:,ks);
       sxk=Wm(ks)*sigma(:,ks)+sxk;
    end 
    %---------------------
    for kp=1:2*n+1
        pp=Wc(kp)*(sigma(:,kp)-sxk)*(sigma(:,kp)-sxk)'+pp;   %到此完成对Pk的估计
    end
    pp=pp+G*0.005*G';
    %-----------------------
     %pkk=cc*chol(pp);% B=chol(A);meant:A'*A=B;
     %sigma1=[sxk sxk+pkk(:,1) sxk+pkk(:,2) sxk+pkk(:,3) sxk+pkk(:,4) sxk-pkk(:,1) sxk-pkk(:,2) sxk-pkk(:,3)...
     %    sxk-pkk(:,4)];
    %---------------------
    for kz=1:2*n+1
        zk(kz)=atan(sigma(3,kz)/sigma(1,kz));
        %zk(JJ)=atan(sigma1(2,JJ)/sigma1(1,JJ));
    end
    for JJs=1:2*n+1
        szk=szk+Wm(JJs)*zk(JJs);
    end
    %-----------------------
    for kpz=1:2*n+1
        pzz=Wc(kpz)*(zk(kpz)-szk)*(zk(kpz)-szk)'+pzz;
    end
    pzz=pzz+0.008;
    for kpx=1:2*n+1
        pxz=Wc(kpx)*(sigma(:,kpx)-sxk)*(zk(kpx)-szk)'+pxz;
    end
    kgs=pxz/pzz;
    xc(:,j+1)=sxk+kgs*(B(j+1)-szk);
    p=pp-kgs*pzz*kgs';
    sxk=0;szk=0;pzz=0;pxz=0;pp=0;
    %tt=tt+0.1;
end   
%------------------
for ie=1:1000
    ae(ie)=Xt(1,ie)-xc(1,ie);
    be(ie)=Xt(2,ie)-xc(2,ie);
    ce(ie)=Xt(3,ie)-xc(3,ie);
    de(ie)=Xt(4,ie)-xc(4,ie);
end
plot(1:1000,Xt(1,1:1000),'b',1:1000,X(1,1:1000),'k',1:1000,real(xc(1,1:1000)),'r');
%plot(1:1000,B)
%plot(X(1,:),X(3,:)) 
%hold on
%plot(xkk(1,:),xkk(3,:))