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

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

2、)*T)-x(4)*sin(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)-30

3、0)/(x(1)-900); %节点三观测模型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

4、=Q1*randn(6,N0); %模型一过程噪声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设

5、为3-n% Wm=lambda/(n+lambda) 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

6、 meansWc=Wm;Wc(1)=Wc(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)

7、=node(s).h(x(:,k)+node(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+(x

8、g2(:,k-1)-xo1)*(xg2(:,k-1)-xo1)'); Po2=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)=

9、node(s).xo1(:,k); elseif i>1&&i<=n+1 sigma(:,i)=node(s).xo1(:,k)+Jx(:,i-1); else sigma(:,i)=node(s).xo1(:,k)-Jx(:,i-7); end end for i=1:2*n+1 xk(:,i)=F*sigma(:,i); %sigma点做非线性变换 end node(s).xg1(:,k)=(Wm*xk')' %求加权均值 即先验估计 Pxg=zeros(6); for i=1:2*n+1 Pxg=Pxg+Wc(i)*(xk(:,i)-node(

10、s).xg1(:,k)*(xk(:,i)-node(s).xg1(:,k)' end node(s).Pxg=Pxg+Q1; node(s).Lg1=inv(node(s).Pxg); node(s).lg1(:,k)=node(s).Lg1*node(s).xg1(:,k); %构造输出sigma点（利用上面计算的xg 和Pxg 构造）Jy=sqrt(n+lamda)*(chol(node(s).Pxg)' %乔列斯基分解for i=1:2*n+1 if i=1 ysigma(:,i)=node(s).xg1(:,k); elseif i>1&&i<

11、;=n+1 ysigma(:,i)=node(s).xg1(:,k)+Jy(:,i-1); else ysigma(:,i)=node(s).xg1(:,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).v

12、1=node(s).z(k)-node(s).yg(:,k);node(s).Pyg=Pyg+R; Pxyg=zeros(6,1); for i=1:2*n+1 Pxyg=Pxyg+Wc(i)*(xk(:,i)-node(s).xg1(:,k)*(yk(:,i)-node(s).yg(:,k)' end %同上 Hp=node(s).Lg1*Pxyg; node(s).Ik1=Hp*inv(R)*Hp' node(s).ik1(:,k)=Hp*inv(R)*(node(s).v(:,k)+Hp'*node(s).xg1(:,k);% node(s).ik1(:,k)=n

13、ode(s).Lg1*Pxyg*inv(R)*node(s).z(:,k);% node(s).Ik1=node(s).Lg1*Pxyg*inv(R)*Pxyg'*node(s).Lg1' 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).ik1(:,k); node(s).Ik0=node(s).Ik0+T1(g,s)* node(g).Ik1; end for g=1:sNUM node(s).lg1(:,k

14、)=node(s).lg1(:,k)+node(g).ik0; node(s).Lg1=node(s).Lg1+node(s).Ik0; end G=zeros(6,sNUM); Lg_rh=zeros(n); vc1=0; Sc1=zeros(1); for g=1:sNUM G(:,g)=node(g).lg1(:,k); Lg_rh=Lg_rh+node(g).Lg1; vc1=vc1+node(g).v1; Sc1=Sc1+node(g).Pyg; end Lg_rh= Lg_rh/sNUM; lg_rh(1)=consencous(G(1,:)',L); lg_rh(2)=c

15、onsencous(G(2,:)',L); lg_rh(3)=consencous(G(3,:)',L); lg_rh(4)=consencous(G(4,:)',L); lg_rh(5)=consencous(G(5,:)',L); lg_rh(6)=consencous(G(6,:)',L); P1=inv(Lg_rh); xg1(:,k)=P1*lg_rh' %模型二 for s=1:sNUM %构造过程sigma点 node(s).Po2=Po2; node(s).xo2(:,k)=xo2; Jx=sqrt(n+lamda)*(chol(

16、node(s).Po2)' %乔列斯基分解 for i=1:2*n+1 if i=1 sigma(:,i)=node(s).xo2(:,k); elseif i>1&&i<=n+1 sigma(:,i)=node(s).xo2(:,k)+Jx(:,i-1); else sigma(:,i)=node(s).xo2(:,k)-Jx(:,i-7); end end for i=1:2*n+1 xk(:,i)=f(sigma(:,i); %sigma点做非线性变换 end node(s).xg2(:,k)=(Wm*xk')' %求加权均值 即先验估

17、计 Pxg=zeros(6); for i=1:2*n+1 Pxg=Pxg+Wc(i)*(xk(:,i)-node(s).xg2(:,k)*(xk(:,i)-node(s).xg2(:,k)' end Pxg=Pxg+Q2; node(s).Lg2=inv(Pxg); node(s).lg2(:,k)=node(s).Lg2*node(s).xg2(:,k); %构造输出sigma点（利用上面计算的xg 和Pxg 构造）Jy=sqrt(n+lamda)*(chol(Pxg)' %乔列斯基分解for i=1:2*n+1 if i=1 ysigma(:,i)=node(s).xg2

18、(:,k); elseif i>1&&i<=n+1 ysigma(:,i)=node(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)

19、-node(s).yg(:,k)' endnode(s).v2=node(s).z(k)-node(s).yg(:,k); %残差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)+

20、Hp'*node(s).xg2(:,k);% node(s).ik2(:,k)=node(s).Lg2*Pxyg*inv(R)*node(s).z(:,k);% 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)*

21、node(g).Ik2; end for g=1:sNUM node(s).lg2(:,k)=node(s).lg2(:,k)+node(g).ik0; node(s).Lg2=node(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; l

22、g_rh(1)=consencous(G(1,:)',L); lg_rh(2)=consencous(G(2,:)',L); lg_rh(3)=consencous(G(3,:)',L); 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)/sq

23、rt(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)=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); %

24、误差 Ex(:,k)=diag(P);end% 绘图% figure% plot(x(1,:),x(2,:),'r-',xg(1,:),xg(2,:),'bo')% axis equal% title('目标真实轨迹和估计轨迹')% legend('真实轨迹','轨迹估计');xg=x;for 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*

25、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);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,'观测