交互式多模型算法卡尔曼滤波仿真

交互式多模型算法卡尔曼滤波仿真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:3u_(i,:)=p_tran(i,:)*p_pri(i);endfor i=1:3c_max=c_max+u_(i,:);endfor i=1:3u_(:,i)=u_(:,i)/c_max(i);end%各模型状态、方差重初始化x_hat_hat=x_hat*u_;%运动状态重初始化for j=1:3for i=1:3p_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);%方差混合估计endend%各模型进行依次次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% 模型概率更新c_=p_tran*p_pri;for i=1:3c_(i)=c_(i)*like(i);ct=ct+c_(i);endp_pri=c_/ct;%模型先验概率更新xhat=x_hat*p_pri;%运动状态估计融合for i=1:3 %方差融合pvar=pvar+(p_var(:,n(i):n(i)+1)+(xhat-x_hat(:,i))*(xhat-x_hat(:,i))');endphat=[phat;xhat(1)];%位置估计值保存vhat=[vhat;xhat(2)];%速度估计值保存end% 图形输出t=0:dt:tg;subplot(3,2,1);%ylabel(‘Position (m)’);plot(t,position);grid;title('真位置');subplot(3,2,2);plot(t,velocity);grid;title('真速度');subplot(3,2,3);plot(t,pmeas);grid;title('位置量测值');subplot(3,2,4);plot(t,vhat);grid;title('速度估计值');subplot(3,2,5);plot(t,phat);grid;title('位置估计值');subplot(3,2,6);plot(t,position-phat,t,velocity-vhat);grid;title('位置（蓝）、速度估计误差（绿）');function move_model1% 参数初始化dt=0.1;%仿真步长（秒）tg=200;%仿真持续时间（秒）a=[1 dt;0 1];%状态转移矩阵b=[dt^2/2;dt];%激励输入矩阵d=[dt^2/2;dt];%机动加速度系数矩阵c=[1 0];%量测矩阵x=[0;0];%初始化状态矢量x_hat=[x x x];%初始化状态估计矩阵（三个模型状态矢量综合考虑）q=0.04;%过程随机噪声方差r=36;%量测随机噪声方差p_var=[b*q*b' b*q*b' b*q*b'];%初始化方差矩阵（三个模型状态矢量综合考虑）% 数据保存数组初始化position=[];%真实位置pmeas=[];%位置量测值velocity=[];%真实速度u=0:1:2;%k=0:dt:tg% 生成仿真模拟数据for i=0:dt:tgw=0.2*randn;%过程随机噪声，均值为0，方差为0.04v=6*randn;%量测随机噪声，均值为0，方差为36if i<=30x=a*x+b*w+d*u(2);elseif i>30&i<100x=a*x+b*w+d*u(1);elseif i>100&i<150x=a*x+b*w+d*u(3);elseif i>150&i<180x=a*x+b*w+d*u(1);elsex=a*x+b*w+d*u(3);endy=c*x+v;%量测值计算position=[position;x(1)];%真实位置pmeas=[pmeas;y];%位置量测值velocity=[velocity;x(2)];%真实速度endsave movedata a b d c u position velocity pmeas dt tg q r x_hat p_var3 运行结果图一：机动方式： 0-30秒：u=1； 30-100秒：u=0； 100-150秒：u=2； 150-180秒：u0； 180-200秒：u=2 过程噪声方差：0.04 量测噪声方差：36050100150200050001000015000真位置0501001502000100200真速度50100150200-10124位置量测值050100150200100200速度估计值50100150200050001000015000位置估计值050100150200-1010位置（蓝）、速度估计误差（绿）图一图二：机动方式： 0-30秒：u=2； 30-100秒：u=0； 100-150秒：u=1； 150-180秒：u=0； 180-200秒：u=2 过程噪声方差：4 量测噪声方差：100050100150200050001000015000真位置0501001502000100200真速度50100150200-10124位置量测值050100150200100200速度估计值50100150200050001000015000位置估计值050100150200-1010位置（蓝）、速度估计误差（绿）图二。