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

1、交互式多模型算法卡尔曼滤波仿真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=dtA2/2;dt,d=dtA2/2;dt,c=1 02程序代码由两个功能函数组成，imm_main用来实现imm 算法，move_model1用来生成仿真数据，初始化运动参数一一function imm_main%交互式多则算法主程序%初始化有关参数move_model %调用运动模型初始化及仿真运动状态生成函数load movedata %调入有关参数初始值 ( a b d

2、 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;%融合

3、和运动状态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

4、_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);%方差混合估计e

5、ndend%各模型进彳f依次次kalman滤波for i=1:3 %各模型进彳f依次次 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_

6、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*zA2/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(:

7、,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

8、,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;%状态转

9、移矩阵b=dtA2/2;dt;%激励输入矩阵d=dtA2/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*

10、randn;%过程随机噪声，均值为0,方差为0.04v=6*randn;%量测随机噪声，均值为 0,方差为36 if 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); end y=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.