%%?SIR粒子濾波的應用，算法流程參見博客http://blog.csdn.net/heyijia0327/article/details/40899819
clear?all
close?all
clc
%%?initialize?the?variables
x?=?0.1;?%?initial?actual?state
x_N?=?1;?%?系統過程噪聲的協方差??（由于是一維的，這里就是方差）
x_R?=?1;?%?測量的協方差
T?=?75;??%?共進行75次
N?=?100;?%?粒子數，越大效果越好，計算量也越大

%initilize?our?initial?prior?particle?distribution?as?a?gaussian?around
%the?true?initial?value

V?=?2;?%初始分布的方差
x_P?=?[];?%?粒子
%?用一個高斯分布隨機的產生初始的粒子
for?i?=?1:N
????x_P（i）?=?x?+?sqrt（V）?*?randn;
end

z_out?=?[x^2?/?20?+?sqrt（x_R）?*?randn];??%實際測量值
x_out?=?[x];??%the?actual?output?vector?for?measurement?values.
x_est?=?[x];?%?time?by?time?output?of?the?particle?filters?estimate
x_est_out?=?[x_est];?%?the?vector?of?particle?filter?estimates.

for?t?=?1:T
????x?=?0.5*x?+?25*x/（1?+?x^2）?+?8*cos（1.2*（t-1））?+??sqrt（x_N）*randn;
????z?=?x^2/20?+?sqrt（x_R）*randn;
????for?i?=?1:N
????????%從先驗p（x（k）|x（k-1））中采樣
????????x_P_update（i）?=?0.5*x_P（i）?+?25*x_P（i）/（1?+?x_P（i）^2）?+?8*cos（1.2*（t-1））?+?sqrt（x_N）*randn;
????????%計算采樣粒子的值，為后面根據似然去計算權重做鋪墊
????????z_update（i）?=?x_P_update（i）^2/20;
????????%對每個粒子計算其權重，這里假設量測噪聲是高斯分布。所以?w?=?p（y|x）對應下面的計算公式
????????P_w（i）?=?（1/sqrt（2*pi*x_R））?*?exp（-（z?-?z_update（i））^2/（2*x_R））;
????end
????%?歸一化.
????P_w?=?P_w./sum（P_w）;
??
????%%?Resampling
????%這里沒有用博客里之前說的histc函數，不過目的和效果是一樣的
????for?i?=?1?:?N
????????x_P（i）?=?x_P_update（find（rand?<=?cumsum（P_w）1））;???%?粒子權重大的將多得到后代
????end?????????????????????????????????????????????????????%?find（?1）?返回第一個?符合前面條件的數的?下標
????
????%狀態估計，重采樣以后，每個粒子的權重都變成了1/N
????x_est?=?mean（x_P）;
????
????%?Save?data?in?arrays?for?later?plotting
????x_out?=?[x_out?x];
????z_out?=?[z_out?z];
????x_est_out?=?[x_est_out?x_est];
????
end

t?=?0:T;
figure（1）;
clf
plot（t?x_out?‘.-b‘?t?x_est_out?‘-.r‘‘linewidth‘3）;
set（gca‘FontSize‘12）;?set（gcf‘Color‘‘White‘）;
xlabel（‘time?step‘）;?ylabel（‘flight?position‘）;
legend（‘True?flight?position‘?‘Particle?filter?estimate‘）;

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

????.......????????28??2011-11-26?14:14??particle_filter_demo\particle_filter_demo-master\.gitignore

????.......??????4278??2011-11-26?14:14??particle_filter_demo\particle_filter_demo-master\draw.py

?????文件???????4788??2014-10-22?23:05??particle_filter_demo\particle_filter_demo-master\draw.pyc

????.......??????8263??2011-11-26?14:14??particle_filter_demo\particle_filter_demo-master\particle_filter.py

????.......??????1358??2011-11-26?14:14??particle_filter_demo\particle_filter_demo-master\README

?????文件???????2211??2014-11-15?11:33??particle_filter_demo\Particle_filter_SIR_demo1.m

?????文件????????713??2014-10-24?22:48??particle_filter_demo\PF_Video_EN\calc_log_likelihood.m

?????文件???????1680??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\calc_log_likelihood_mex_handcoded.cpp

?????文件????????328??2014-10-24?20:32??particle_filter_demo\PF_Video_EN\create_particles.m

?????文件???????1052??2014-11-15?11:35??particle_filter_demo\PF_Video_EN\particle_filter_by_saved_movie.m

?????文件????????999??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\particle_filter_by_usb_camera.m

?????文件?????964717??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\Person.wmv

?????文件????????261??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\Readme.txt

?????文件????????322??2014-10-24?21:06??particle_filter_demo\PF_Video_EN\resample_particles.m

?????文件????????153??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\show_particles.m

?????文件????????260??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\show_state_estimated.m

?????文件?????857622??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\Slides_LongVersion.pdf

?????文件?????569154??2014-10-23?21:09??particle_filter_demo\PF_Video_EN\Slides_ShortVersion.pdf

?????文件????????258??2014-10-24?20:35??particle_filter_demo\PF_Video_EN\update_particles.m

?????目錄??????????0??2014-10-22?23:05??particle_filter_demo\particle_filter_demo-master

?????目錄??????????0??2014-11-15?11:42??particle_filter_demo\PF_Video_EN

?????目錄??????????0??2018-11-24?16:31??particle_filter_demo

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

??????????????2418445????????????????????22