www.pudn.com > IMM_AFUKF.rar > UKF.m, change:2014-12-10,size:4008b


clear all; 
clc 
close all 
load Measure.mat%加载量测数据 
load Real.mat%加载真实值数据 
Z=TimeRPhi(2:3,:);%量测值 
Xr=TXYdXdYAxAyTPhi;%真值 
  
T=0.5; 
 
 
%UKF滤波器初始化 
alf=0.001; 
beta=2; 
i=6;%系统状态 
a=((alf)^2-1)*i; 
Wm0=a/(i+a); 
Wc0=a/(i+a)+(1-(alf)^2+beta); 
Wm=1/(2*(i+a));  %求一阶统计特性时的权系数 
Wc=1/(2*(i+a));  %求二阶统计特性时的权系数 
 
 
X1=[10000;0;0;0;0;0];%状态变量初值 
P1=diag([0.1,0.1,0.1,0.1,0.1,0.1]);%误差协方差初值 
Rk1=diag([100,(0.1/57.3)^2]);%量测噪声方差 
 Ta1=[T^2/2 0 T 0 1 0; 
     0 T^2/2 0 T 0 1]; 
 Ta1=Ta1';%噪声阵 
 sigam1=10; 
 Qk1=Ta1*Ta1'*sigam1^2;%系统噪声方差初值 
Qk1=0; 
 
 
 
 
X11=X1; 
 
P11=P1; 
 
 
 
 
for k=1:501 
    %第一步 模型条件重初始化 
    %1。首先计算混合概率 
    %计算c 
    X1=X11 
    P1=(P11+(X11-X1)*(X11-X1)') 
    PP(:,:,k)=P1; 
    A1=sqrtm(P1);%p1必须是非奇异 
    a1=det(A1);%行列式的值 
    A1=A1'; 
    %---------------------------------------------------------------------- 
    %第二步 模型条件滤波 
     
    k 
                                                                  %%%%%  UKF_1  滤波器开始滤波%%%%% 
    %%%%%%计算Sigma点%%%%%%% 
    si1(:,1)=X1; 
    for ii=2:7 
        si1(:,ii)=X1+((i+a)^(0.5))*A1(:,ii-1); 
    end 
    for ii=8:13 
        si1(:,ii)=X1-((i+a)^(0.5))*A1(:,ii-7); 
    end 
    %%%%%%时间更新%%%%%%% 
    %利用状态方程传递取样点 
  for ii=1:13 
      SI1(:,ii)=FX1(si1(:,ii)); 
  end 
%     %利用预测取样点,权值计算预测均值和协方差 
    X1=Wm0*SI1(:,1); 
    for ii=2:13 
        X1=X1+Wm*SI1(:,ii); 
    end 
     
    Xk1=X1; 
     
    %Pk1=1.02*Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)'); 
    Pk1=Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)'); 
    for ii=2:13 
        Pk1=Pk1+Wc*(SI1(:,ii)-X1)*((SI1(:,ii)-X1)'); 
    end 
    % Pk1=1.02*Pk1+Qk1;  
   Pk1=Pk1+Qk1;  
     
    %利用预测取样点预测测量取样点 
     for ii=1:13 
      zk1(:,ii)=FZ(SI1(:,ii)); 
     end 
      
    %预测测量值 
    Zk1=Wm0*zk1(:,1); 
    for ii=2:13 
        Zk1=Zk1+Wm*zk1(:,ii); 
    end 
    %%%%%%量测更新%%%%%%% 
      
    %预测协方差 
    Pzz1=Wc0*(zk1(:,1)-Zk1)*((zk1(:,1)-Zk1)'); 
    for ii=2:13 
        Pzz1=Pzz1+Wc*(zk1(:,ii)-Zk1)*((zk1(:,ii)-Zk1)'); 
    end 
     
    %%计算自适应衰减因子 
    % yk1=Z(:,k)-Zk1;%残差 
     %tryk1=trace(yk1*yk1');%残差的迹 
     %trpk1=trace(Pzz1);%新息的迹 
     %tr1=tryk1/trpk1 
     %niu1=max(1,tr1)%niu为所求自适应因子 
    
    % Pzz1=niu1*Pzz1+Rk1; 
    Pzz1=Pzz1+Rk1; 
    
    Pxz1=Wc0*(SI1(:,1)-X1)*((zk1(:,1)-Zk1)'); 
    for ii=2:13 
        Pxz1=Pxz1+Wc*(SI1(:,ii)-X1)*((zk1(:,ii)-Zk1)'); 
         %Pxz1=niu1*Pxz1+Wc*(SI1(:,ii)-X1)*((zk1(:,ii)-Zk1)'); 
    end 
   %计算UKF增益,更新状态向量和方差 
    zk1=Z(:,k)-Zk1; 
    K1=Pxz1*(inv(Pzz1))  %接近奇异矩阵 
    X1=X1+K1*(Z(:,k)-Zk1); 
    % P1=niu1*Pk1-K1*Pzz1*(K1)'; 
    P1=Pk1-K1*Pzz1*(K1)'; 
 
    
    Xe1(:,k)=X1;  %赋初值                                 
     
                                                                  %%%%%UKF_2 滤波器开始滤波%%%%% 
                                      
    %---------------------------------------------------------------------- 
    %第三步 模型概率更新 
    
    %---------------------------------------------------------------------- 
    %第四步 估计融合   
    X=X1 
     
    Xe(:,k)=X; 
    X11=X1; 
 
    P11=P1; 
 
     
end 
 
 
figure(1) 
plot(Xr(2,:)-Xe(1,:),'b')%x位置误差 
%subplot(3,2,1);plot(Xr(2,:),Xr(3,:),'.r')%真实轨迹。x.y 
hold 
%plot(Xe(1,:),Xe(2,:),'b')估计值想x,y 
grid; 
title('x位置误差'); 
hold 
 
figure(2) 
%subplot(3,2,2); 
plot(Xr(3,:)-Xe(2,:),'b')%y位置误差 
grid; 
title('y位置误差'); 
hold 
figure(3) 
%subplot(3,2,3); 
plot(Xr(4,:)-Xe(3,:),'b')%x速度误差 
grid; 
title('x速度误差'); 
hold 
figure(4) 
%subplot(3,2,4); 
plot(Xr(5,:)-Xe(4,:),'b')%y速度误差 
grid; 
title('y速度误差'); 
 
hold 
figure(5) 
%subplot(3,2,5); 
plot(Xr(6,:)-Xe(5,:),'b')%x加速度误差 
grid; 
title('x加速度误差'); 
hold 
figure(6) 
%subplot(3,2,6); 
plot(Xr(7,:)-Xe(6,:),'b')%y加速度误差 
grid; 
title('y加速度误差');hold