资源描述:
《交互式多模型分布式信息滤波.doc》由会员上传分享,免费在线阅读,更多相关内容在教育资源-天天文库。
1、%%IMM_DUIFcloseallclearclc%初始化n=6;%状态维数T=1;%采样周期t=300;%仿真时间N0=t/T;%总采样点数N=N0/5;%每个运动阶段采样点数sNUM=3;%节点数目F=[10T000;010T00;001000;000100;000010;000001];%模型一状态矩阵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
2、)*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))
3、;%节点二观测模型node(3).h=@(x)atan((x(2)-300)/(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;P
4、2=P0;P=P0;Q1=zeros(n);%模型一过程噪声方差Q2=zeros(n);%模型二过程噪声方差R=0.05;%观测噪声方差w1=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(
5、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+lambda)1/(2*(n+lambda))*ones(1,2*n)];%Wc=[lambda/(n+lambda)+1-alfa^2+beta1/(2*(n+lambda))*ones(1,2*n)];%加权系数alfa=1;%default,tunablekap=
6、3-n;%default,tunablebeta=2;%default,tunablelamda=alfa^2*(n+kap)-n;%scalingfactorWm=[lamda/(lamda+n)0.5/(lamda+n)*ones(1,2*n)];%weightsformeansWc=Wm;Wc(1)=Wc(1)+(1-alfa^2+beta);%weightsforcovariance%%模拟目标真实轨迹fork=2:Nx(:,k)=F*x(:,k-1);endfork=(N+1):2*Nx(:,k)=
7、f(x(:,k-1));endfork=(2*N+1):3*Nx(:,k)=F*x(:,k-1);endfork=(3*N+1):4*Nx(:,k)=f(x(:,k-1));endfork=(4*N+1):N0x(:,k)=F*x(:,k-1);end%模拟观测值fork=1:N0fors=1:sNUMnode(s).z(k)=node(s).h(x(:,k))+node(s).v(k);endend%%滤波算法fork=2:N0%输入交互P1=P0P2=P0;c=A'*mu(:,k-1);uu=A.*(mu
8、(:,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