%?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;?%PF的初始估計(jì)
Q?=?10;%input（‘請(qǐng)輸入過程噪聲方差Q的值:?‘）;?%?過程狀態(tài)協(xié)方差?
R?=?1;%input（‘請(qǐng)輸入測(cè)量噪聲方差R的值:?‘）;?%?測(cè)量噪聲協(xié)方差?
P?=5;%初始估計(jì)方差
e_P?=?P;?%EKF方差
u_P?=?P;%UKF方差
pf_P?=?P;%PF方差
tf?=?50;?%?模擬長度?
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;?%?對(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）?=?p_x_estimate?+?sqrt（pf_P）?*?randn;
end
for?k?=?1?:?tf?
????%?模擬系統(tǒng)?
????x?=?linear?*?x?+?（25?*?x?/?（1?+?x^2））?+?8?*?cos（1.2*（k-1））?+?sqrt（Q）?*?randn;?%狀態(tài)值?
????y?=?（x^2?/?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_estimate^2）?+?8?*?cos（1.2*（k-1））;
????e_y_estimate?=??（e_x_estimate_1）^2/20;?%這是根據(jù)k=1時(shí)估計(jì)值為1得到的觀測(cè)值；只是這個(gè)由我估計(jì)得到的??第24行的y也是觀測(cè)值??不過是由加了噪聲的真實(shí)值得到的
????%相關(guān)矩陣
????e_A?=?linear?+?25?*?（1-e_x_estimate^2）/（（1+e_x_estimate^2）^2）;%傳遞矩陣
????e_H?=?e_x_estimate_1/10;?%觀測(cè)矩陣
????%估計(jì)的誤差
????e_p_estimate?=??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?-?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;????????
???%%%%%%%%%%%%%%%%%%%%%%%%%%%粒子濾波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
??????????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））?+?sqrt（Q）?*?randn;?%這個(gè)式子比下面一行的效果好
%????????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è)的差?
????????p_q（i）?=?（1?/?sqrt（R）?/?sqrt（2*pi））?*?exp（-p_vhat^2?/?2?/?R）;?%各個(gè)粒子的權(quán)值
????end?
????%?平均每一個(gè)估計(jì)的可能性?
????p_qsum?=?sum（p_q）;?
????for?i?=?1?:?N?
????????p_q（i）?=?p_q（i）?/?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?=?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?=?0;
%?????for?i?=?1?:?N
%?????????p_x_estimate?=p_x_estimate?+?p_q（i）*p_xpart（i）;
%?????end
?%%%%%%%%%%%%%%%%%%%%%%%%%%%%不敏卡爾曼濾波器%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%