ca_ukf_1wei_extend_state.m

基于常加速模型的目标跟踪算法

%CA_UKF_1维     X=[x xv xa W V] 状态扩维     比例修正
clear all;    clc;
T=0.1;   N=5;   r=100; 
n=5;alpha=0.01;beta=2;l=0;lamda=alpha^2*(n+l)-n;%alpha=0.775;
F=[1 T T^2/2 0 0;0 1 T 0 0;0 0 1 0 0;0 0 0 1 0;0 0 0 0 1];
G=[T^3/6;T^2/2;T;0;0];
H=[1 0 0 0 0];    Q=0.1;    R=r^2;
%模拟目标真实运动轨迹
x0=50000; v0=-150;
a1x=-5;   a2x=60;a3x=-25;
t1=50;    K1=t1/T;
t2=100;   K2=(t2-t1)/T;
t3=150;   K3=(t3-t2)/T;
t4=155;   K4=(t4-t3)/T;
t5=200;   K5=(t5-t4)/T;
t6=220;   K6=(t6-t5)/T;
t7=250;   K7=(t7-t6)/T;
% 0---50s，匀速运动
k1=1:K1;
x1=x0+v0*(k1*T);
v1=v0*ones(1,length(k1));
a1= 0*ones(1,length(k1));
% 50---100s，慢加速
k2=1:K2;
x2=x1(K1)+v1(K1)*(k2*T)+a1x*(k2*T).^2/2;
v2=v1(K1)+a1x*(k2*T);
a2=a1x*ones(1,length(k2));
% 100---150s，匀速运动
k3=1:K3;
x3=x2(K2)+v2(K2)*(k3*T);
v3=v2(K2)*ones(1,length(k3));
a3= 0*ones(1,length(k3));
% 150---155s，快减速
k4=1:K4;
x4=x3(K3)+v3(K3)*(k4*T)+a2x*(k4*T).^2/2;
v4=v3(K3)+a2x*(k4*T);
a4=a2x*ones(1,length(k4));
% 155---200s，匀速运动
k5=1:K5;
x5=x4(K4)+v4(K4)*(k5*T);
v5=v4(K4)*ones(1,length(k5));
a5=0*ones(1,length(k5));
% 200---220s，中加运动
k6=1:K6;
x6=x5(K5)+v5(K5)*(k6*T)+a3x*(k6*T).^2/2;
v6=v5(K5)+a3x*(k6*T);
a6=a3x*ones(1,length(k6));
% 220---250s，匀速运动
k7=1:K7;
x7=x6(K6)+v6(K6)*(k7*T);
v7=v6(K6)*ones(1,length(k7));
a7=0*ones(1,length(k7));

x=[x1 x2 x3 x4 x5 x6 x7];
v=[v1 v2 v3 v4 v5 v6 v7];
a=[a1 a2 a3 a4 a5 a6 a7];
K=K1+K2+K3+K4+K5+K6+K7;
Wm0=lamda/(lamda+n);Wc0=lamda/(lamda+n)+(1-alpha^2+beta);
Wi=0.5/(lamda+n)*ones(1,10);
Wm=[Wm0 Wi];
Wc=[Wc0 Wi];
xx=zeros(1,K);    zz=zeros(1,K);  
vv=zeros(1,K);    aa=zeros(1,K);
ex1=zeros(1,K);   ex2=zeros(1,K);
ev1=zeros(1,K);   ev2=zeros(1,K);
ea1=zeros(1,K);   ea2=zeros(1,K);
%进行N次仿真
for i=1:N
    %产生观测数据
    zx=randn(1,K)*r+x;  
    zz=zz+zx;
    Z=zx;    
    %两点起始法确定初值
    LX=[zx(2) (zx(2)-zx(1))/T 0]';
    LP=[r r/T 0;r/T 2*r/T^2 0;0 0 0];
    %画图所需量初值
    xx(1)=xx(1)+zx(1); 
    ex1(1)=ex1(1)+x(1)-zx(1);    ex2(1)=ex2(1)+(x(1)-zx(1))^2; 
    ev1(1)=ev1(1)+0;    ev2(1)=ev2(1)+(0)^2; 
    ea1(1)=ea1(1)+0;    ea2(1)=ea2(1)+(0)^2; 
    LX=[zx(2) (zx(2)-zx(1))/T 0 0 0]';
    LP=[r r/T 0 0 0;r/T 2*r/T^2 0 0 0;0 0 0 0 0;0 0 0 Q 0;0 0 0 0 R];
    %开始滤波
    for k=2:K
        xx(k)=xx(k)+LX(1);vv(k)=vv(k)+LX(2);aa(k)=aa(k)+LX(3);
        ex1(k)=ex1(k)+x(k)-LX(1);    
        ex2(k)=ex2(k)+(x(k)-LX(1))^2; 
        ev1(k)=ev1(k)+v(k)-LX(2);    
        ev2(k)=ev2(k)+(v(k)-LX(2))^2; 
        ea1(k)=ea1(k)+a(k)-LX(3);    
        ea2(k)=ea2(k)+(a(k)-LX(3))^2; 
        %%%%%%%%%%%%%%%%%%%%  一步预测   %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %求 UT 点
        KeSeiF=LX;
        temp=((n+lamda)*LP)^0.5;
        for i=1:1:5            
            KeSeiF=cat(2,KeSeiF,LX+temp(:,i));    
        end
        for i=6:1:10            
            KeSeiF=cat(2,KeSeiF,LX-temp(:,i-5)); 
        end
        % UT通过状态方程传播
        KeSeiPre=F*KeSeiF;
        XPre=KeSeiPre*Wm';            %X的一步预测
        PTemp=zeros(5,5);
        for i=2:1:11
            PTemp=PTemp+Wc(i)*(KeSeiPre(:,i)-XPre)*(KeSeiPre(:,i)-XPre)';
        end
        PTemp=PTemp+Wc(1)*(KeSeiPre(:,1)-XPre)*(KeSeiPre(:,1)-XPre)';   
        PxPre=PTemp+G*Q*G';           %P的一步预测
        %%%%%%%%%%%%%%%%%%%%%%%%%%%   UT通过量测方程传播    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %% 一步预测通过量测方程传播
        KeSeiPre2=XPre;
        temp=((n+lamda)*PxPre)^0.5;
        for i=1:1:5           
           KeSeiPre2=cat(2,KeSeiPre2,XPre+temp(:,i));    
        end
        for i=6:1:10
           KeSeiPre2=cat(2,KeSeiPre2,XPre-temp(:,i-5));  
        end
        %输出一步预测
        KeSeiPre3=H*KeSeiPre2;
        
        ZPre=KeSeiPre3*Wm';        %%  Z的一步预测
        PzChaTemp=0;
        for i=2:1:11
            PzChaTemp=PzChaTemp+Wc(i)*(KeSeiPre3(:,i)-ZPre)*(KeSeiPre3(:,i)-ZPre)';
        end
        PzChaTemp=PzChaTemp+Wc(1)*(KeSeiPre3(:,1)-ZPre)*(KeSeiPre3(:,1)-ZPre)';
        PzCha=PzChaTemp+R+(KeSeiPre3(:,i)-ZPre)*(KeSeiPre3(:,i)-ZPre)';%%%%%%%%
        PxzChaTemp=zeros(5,1);
        for i=2:1:11
            PxzChaTemp=PxzChaTemp+Wc(i)*(KeSeiPre2(:,i)-XPre)*(KeSeiPre3(:,i)-ZPre)';
        end
        PxzChaTemp=PxzChaTemp+Wc(1)*(KeSeiPre2(:,1)-XPre)*(KeSeiPre3(:,1)-ZPre)';
        PxzCha=PxzChaTemp;
        %%%%%%%%%%%%%%%%%%%%%%%  获得Z（k），滤波更新     %%%%%%%%%%%%%%%%%%%%%%%%%%
        Kk=PxzCha*inv(PzCha);
        XF=XPre+Kk*(Z(k)-ZPre); %%%%  更新XF
        PF=PxPre-Kk*PzCha*Kk';  %%%%  更新PF
        LX=XF;
        LP=PF;
    end  
end
%计算滤波误差
ex_=zeros(1,K);ex=zeros(1,K);ev_=zeros(1,K);ev=zeros(1,K);ea_=zeros(1,K);ea=zeros(1,K);
for j=1:K
    xx(j)=xx(j)/N;  zz(j)=zz(j)/N;
    vv(j)=vv(j)/N;  aa(j)=aa(j)/N;     
    ex_(j)=ex1(j)/N;
%     ex(j)=sqrt(ex2(j)/N-ex_(j)^2);
%     ev_(j)=ev1(j)/N;
%     ev(j)=sqrt(ev2(j)/N-ev_(j)^2);
%     ea_(j)=ea1(j)/N;
%     ea(j)=sqrt(ea2(j)/N-ea_(j)^2);    
    ex(j)=sqrt(ex2(j)/N);
    ev_(j)=ev1(j)/N;
    ev(j)=sqrt(ev2(j)/N);
    ea_(j)=ea1(j)/N;
    ea(j)=sqrt(ea2(j)/N);  
end
% 输出图形
figure(5);
plot(x,'k');hold on;
plot(zz,'g');hold on;
plot(xx,'r');

figure(6);
plot(ex);
title('位置');axis([0,2000,0,150]);

figure(7);
plot(ev);
title('速度');axis([0,2000,0,20]);

figure(8);
plot(ea);
title ('加速度');