www.pudn.com > IMM_AFUKF.rar > IMMUKF.asv, change:2014-12-16,size:6492b


clear all; 
clc 
close all 
load Measure.mat%加载量测数据 
load Real.mat%加载真实值数据 
Z=TimeRPhi(2:3,:);%量测值 
%%%%%%%%%%%%%%%%%%%%%%加上Z噪声%%%%%%%%%%%%%%%%%%%%%%%%%% 
r=1+0.5*randn(1,501);%均值为1方差为0.2正态分布的500个随机数 
u=zeros(1,501);o=[r;u]; 
Z=Z+o;%量测值 
 
Xr=TXYdXdYAxAyTPhi;%真值 
o1=[u;r;r;u;r;u;r;u;u]; 
Xr=Xr+o1; 
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%最原始————最差劲数据%%%%%%%%%%%%% 
T=0.1;%原为0.1 
pai=[0.8 0.2;0.1 0.9];  %定义一步转移概率矩阵 
miu_CV=0;            %匀速运动模型在初始时刻正确的概率 
miu_CA=1;            %匀加速运动模型在初始时刻正确概率 
 
%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([900,(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; 
 
X2=[10000;0;0;0;0;0];%状态变量初值 
P2=diag([0.1,0.1,0.1,0.1,0.1,0.1]);%误差协方差初值 
Rk2=diag([900,(0.1/57.3)^2]);%量测噪声方差 
Ta2=[T^2/2 0 T 0 1 0; 
    0 T^2/2 0 T 0 1]; 
Ta2=Ta2';%噪声阵 
sigam2=7; 
Qk2=Ta2*Ta2'*sigam2^2;%系统噪声方差初值 
 
 
X11=X1; 
X22=X2; 
P11=P1; 
P22=P2; 
 
 
 
for k=1:501 
    %第一步 模型条件重初始化 
    %1。首先计算混合概率 
    %计算c 
    c_1=pai(1,1)*miu_CV+pai(2,1)*miu_CA; 
    c_2=pai(1,2)*miu_CV+pai(2,2)*miu_CA; 
    %计算miu_temp 
    miu11=pai(1,1)*miu_CV/c_1;    
    miu12=pai(1,2)*miu_CV/c_2; 
    miu21=pai(2,1)*miu_CA/c_1; 
    miu22=pai(2,2)*miu_CA/c_2; 
    %2。进行混合估计 
    %匀速运动模型 
    X1=X11*miu11+X22*miu21;% 
    P1=(P11+(X11-X1)*(X11-X1)')*miu11+(P22+(X22-X1)*(X22-X1)')*miu21; 
    PP(:,:,k)=P1; 
    A1=sqrtm(P1);%得到方根阵,A1*A1=P1 
    A1=A1'; 
    %匀加速运动模型 
    X2=X11*miu12+X22*miu22;% 
    P2=(P11+(X11-X2)*(X11-X2)')*miu12+(P22+(X22-X2)*(X22-X2)')*miu22; 
    A2=sqrtm(P2); 
    A2=A2'; 
    %---------------------------------------------------------------------- 
    %第二步 模型条件滤波 
     
     
                                                                  %%%%%UKF滤波器开始滤波%%%%% 
    %%%%%%计算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=Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)'); 
    for ii=2:13 
        Pk1=Pk1+Wc*(SI1(:,ii)-X1)*((SI1(:,ii)-X1)'); 
    end 
    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 
    Pzz1=Pzz1+Rk1; 
    
    Pxz1=Wc0*(SI1(:,1)-X1)*((zk1(:,1)-Zk1)'); 
    for ii=2:13 
        Pxz1=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=Pk1-K1*Pzz1*(K1)'; 
 
    Xe1(:,k)=X1;                                   
     
                                                                  %%%%%UKF滤波器开始滤波%%%%% 
    %%%%%%计算Sigma点%%%%%%% 
    si2(:,1)=X2; 
    for ii=2:7 
        si2(:,ii)=X2+((i+a)^(0.5))*A2(:,ii-1); 
    end 
    for ii=8:13 
        si2(:,ii)=X2-((i+a)^(0.5))*A2(:,ii-7); 
    end 
    %%%%%%时间更新%%%%%%% 
    %利用状态方程传递取样点 
  for ii=1:13 
      SI2(:,ii)=FX2(si2(:,ii)); 
  end 
%     %利用预测取样点,权值计算预测均值和协方差 
    X2=Wm0*SI2(:,1); 
    for ii=2:13 
        X2=X2+Wm*SI2(:,ii); 
    end 
     
    Xk2=X2; 
     
    Pk2=Wc0*(SI2(:,1)-X2)*((SI2(:,1)-X2)'); 
    for ii=2:13 
        Pk2=Pk2+Wc*(SI2(:,ii)-X2)*((SI2(:,ii)-X2)'); 
    end 
    Pk2=Pk2+Qk2;  
     
    %利用预测取样点预测测量取样点 
     for ii=1:13 
      zk2(:,ii)=FZ(SI2(:,ii)); 
     end 
      
    %预测测量值 
    Zk2=Wm0*zk2(:,1); 
    for ii=2:13 
        Zk2=Zk2+Wm*zk2(:,ii); 
    end 
    %%%%%%量测更新%%%%%%% 
    %预测协方差 
    Pzz2=Wc0*(zk2(:,1)-Zk2)*((zk2(:,1)-Zk2)'); 
    for ii=2:13 
        Pzz2=Pzz2+Wc*(zk2(:,ii)-Zk2)*((zk2(:,ii)-Zk2)'); 
    end 
    Pzz2=Pzz2+Rk2; 
    
    Pxz2=Wc0*(SI2(:,1)-X2)*((zk2(:,1)-Zk2)'); 
    for ii=2:13 
        Pxz2=Pxz2+Wc*(SI2(:,ii)-X2)*((zk2(:,ii)-Zk2)'); 
    end 
   %计算UKF增益,更新状态向量和方差 
     zk2=Z(:,k)-Zk2; 
    K2=Pxz2*(inv(Pzz2)); 
    X2=X2+K2*(Z(:,k)-Zk2); 
    P2=Pk2-K2*Pzz2*(K2)'; 
    Xe2(:,k)=X2;                                   
    %---------------------------------------------------------------------- 
    %第三步 模型概率更新 
     %计算模型匹配函数 
    like1=exp(-0.5*zk1'*inv(Pzz1)*zk1)/(sqrt(det(2*3.1415926*Pzz1))); 
    like2=exp(-0.5*zk2'*inv(Pzz2)*zk2)/(sqrt(det(2*3.1415926*Pzz2)));    
     
    c_1=pai(1,1)*miu_CV+pai(2,1)*miu_CA; 
    c_2=pai(1,2)*miu_CV+pai(2,2)*miu_CA; 
    c=like1*c_1+like2*c_2; 
    miu_CV=like1*c_1/c; 
    miu_CA=like2*c_2/c; 
     
    %---------------------------------------------------------------------- 
    %第四步 估计融合   
    X=X1*miu_CV+X2*miu_CA; 
     
    Xe(:,k)=X; 
    X11=X1; 
    X22=X2; 
    P11=P1; 
    P22=P2; 
     
end 
 
%%% 求均方根误差 
M1=Xr(2,:)-Xe(1,:); 
N1=M1(1:500); 
O1=sum(N1); 
O2=O1/500 
RMSE1=sqrt((N1.^2)/k); 
RMSE=sqrt(mse(N1))  
figure(1) 
plot(RMSE1,'--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('Y位置均方根误差'); 
hold on 
 
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