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

```clear all;
clc
close all
Z=TimeRPhi(2:3,:);%量测值

%%%%%%%%%%%%%%%%%%%%%%加上Z噪声%%%%%%%%%%%%%%%%%%%%%%%%%%
r=0.5+0.1*randn(1,501);%均值为1方差为0.2正态分布的500个随机数
r1=0.01+0.0002*randn(1,501);
u=zeros(1,501);
o=[r;u];
o1=[u;r;r;u;r;r;r;u;u];
%Z=Z+o;

Xr=TXYdXdYAxAyTPhi;%真值
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]);%量测噪声方差  创建对角矩阵
%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;%系统噪声方差初值

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_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=Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)');
Pk1=1.02*Wc0*(SI1(:,1)-X1)*((SI1(:,1)-X1)');
for ii=2:13
Pk1=Pk1+Wc*(SI1(:,ii)-X1)*((SI1(:,ii)-X1)');
end
% Pk1=Pk1+Qk1;
Pk1=1.02*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;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%计算自适应衰减因子%%%%%%%%%%%%%

yk1=Z(:,k)-Zk1 ;%残差
tryk1=trace(yk1*yk1');  %残差的迹
trpk1=trace(Pzz1);   %新息的迹
tr1=tryk1/trpk1;
tr1=min(10000,tr1);
niu1=max(1,tr1);  %niu为所求自适应因子

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)';
P1=niu1*Pk1-K1*(niu1*Pzz1)*(K1)';
Xe1(:,k)=X1;

%%%%%  UKF_2      滤波器开始滤波%%%%%
%%%%%%计算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=1.02*Wc0*(SI2(:,1)-X2)*((SI2(:,1)-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;
Pk2=1.02*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
%%%%%%%%%%%%%%%自适应因子%%%%%%%%%%%%%%%%%%%%
yk2=Z(:,k)-Zk2 ;%残差
tryk2=trace(yk2*yk2');  %残差的迹
trpk2=trace(Pzz2);   %新息的迹
tr2=tryk2/trpk2;
tr2=min(1000,tr2);
niu2=max(1,tr2);  %niu为所求自适应因子

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)';
P2=niu2*Pk2-K2*(niu2*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(6,:)-Xe(5,:);
N1=M1(1:500);
O1=sum(N1);
O2=O1/500   %均值
RMSE1=sqrt((N1.^2)/k);
RMSE=sqrt(mse(N1))  %均方根误差

% 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(1)
plot(RMSE1,'r')
grid;
title('x位置均方根误差');%均方根误差
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

```