交互式多模型算法卡爾曼濾波仿真

1、交互式多模型算法卡爾曼濾波仿真1 模型建立分別以加速度 u=0、1、2 代表三個不同的運(yùn)動模型 狀態(tài)方程 x(k+1)=a*x(k)+b*w(k)+d*u 觀察方程 z(k)=c*x(k)+v(k)其中，a=1 dt;O 1,b=dtA2/2;dt,d=dtA2/2;dt,c=1 02 程序代碼由兩個功能函數(shù)組成， imm_main 用來實(shí)現(xiàn) imm 算法， move_model1 用來 生成仿真數(shù)據(jù)，初始化運(yùn)動參數(shù)function imm_main%交互式多模型算法主程序%初始化有關(guān)參數(shù)move_model %調(diào)用運(yùn)動模型初始化及仿真運(yùn)動狀態(tài)生成函數(shù)load movedata %調(diào)入有關(guān)參數(shù)

2、初始值( 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;% 轉(zhuǎn)移概率p_pri=0.1;0.6;0.3;% 模型先驗概率 n=1:2:5; % 提取各模型方差矩陣 k=0; % 記錄仿真步數(shù) like=0;0;0;% 視然函數(shù)x_hat_hat=zeros(2,3);%三模型運(yùn)動狀態(tài)重初始化矩陣u_=zeros(3,3);%混合概率矩陣c_=0;0;0;% 三模型概率更新系數(shù)% 數(shù)據(jù)保存有關(guān)參數(shù)初始化 phat=;% 保存位置估值 vhat

3、=;% 保存速度估值xhat=0;0;% 融合和運(yùn)動狀態(tài)z=0;%量測偏差(一維位置)pvar=zeros(2,2);%融合后估計方差for t=0:dt:tg; %dt 為為仿真步長； tg 為仿真時間長度 k=k+1;% 記錄仿真步數(shù) ct=0; %三模型概率更新系數(shù) c_max=0 0 0;% 混合概率規(guī)范系數(shù)P_var_hat=zeros(2,6);%方差估計重初始化矩陣，%x_hat_hat p_var_hat=model_reinitialization(p_tran,p_pri,x_hat,p_var);% 調(diào)用重初始化函數(shù)，進(jìn)行混合 估計，生成三個模型重初始化后的運(yùn)動狀態(tài)、方差

4、%混合概率計算 for i=1:3u_(i,:)=p_tran(i,:)*p_pri(i);endfor i=1:3c_max=c_max+u_(i,:);endfor i=1:3 u_(:,i)=u_(:,i)/c_max(i);end%各模型狀態(tài)、方差重初始化 x_hat_hat=x_hat*u_;% 運(yùn)動狀態(tài)重初始化 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_ha

5、t_hat(:,j)')*u_(i,j);% 混合估計end end%各模型進(jìn)行依次次 kalman 濾波 for i=1:3 % 各模型進(jìn)行依次次 kalman 濾波% 模型條件濾波 x_hat(:,i)=a*x_hat_hat(:,i)+d*u(i);% 一步狀態(tài)預(yù)測 p_var(:,n(i):n(i)+1)=a*p_var_hat(:,n(i):n(i)+1)*a'+b*q*b'% 一步狀態(tài)預(yù)測方差 z=pmeas(k)-c*x_hat(:,i);% 量測誤差估計s=c*p_var(:,n(i):n(i)+1)*c'+r;% 量測方差 k_add=p_va

6、r(:,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' %計算與當(dāng)前模型匹配的視然函數(shù)like(i)=1/sqrt(2*pi*s)*exp(-1/2*zA2/s);end% 模型概率更新 c_=p_tran*p_pri; for i=1:3 c_(i)=c_(i)*like(i); ct=ct+c_(i);end p_pri=c_/ct;% 模型先驗概率更新 xhat=x_hat*p_p

7、ri;% 運(yùn)動狀態(tài)估計融合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);g

8、rid;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(' 位置(藍(lán))、速度估計誤差(綠) ')

9、;function move_model1% 參數(shù)初始化dt=0.1;%仿真步長(秒)tg=200;%仿真持續(xù)時間(秒)a=1 dt;0 1 ;%狀態(tài)轉(zhuǎn)移矩陣b=dtA2/2;dt;%激勵輸入矩陣d=dtA2/2;dt;%機(jī)動加速度系數(shù)矩陣c=1 0;% 量測矩陣x=0;0;% 初始化狀態(tài)矢量x_hat=x x x;% 初始化狀態(tài)估計矩陣(三個模型狀態(tài)矢量綜合考慮)q=0.04;%過程隨機(jī)噪聲方差 r=36;%量測隨機(jī)噪聲方差 p_var=b*q*b' b*q*b' b*q*b'% 初始化方差矩陣(三個模型狀態(tài)矢量綜合考慮)% 數(shù)據(jù)保存數(shù)組初始化 position=;%

10、 真實(shí)位置 pmeas= ; %位置量測值 velocity=;% 真實(shí)速度 u=0:1:2;%k=0:dt:tg% 生成仿真模擬數(shù)據(jù)for i=0:dt:tgw=0.2*randn;% 過程隨機(jī)噪聲，均值為0，方差為 0.04v=6*randn;% 量測隨機(jī)噪聲，均值為 0，方差為 36if i<=30 x=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

11、+d*u(1);elsex=a*x+b*w+d*u(3);end y=c*x+v;% 量測值計算 position=position;x(1);% 真實(shí)位置pmeas=pmeas;y;% 位置量測值 velocity=velocity;x(2);% 真實(shí)速度 endsave movedata a b d c u position velocity pmeas dt tg q r x_hat p_var3 運(yùn)行結(jié)果圖一：機(jī)動方式：0-30 秒：u=1;30-100 秒：u=0; 100-150 秒：u=2;150-180 秒：u0; 180-200 秒：u=2過程噪聲方差：0.04 量測噪聲方差：36真位置真速度位置估計值150001000050000050100150200圖一圖二：機(jī)動方式：0-30 秒：u=2；30-100 秒：u