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

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;0 1,b=dtA2/2;dt,d=dtA2/2;dt,c=1 02程序代碼由兩個功能函數(shù)組成，imm_main用來實現(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ù)初始值 ( 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;% 轉(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=;%保存速度估值xhat=0;0;%融合

3、和運(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)、方差 一 一 -%混合概率計算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%各模型狀態(tài)、方差重初始化x_hat_hat=x_hat*u_;%運(yùn)動狀態(tài)重初始化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%各模型進(jìn)彳f依次次kalman濾波for i=1:3 %各模型進(jìn)彳f依次次 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_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'%'卬算與當(dāng)前模型匹前的視然函數(shù)一 一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;%運(yùn)動狀態(tài)估計融合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('位置(藍(lán))、速度估計誤差(綠),);function move_model1%參數(shù)初始化dt=0.1;%仿真步長(秒)tg=200;%仿真持續(xù)時間(秒)a=1 dt;0 1;%狀態(tài)轉(zhuǎn)

9、移矩陣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=;% 真實位置 pmeas=;%位置量測值 velocity=;% 真實速度 u=0:1:2;%k=0:dt:tg%生成仿真模擬數(shù)據(jù)for i=0:dt:tgw=0.2*

10、randn;%過程隨機(jī)噪聲，均值為0,方差為0.04v=6*randn;%量測隨機(jī)噪聲，均值為 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運(yùn)行結(jié)果圖一：機(jī)動方式：0-30 秒：u=1;30-100 秒：u=0;100-150 秒：u=2;150-180 秒:U0;180-200 秒：u=2過程噪聲方差：0.