%%%?EKF?UKF?PF三種算法對比
clc
close?all
clear;
%?tic;
x?=?0.1;?%?初始狀態(tài)?
x_estimate?=?1;%狀態(tài)的估計
e_x_estimate?=?x_estimate;??%EKF的初始估計
u_x_estimate?=?x_estimate;??%UKF的初始估計
p_x_estimate?=?x_estimate;?%PF的初始估計
Q?=?10;%input（‘請輸入過程噪聲方差Q的值:?‘）;?%?過程狀態(tài)協(xié)方差?
R?=?1;%input（‘請輸入測量噪聲方差R的值:?‘）;?%?測量噪聲協(xié)方差?
P?=5;%初始估計方差
e_P?=?P;?%UKF方差
u_P?=?P;%UKF方差
pf_P?=?P;%PF方差
tf?=?50;?%?模擬長度?
x_array?=?[x];%真實值數組
e_x_estimate_array?=?[e_x_estimate];%EKF最優(yōu)估計值數組
u_x_estimate_array?=?[u_x_estimate];%UKF最優(yōu)估計值數組
p_x_estimate_array?=?[p_x_estimate];%PF最優(yōu)估計值數組
u_k?=?1;?%微調參數
u_symmetry_number?=?4;?%?對稱的點的個數
u_total_number?=?2?*?u_symmetry_number?+?1;?%總的采樣點的個數
linear?=?0.5;
N?=?500;?%粒子濾波的粒子數
close?all;
%粒子濾波初始?N?個粒子
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;?%觀測值
????????????%擴展卡爾曼濾波器
????%進行估計??第一階段的估計
????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;?%這是根據k=1時估計值為1得到的觀測值；只是這個由我估計得到的??第24行的y也是觀測值??不過是由加了噪聲的真實值得到的
????%相關矩陣
????e_A?=?linear?+?25?*?（1-e_x_estimate^2）/（（1+e_x_estimate^2）^2）;%傳遞矩陣
????e_H?=?e_x_estimate_1/10;?%觀測矩陣
????%估計的誤差
????e_p_estimate?=??e_A?*?e_P?*?e_A‘?+?Q;
????%擴展卡爾曼增益
????e_K?=?e_p_estimate?*?e_H‘/（e_H?*?e_p_estimate?*?e_H‘?+?R）;
????%進行估計值的更新??第二階段
????e_x_estimate_2?=?e_x_estimate_1?+?e_K?*?（y?-?e_y_estimate）;
????%更新后的估計值的誤差
????e_p_estimate_update?=?e_p_estimate?-?e_K?*?e_H?*?e_p_estimate;
????%進入下一次迭代的參數變化?
????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;?%這個式子比下面一行的效果好
%????????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;?%預測值?
????????p_vhat?=?y?-?p_ypart;%?觀測和預測的差?
????????p_q（i）?=?（1?/?sqrt（R）?/?sqrt（2*pi））?*?exp（-p_vhat^2?/?2?/?R）;?%各個粒子的權值
????end?
????%?平均每一個估計的可能性?
????p_qsum?=?sum（p_q）;?
????for?i?=?1?:?N?
????????p_q（i）?=?p_q（i）?/?p_qsum;%各個粒子進行權值歸一化?
????end?
???%?重采樣?權重大的粒子多采點，權重小的粒子少采點?相當于每一次都進行重采樣；
????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）?實現循環(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
????????%不敏卡爾曼濾波器
???????%采樣點的選取?存在x（i）
????u_x_par?=?u_x_estimate;
????for?i?=?2?:?（u_symmetry_number+1）
????????u_x_par（i:）?=?u_x_estimate?+?sqrt（（u_symmetry_num