%?UKF算法
clear;
%?tic;
x?=?0;?%?初始狀態?
x_estimate?=?1;%狀態的估計
u_x_estimate?=?x_estimate;??%UKF的初始估計
Q?=?10;?%?過程狀態協方差?
R?=1;?%?測量噪聲協方差?
P?=5;%初始估計方差
u_P?=?P;%UKF方差
tf?=?50;?%?模擬長度?
x_array?=?[x];%真實值數組
u_x_estimate_array?=?[u_x_estimate];%UKF最優估計值數組
u_k?=?1;?%微調參數
u_symmetry_number?=?4;?%?對稱的點的個數
u_total_number?=?2?*?u_symmetry_number?+?1;?%總的采樣點的個數
linear?=?0.5;
close?all;

for?k?=?1?:?tf?
????%?模擬系統?
????x?=?linear?*?x?+?（25?*?x?/?（1?+?x^2））?+?8?*?cos（1.2*（k-1））?+?sqrt（Q）?*?randn;?%狀態值?
????y?=?（x^2?/?20）?+?sqrt（R）?*?randn;?%觀測值
????
????
????%不敏卡爾曼濾波器
????
???%采樣點的選取?存在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_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）;
????end
????%計算權值
????u_w_1?=?u_k/（u_symmetry_number+u_k）;
????u_w_N1?=?1/（2?*?（u_symmetry_number+u_k））;
????
????
????%把這些粒子通過傳遞方程?得到下一個狀態
????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）^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?+?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
????
%????%對傳遞后的均值和方差進行采樣??產生粒子?存在y（i）

%另外存在y_2obser（i）?中；
????for?i?=?1?:u_total_number
????????u_y_2obser（i:）?=?u_x_par（i）;
????end
????
????%通過觀測方程?得到一系列的粒子?
????for?i?=?1:?u_total_number
????????u_y_2obser（i）?=?u_y_2obser（i）^2/20;
????end
????%通過觀測方程后的均值?y_obse
????u_y_obse?=?u_w_1?*?u_y_2obser（1）;
????for?i?=?2?:?u_total_number
????????u_y_obse?=?u_y_obse?+?u_w_N1?*?u_y_2obser（i）;
????end
????%Pzz測量方差矩陣
????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
????????u_pzz?=?u_pzz?+?u_w_N1?*?（u_y_2obser（i）?-?u_y_obse）*（u_y_2obser（i）?-?u_y_obse）‘;
????end
????%Pxz狀態向量與測量值的協方差矩陣
????u_pxz?=?u_w_1?*?（u_x_par（1）?-?u_x_next）*?（u_y_2obser（1）-u_y_obse）‘;
????for?i?=?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;
???
???%估計量的更新
???u_x_next_optimal?=?u_x_next?+?u_K?*?（y?-?u_y_obse）;%第一步的估計值?+?修正值；
???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;
??

????%進行畫圖程序?
????x_array?=?[x_arrayx];
????u_x_estimate_array?=?[u_x_estimate_arrayu_x_estimate];
????
????u_error（k:）?=?abs（x_array（k）-u_x_estimate_array（k））;
?
end
????t?=?0?:?tf;
????figure;
????plot（tx_array‘k.‘tu_x_estimate_array‘b:‘）;
????set（gca‘FontSize‘10）;
????set（gcf‘color‘‘Yellow‘）;
????xlabel（‘time?step‘

?屬性????????????大小?????日期????時間???名稱
-----------?---------??----------?-----??----

?????文件???????3845??2012-03-15?21:25??UKF.m

-----------?---------??----------?-----??----

?????????????????3845????????????????????1