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

交互式多模型算法卡尔曼滤波仿真 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;% 量测方差