www.gusucode.com > 上传的为imm交互多模型跟踪算法的matlab源程序 > code/imm_ca_cv.m
%imm算法 function [X_estimate,P,x_model_filter,p_model_filter,u_output]=imm_cs_ca(z,x1,p1,u_input) % % T=1; u_output=zeros(2,1); fai=[]; Q=[]; %模型1,CA模型 F_ca=[1 T T^2/2;0 1 T;0 0 1]; G_ca=[T^2/2,T,1]'; Q_ca=10; Q_ca=G_ca*Q_ca*G_ca'; %模型2,CV模型 F_cv=[1 T 0;0 1 0;0 0 0]; G_cv=[T^2/2,T,0]'; Q_cv=10; Q_cv=G_cv*Q_cv*G_cv'; fai=[F_ca F_cv]; Q=[Q_ca Q_cv]; h=[1 0 0]; r=100^2; %模型参数设置完毕 dim=3;%状态维数 model_number=2;%模型数 % MarkoProb=[0.85 0.05 0.05 0.05;0.05 0.85 0.05 0.05;0.05 0.05 0.85 0.05;0.05 0.05 0.05 0.85]; MarkoProb=[0.95 0.05;0.05 0.95]; predictProb=MarkoProb'*u_input; mixedProb=[]; mixedInitX=zeros(dim,model_number); mixedInitP=zeros(dim,dim*model_number); mixedInitC=zeros(dim,dim*model_number); for i=1:model_number for j=1:model_number mixedProb(i,j)=MarkoProb(i,j)*u_input(i)/predictProb(j); end end mixedInitX=x1*mixedProb; for j=1:model_number for i=1:model_number mixedInitP(:,(j-1)*dim+1:j*dim)=mixedInitP(:,(j-1)*dim+1:j*dim)+(p1(:,(i-1)*dim+1:i*dim)+(x1(:,i)-mixedInitX(:,j))*(x1(:,i)-mixedInitX(:,j))')*mixedProb(i,j); end end for i=1:model_number temp1=(i-1)*dim+1; temp2=i*dim; x_model_predict(:,i)=fai(:,temp1:temp2)*mixedInitX(:,i); % p_model_predict(:,temp1:temp2)=fai(:,temp1:temp2)*mixedInitP(:,temp1:temp2)*fai(:,temp1:temp2)'+g(:,(i-1)*2+1:i*2)*q(:,(i-1)*2+1:i*2)*g(:,(i-1)*2+1:i*2)'; p_model_predict(:,temp1:temp2)=fai(:,temp1:temp2)*mixedInitP(:,temp1:temp2)*fai(:,temp1:temp2)'+Q(:,temp1:temp2); s(:,i)=h*p_model_predict(:,temp1:temp2)*h'+r; k(:,i)=p_model_predict(:,temp1:temp2)*h'*inv(s(:,i)); v_model_filter(:,i)=z-h*x_model_predict(:,i); x_model_filter(:,i)=x_model_predict(:,i)+k(:,i)*v_model_filter(:,i); p_model_filter(:,temp1:temp2)=p_model_predict(:,temp1:temp2)-k(:,i)*s(:,i)*k(:,i)'; likelyhood(i)=(det(2*pi*s(:,i)))^(-0.5)*exp(-0.5*v_model_filter(:,i)'*inv(s(:,i))*v_model_filter(:,i)); % A=det(2*pi*s(:,(i-1)*2+1:i*2)); % B=exp(-0.5*v_model_filter(:,i)'*inv(s(:,(i-1)*2+1:i*2))*v_model_filter(:,i)); u_output(i)=likelyhood(i)*predictProb(i); end u_output=(u_output/sum(u_output)); X_estimate=x_model_filter*u_output; % X_estimate=real(X_estimate);%%%%仿真表明估计可能出现虚部,通过三角分解可消除此现象,为简略起见直接去掉虚部 P=zeros(dim); for i=1:model_number temp1=(i-1)*dim+1; temp2=i*dim; P=P+(p_model_filter(:,temp1:temp2)+(x_model_filter(:,i)-X_estimate)*(x_model_filter(:,i)-X_estimate)')*u_output(i); end