作业二交互式多模型算法卡尔曼滤波仿真.doc

交互式多模型算法卡尔曼滤波仿真 1 模型建立 分别以加速度u=0、1、2代表三个不同的运动模型 状态方程 x(k+1)=a*x(k)+b*w(k)+d*u 观察方程 z(k)=c*x(k)+v(k) 其中，a=[1 dt;0 1],b=[dt^2/2;dt],d=[dt^2/2;dt],c=[1 0] 2 程序代码 由两个功能函数组成，imm_main用来实现imm 算法，move_model1用来生成仿真数据，初始化运动参数 function imm_main %交互式多模型算法主程序 %初始化有关参数 move_model %调用运动模型初始化及仿真运动状态生成函数 load movedata %调入有关参数初始值（a b d c u position velocity pmeas dt tg q r x_hat p_var） p_tran=[0.8 0.1 0.1;0.2 0.7 0.1;0.1 0.2 0.7];%转移概率 p_pri=[0.1;0.6;0.3];%模型先验概率 n=1:2:5; %提取各模型方差矩阵 k=0; %记录仿真步数 like=[0;0;0];%视然函数 x_hat_hat=zeros(2,3);%三模型运动状态重初始化矩阵 u_=zeros(3,3);%混合概率矩阵 c_=[0;0;0];%三模型概率更新系数 %数据保存有关参数初始化 phat=[];%保存位置估值 vhat=[];%保存速度估值 xhat=[0;0];%融合和运动状态 z=0;%量测偏差（一维位置） pvar=zeros(2,2);%融合后估计方差 for t=0:dt:tg; %dt为为仿真步长；tg为仿真时间长度 k=k+1;%记录仿真步数 ct=0; %三模型概率更新系数 c_max=[0 0 0];%混合概率规范系数 p_var_hat=zeros(2,6);%方差估计重初始化矩阵, %[x_hat_hat p_var_hat]=model_reinitialization(p_tran,p_pri,x_hat,p_var);%调用重初始化函数，进行混合估计，生成三个模型重初始化后的运动状态、方差 %混合概率计算 for i=1:3 u_(i,:)=p_tran(i,:)*p_pri(i); end for i=1:3 c_max=c_max+u_(i,:); end for i=1:3 u_(:,i)=u_(:,i)/c_max(i); end %各模型状态、方差重初始化 x_hat_hat=x_hat*u_;%运动状态重初始化 for j=1:3 for i=1:3 p_var_hat(:,n(j):n(j)+1)= p_var_hat(:,n(j):n(j)+1)+(p_var(:,n(i):n(i)+1)+(x_hat(:,i)-x_hat_hat(:,j))*(x_hat(:,i)-x_hat_hat(:,j)))*u_(i,j);%方差混合估计 end end %各模型进行依次次kalman滤波 for i=1:3 %各模型进行依次次kalman滤波 % 模型条件滤波 x_hat(:,i)=a*x_hat_hat(:,i)+d*u(i);%一步状态预测 p_var(:,n(i):n(i)+1)=a*p_var_hat(:,n(i):n(i)+1)*a+b*q*b;%一步状态预测方差 z=pmeas(k)-c*x_hat(:,i);%量测误差估计 s=c*p_var(:,n(i):n(i)+1)*c+r;%量测方差 k_add=p_var(:,n(i):n(i)+1)*c*inv(s);%kalman增益 x_hat(:,i)=x_hat(:,i)+k_add*z; p_var(:,n(i):n(i)+1)=p_var(:,n(i):n(i)+1)-k_add*s*k_add; %计算与当前模型匹配的视然函数 like(i)=1/sqrt(2*pi*s)*exp(-1/2*z^2/s); end % 模型概率更新