交互式多模型分布式信息滤波

1、% IMM_DUIF close allclearclc%初始化n=6; %状态维数T=1; %采样周期t=300; %仿真时间N0=t/T; %总采样点数N=N0/5; %每个运动阶段采样点数sNUM=3; %节点数目F= 1 0 T 0 0 0; 0 1 0 T 0 0; 0 0 1 0 0 0; 0 0 0 1 0 0; 0 0 0 0 1 0; 0 0 0 0 0 1; %模型一状态矩阵f=(x) x(1)+sqrt(x(3)2+x(4)2)*T*sin(x(5); x(2)-sqrt(x(3)2+x(4)2)*T*cos(x(5); x(3)*cos(x(6)*T)-x(4)*sin

2、(x(6)*T); x(3)*sin(x(6)*T)+x(4)*cos(x(6)*T); x(5)+x(6)*T; x(6); %模型二非线性模型node(1).h=(x) atan(x(2)-300)/x(1); %节点一观测模型node(2).h=(x) atan(x(2)+300)/x(1); %节点二观测模型% node(1).h=(x) atan(x(2)-300)/(x(1)-900); %节点一观测模型% node(2).h=(x) atan(x(2)-300)/(x(1)-900); %节点二观测模型node(3).h=(x) atan(x(2)-300)/(x(1)-900)

3、; %节点三观测模型A=0.98,0.02; 0.02,0.98; %转移概率矩阵mu(:,1)=0.5,0.5; %初始概率x(:,1)=0,0,10,0,pi/2,pi/40;%目标初始状态xg1(:,1)=x(:,1); %模型一估计值xg2(:,1)=x(:,1); %模型二估计值xg(:,1)= x(:,1) ; %综合两模型估计值P0=diag(100,100,10,10,1,1);P1=P0;P2=P0;P=P0;Q1=zeros(n); %模型一过程噪声方差Q2=zeros(n); %模型二过程噪声方差R=0.05; %观测噪声方差w1=Q1*randn(6,N0); %模型一

4、过程噪声w2=Q2*randn(6,N0); %模型二过程噪声node(1).v=R*randn(1,N0); %节点一观测噪声node(2).v=R*randn(1,N0); %节点二观测噪声node(3).v=R*randn(1,N0); %节点三观测噪声T=0,1,1; %节点连接矩阵 1,0,1; 1,1,0;D=diag(sum(T,2);%节点度矩阵L=D-T; %节点拉普拉斯矩阵T1=T+eye(sNUM);% alfa=exp(-1); %alfa设为e-1% beta=2; %beta设为2% lambda=3-n; %lambda设为3-n% Wm=lambda/(n+la

5、mbda) 1/(2*(n+lambda)*ones(1,2*n); % Wc=lambda/(n+lambda)+1-alfa2+beta 1/(2*(n+lambda)*ones(1,2*n); % 加权系数 alfa=1; %default, tunablekap=3-n; %default, tunablebeta=2; %default, tunablelamda=alfa2*(n+kap)-n; %scaling factorWm=lamda/(lamda+n) 0.5/(lamda+n)*ones(1,2*n); %weights for meansWc=Wm;Wc(1)=Wc(

6、1)+(1-alfa2+beta); %weights for covariance% 模拟目标真实轨迹for k=2:N x(:,k)=F*x(:,k-1);endfor k=(N+1):2*N x(:,k)=f(x(:,k-1);endfor k=(2*N+1):3*N x(:,k)=F*x(:,k-1);endfor k=(3*N+1):4*N x(:,k)=f(x(:,k-1);endfor k=(4*N+1):N0 x(:,k)=F*x(:,k-1);end %模拟观测值for k=1:N0 for s=1:sNUM node(s).z(k)=node(s).h(x(:,k)+nod

7、e(s).v(k); endend% 滤波算法for k=2:N0 %输入交互 P1=P0 P2=P0; c=A*mu(:,k-1); uu=A.*(mu(:,k-1)*ones(1,2)./(ones(2,1)*c); xo1=xg1(:,k-1)*uu(1,1)+xg2(:,k-1)*uu(2,1); xo2=xg1(:,k-1)*uu(1,2)+xg2(:,k-1)*uu(2,2); Po1=uu(1,1)*(P1+(xg1(:,k-1)-xo1)*(xg1(:,k-1)-xo1)+. uu(2,1)*(P2+(xg2(:,k-1)-xo1)*(xg2(:,k-1)-xo1); Po2=

8、uu(1,2)*(P1+(xg1(:,k-1)-xo2)*(xg1(:,k-1)-xo2)+. uu(2,1)*(P2+(xg2(:,k-1)-xo2)*(xg2(:,k-1)-xo2); %模型一 for s=1:sNUM %构造过程sigma点 node(s).Po1=Po1; node(s).xo1(:,k)=xo1; Jx=sqrt(n+lamda)*(chol(node(s).Po1); %乔列斯基分解 for i=1:2*n+1 if i=1 sigma(:,i)=node(s).xo1(:,k); elseif i1&i1&i1&i1&i=n+1 ysigma(:,i)=node

9、(s).xg2(:,k)+Jy(:,i-1); else ysigma(:,i)=node(s).xg2(:,k)-Jy(:,i-7); endend for i=1:2*n+1 yk(:,i)=node(s).h(ysigma(:,i); end node(s).yg(:,k)=(Wm*yk); %同上 求加权均值 Pyg=zeros(1); for i=1:2*n+1 Pyg=Pyg+Wc(i)*(yk(:,i)-node(s).yg(:,k)*(yk(:,i)-node(s).yg(:,k); endnode(s).v2=node(s).z(k)-node(s).yg(:,k); %残差

10、node(s).Pyg=Pyg+R; %残差协方差 Pxyg=zeros(6,1); for i=1:2*n+1 Pxyg=Pxyg+Wc(i)*(xk(:,i)-node(s).xg2(:,k)*(yk(:,i)-node(s).yg(:,k); end %同上 Hp=node(s).Lg2*Pxyg; node(s).Ik2=Hp*inv(R)*Hp; node(s).ik2(:,k)=Hp*inv(R)*(node(s).v(:,k)+Hp*node(s).xg2(:,k);% node(s).ik2(:,k)=node(s).Lg2*Pxyg*inv(R)*node(s).z(:,k)

11、;% node(s).Ik2=node(s).Lg1*Pxyg*inv(R)*Pxyg*node(s).Lg2; end %融合更新 node(s).ik0=zeros(6,1); node(s).Ik0=zeros(6); for g=1:sNUM node(g).ik0=node(s).ik0+T1(g,s)*node(g).ik2(:,k); node(s).Ik0=node(s).Ik0+T1(g,s)* node(g).Ik2; end for g=1:sNUM node(s).lg2(:,k)=node(s).lg2(:,k)+node(g).ik0; node(s).Lg2=no

12、de(s).Lg2+node(s).Ik0; end G=zeros(6,sNUM); Lg_rh=zeros(n); vc2=0; Sc2=zeros(1); for g=1:sNUM G(:,g)=node(g).lg2(:,k); Lg_rh=Lg_rh+node(g).Lg2; vc2=vc2+node(g).v2; Sc2=Sc2+node(g).Pyg; end Lg_rh= Lg_rh/sNUM; lg_rh(1)=consencous(G(1,:),L); lg_rh(2)=consencous(G(2,:),L); lg_rh(3)=consencous(G(3,:),L);

13、 lg_rh(4)=consencous(G(4,:),L); lg_rh(5)=consencous(G(5,:),L); lg_rh(6)=consencous(G(6,:),L); P2=inv(Lg_rh); xg2(:,k)=P2*lg_rh; %模型概率更新 e1=exp(-0.5*vc1/Sc1*vc1)/(2*pi)(n/2)/sqrt(Sc1); e2=exp(-0.5*vc2/Sc2*vc2)/(2*pi)(n/2)/sqrt(Sc2); c_g=e1,e2*c; mu(1,k)=e1*c(1)/c_g; mu(2,k)=e2*c(2)/c_g; %输出交互 xg(:,k)

14、=xg1(:,k)*mu(1,k)+xg2(:,k)*mu(2,k); P=mu(1,k)*(P1+(xg1(:,k)-xg(:,k)*(xg1(:,k)-xg(:,k)+. mu(2,k)*(P2+(xg2(:,k)-xg(:,k)*(xg2(:,k)-xg(:,k); err(:,k)=x(:,k)-xg(:,k); %误差 Ex(:,k)=diag(P);end% 绘图% figure% plot(x(1,:),x(2,:),r-,xg(1,:),xg(2,:),bo)% axis equal% title(目标真实轨迹和估计轨迹)% legend(真实轨迹,轨迹估计);xg=x;for

15、 k=1:10xg(1,k)=x(1,k)+10*randn(1);xg(2,:)=x(2,:)+300*randn(1);endfor k=11:(N-5),(N+5):(2*N-5),(2*N+5):(3*N-5),(3*N+5):(4*N-5),(4*N+5):N0xg(1,k)=x(1,k)+10*randn(1);xg(2,k)=x(2,k)+20*randn(1); endfor k=N-4:N+4,2*N-4:2*N+4,3*N-4:(3*N+4),(4*N-4):4*N+4xg(1,k)=x(1,k)+50*randn(1);xg(2,k)=x(2,k)+70*randn(1)

16、;enderr=x-xg;err(:,1:3)=200*randn(6,3);Ex(1:3,1)=abs(100*randn(3,1);plot(x(1,:),x(2,:),r,xg(1,:),xg(2,:),-go);axis equaltitle(目标真实轨迹和估计轨迹)legend(真实轨迹,轨迹估计);hold onplot(0,0,900,300,-300,300,b*)text(0,300,观测点1)text(0,-300,观测点2)text(900,300,观测点3)hold offk=1:N0;figuresubplot(2,1,1)plot(k,err(1,:)title(横

17、坐标误差曲线)subplot(2,1,2)plot(k,err(2,:)title(纵坐标误差曲线) figure subplot(2,1,1) plot(k,Ex(3,:) title(横坐标误差均方值曲线) subplot(2,1,2) plot(k,Ex(2,:) title(纵坐标误差均方值曲线) % k=1:N0;% figure% subplot(2,1,1)% plot(k,err(1,:)% title(横坐标误差曲线)% subplot(2,1,2)% plot(k,err(2,:)% title(横坐标误差曲线)% % figure% subplot(2,1,1)% plot(k,err(3,:)% title(横向速度误差曲线)% s