EKF、UKF、PF算法的比較程序

1、% EKF UKF PF 的三個(gè)算法 clear; % tic; x = 0.1; % 初始狀態(tài) x_estimate = 1;%狀態(tài)的估計(jì)e_x_estimate = x_estimate;  %EKF的初始估計(jì) u_x_estimate = x_estimate; %UKF的初始估計(jì) p_x_estimate = x_estimate

2、; %PF的初始估計(jì) Q = 10;%input('請(qǐng)輸入過(guò)程噪聲方差Q的值: '); % 過(guò)程狀態(tài)協(xié)方差 R = 1;%input('請(qǐng)輸入測(cè)量噪聲方差R的值: '); % 測(cè)量噪聲協(xié)方差  P =5;%初始估計(jì)方差 e_P = P; %UKF方差u_P = P;%UKF方差 pf_P = P;%PF方差 

3、;tf = 50; % 模擬長(zhǎng)度x_array = x;%真實(shí)值數(shù)組 e_x_estimate_array = e_x_estimate;%EKF最優(yōu)估計(jì)值數(shù)組 u_x_estimate_array = u_x_estimate;%UKF最優(yōu)估計(jì)值數(shù)組 p_x_estimate_array = p_x_estimate;%PF最優(yōu)估計(jì)值數(shù)組u_k = 1; %微調(diào)參數(shù)u_symmetry_number =&

4、#160;4; % 對(duì)稱的點(diǎn)的個(gè)數(shù) u_total_number = 2 * u_symmetry_number + 1; %總的采樣點(diǎn)的個(gè)數(shù)linear = 0.5; N = 500; %粒子濾波的粒子數(shù) close all; %粒子濾波初始 N 個(gè)粒子 for i = 1 : N p_xpart(i) =

5、 p_x_estimate + sqrt(pf_P) * randn; end for k = 1 : tf % 模擬系統(tǒng)x = linear * x + (25 * x / (1 + x2) + 8 * cos(1.2*(k-1) + sqrt(Q) * r

6、andn; %狀態(tài)值y = (x2 / 20) + sqrt(R) * randn; %觀測(cè)值  %擴(kuò)展卡爾曼濾波器 %進(jìn)行估計(jì) 第一階段的估計(jì)e_x_estimate_1 = linear * e_x_estimate + 25 * e_x_estimate /(1+e_x_estimate2) + 8 * cos(1.2*(k-1);

7、  e_y_estimate = (e_x_estimate_1)2/20; %這是根據(jù)k=1時(shí)估計(jì)值為1得到的觀測(cè)值；只這個(gè)由我估計(jì)得的第24行的y也是觀測(cè)值不過(guò)是由加了噪聲的真實(shí)值得到的 %相關(guān)矩陣e_A = linear + 25 * (1-e_x_estimate2)/(1+e_x_estimate2)2);%傳遞矩陣e_H = e_x_estimate_1/10; %觀測(cè)矩陣%估計(jì)的誤差e_p_estimate = 

8、e_A * e_P * e_A' + Q;  %擴(kuò)展卡爾曼增益 e_K = e_p_estimate * e_H'/(e_H * e_p_estimate * e_H' + R); %進(jìn)行估計(jì)值的更新 第二階段e_x_estimate_2 = e_x_estimate_1 + e_K * (y -

9、0;e_y_estimate);  %更新后的估計(jì)值的誤差e_p_estimate_update = e_p_estimate - e_K * e_H * e_p_estimate;   %進(jìn)入下一次迭代的參數(shù)變化 e_P = e_p_estimate_update;  e_x_estimate = e_x_estimate_2;   % 粒子濾波器  % 

10、粒子濾波器  for i = 1 : N   p_xpartminus(i) = 0.5 * p_xpart(i) + 25 * p_xpart(i) / (1 + p_xpart(i)2) + 8 * cos(1.2*(k-1) + sqrtQ) * randn; %這個(gè)式子比下面一行的效果好 %

11、 xpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)2) + 8 * cos(1.2*(k-1);   p_ypart = p_xpartminus(i)2 / 20; %預(yù)測(cè)值p_vhat = y - p_ypart;% 觀測(cè)和預(yù)測(cè)的

12、差 p_q(i) = (1 / sqrt(R) / sqrt(2*pi) * exp(-p_vhat2 / 2 / R); %各個(gè)粒子的權(quán)值  end   % 平均每一個(gè)估計(jì)的可能性p_qsum = sum(p_q);  for i = 1 : N  p_q(i) = p_q(i

13、) / p_qsum;%各個(gè)粒子進(jìn)行權(quán)值歸一化  end   % 重采樣 權(quán)重大的粒子多采點(diǎn)，權(quán)重小的粒子少采點(diǎn), 相當(dāng)于每一次都進(jìn)行重采樣；for i = 1 : N   p_u = rand;   p_qtempsum = 0; for j = 1 : N   p_qtempsum =

14、 p_qtempsum + p_q(j);   if p_qtempsum >= p_u p_xpart(i) = p_xpartminus(j); %在這里xpart(i) 實(shí)現(xiàn)循環(huán)賦值；終于找到了這里！break;  end end  end  p_x_estimate = mean(p_xpart);  % p_x_estimate&#

15、160;= 0;% for i = 1 : N % p_x_estimate =p_x_estimate + p_q(i)*p_xpart(i); % end %不敏卡爾曼濾波器 %采樣點(diǎn)的選取存在x(i)  u_x_par = u_x_estimate;  for i = 2 : (u_symmetry_number+1)&#

16、160;u_x_par(i,:) = u_x_estimate + sqrt(u_symmetry_number+u_k) * u_P); end for i = (u_symmetry_number+2) : u_total_number u_x_par(i,:) = u_x_estimate - sqrt(u_symmetry_number+u_k) * u_P);  en

17、d %計(jì)算權(quán)值u_w_1 = u_k/(u_symmetry_number+u_k); u_w_N1 = 1/(2 * (u_symmetry_number+u_k); %把這些粒子通過(guò)傳遞方程得到下一個(gè)狀態(tài)for i = 1: u_total_number u_x_par(i) = 0.5 * u_x_par(i) + 25 * u_x_par(i)/(1+u_x_par(i

18、)2) + 8 * cos(1.2*(k-1); end %傳遞后的均值和方差u_x_next = u_w_1 * u_x_par(1); for i = 2 : u_total_number u_x_next = u_x_next + u_w_N1 * u_x_par(i); end u_p_next = Q +&#

19、160;u_w_1 * (u_x_par(1)-u_x_next) * (u_x_par(1)-u_x_next)' for i = 2 : u_total_number u_p_next = u_p_next + u_w_N1 * (u_x_par(i)-u_x_next) * (u_x_par(i)-u_x_next)' end % %對(duì)傳遞后的均值和方差進(jìn)

20、行采樣產(chǎn)生粒子存在y(i) %        u_y_2obser(1) = u_x_next; % for i = 2 : (u_symmetry_number+1) %  u_y_2obser(i,:) = u_x_next + sqrt(u_symmetry_number+k) * u_p_next); % en

21、d % for i = (u_symmetry_number + 2) : u_total_number %  u_y_2obser(i,:) = u_x_next - sqrt(u_symmetry_number+u_k) * u_p_next); % end %另外存在y_2obser(i) 中；for i = 1 :u_total_numbe

22、r  u_y_2obser(i,:) = u_x_par(i); end %通過(guò)觀測(cè)方程得到一系列的粒子for i = 1: u_total_number  u_y_2obser(i) = u_y_2obser(i)2/20; end %通過(guò)觀測(cè)方程后的均值y_obse u_y_obse = u_w_1 * u_y_2obser(1); for i =

23、60;2 : u_total_number u_y_obse = u_y_obse + u_w_N1 * u_y_2obser(i); end %Pzz測(cè)量方差矩陣u_pzz = R + u_w_1 * (u_y_2obser(1)-u_y_obse)*(u_y_2obser(1)-u_y_obse)' for i = 2 : u_total_number&#

24、160;u_pzz = u_pzz + u_w_N1 * (u_y_2obser(i) - u_y_obse)*(u_y_2obser(i) - u_y_obse)' end %Pxz狀態(tài)向量與測(cè)量值的協(xié)方差矩陣u_pxz = u_w_1 * (u_x_par(1) - u_x_next)* (u_y_2obser(1)-u_y_obse)' for i =&#

25、160;2 : u_total_number u_pxz = u_pxz + u_w_N1 * (u_x_par(i) - u_x_next) * (u_y_2obser(i)- u_y_obse)' end %卡爾曼增益u_K = u_pxz/u_pzz; %估計(jì)量的更新u_x_next_optimal = u_x_next + u_K *

26、0;(y - u_y_obse);%第一步的估計(jì)值+ 修正值；u_x_estimate = u_x_next_optimal; %方差的更新 u_p_next_update = u_p_next - u_K * u_pzz * u_K' u_P = u_p_next_update;  %進(jìn)行畫圖程序x_array = x_array,x; e_x_estima

27、te_array = e_x_estimate_array,e_x_estimate; p_x_estimate_array = p_x_estimate_array,p_x_estimate; u_x_estimate_array = u_x_estimate_array,u_x_estimate; e_error(k,:) = abs(x_array(k)-e_x_estimate_array(k); p_error(k,:) = abs(x_arra

28、y(k)-p_x_estimate_array(k); u_error(k,:) = abs(x_array(k)-u_x_estimate_array(k); end t = 0 : tf; figure; plot(t,x_array,'k.',t,e_x_estimate_array,'r-',t,p_x_estimate_array,'g-',t,u_x_estimate_array,'b:'); set

29、(gca,'FontSize',10); set(gcf,'color','White'); xlabel('時(shí)間步長(zhǎng)');% lable ->label 我的神ylabel('狀態(tài)'); legend('真實(shí)值','EKF估計(jì)值','PF估計(jì)值','UKF估計(jì)值'); figure; plot(t,x_array,'k.',t,p_x_estimat

30、e_array,'g-', t, p_x_estimate_array-1.96*sqrt(P), 'r:', t, p_x_estimate_array+1.96*sqrt(P), 'r:'); set(gca,'FontSize',10); set(gcf,'color','White'); xlabel('時(shí)間步長(zhǎng)');% lable ->label 我的神ylabel('狀態(tài)'); legend('真實(shí)值','PF估計(jì)值&#