function?ParticleEx1

%?Particle?filter?example?adapted?from?Gordon?Salmond?and?Smith?paper.

x?=?0.1;?%?initial?state
Q?=?1;?%?process?noise?covariance
R?=?1;?%?measurement?noise?covariance
tf?=?50;?%?simulation?length

N?=?100;?%?number?of?particles?in?the?particle?filter

xhat?=?x;
P?=?2;
xhatPart?=?x;

%?Initialize?the?particle?filter.
for?i?=?1?:?N
????xpart（i）?=?x?+?sqrt（P）?*?randn;
end

xArr?=?[x];
yArr?=?[x^2?/?20?+?sqrt（R）?*?randn];
xhatArr?=?[x];
PArr?=?[P];
xhatPartArr?=?[xhatPart];

close?all;

for?k?=?1?:?tf
????%?System?simulation
????x?=?0.5?*?x?+?25?*?x?/?（1?+?x^2）?+?8?*?cos（1.2*（k-1））?+?sqrt（Q）?*?randn;%狀態方程
????y?=?x^2?/?20?+?sqrt（R）?*?randn;%觀測方程
????%?Extended?Kalman?filter
????F?=?0.5?+?25?*?（1?-?xhat^2）?/?（1?+?xhat^2）^2;
????P?=?F?*?P?*?F‘?+?Q;
????H?=?xhat?/?10;
????K?=?P?*?H‘?*?（H?*?P?*?H‘?+?R）^（-1）;
????xhat?=?0.5?*?xhat?+?25?*?xhat?/?（1?+?xhat^2）?+?8?*?cos（1.2*（k-1））;%預測
????xhat?=?xhat?+?K?*?（y?-?xhat^2?/?20）;%更新
????P?=?（1?-?K?*?H）?*?P;
????%?Particle?filter
????for?i?=?1?:?N
????????xpartminus（i）?=?0.5?*?xpart（i）?+?25?*?xpart（i）?/?（1?+?xpart（i）^2）?+?8?*?cos（1.2*（k-1））?+?sqrt（Q）?*?randn;
????????ypart?=?xpartminus（i）^2?/?20;
????????vhat?=?y?-?ypart;%觀測和預測的差
????????q（i）?=?（1?/?sqrt（R）?/?sqrt（2*pi））?*?exp（-vhat^2?/?2?/?R）;
????end
????%?Normalize?the?likelihood?of?each?a?priori?estimate.
????qsum?=?sum（q）;
????for?i?=?1?:?N
????????q（i）?=?q（i）?/?qsum;%歸一化權重
????end
????%?Resample.
????for?i?=?1?:?N
????????u?=?rand;?%?uniform?random?number?between?0?and?1
????????qtempsum?=?0;
????????for?j?=?1?:?N
????????????qtempsum?=?qtempsum?+?q（j）;
????????????if?qtempsum?>=?u
????????????????xpart（i）?=?xpartminus（j）;
????????????????break;
????????????end
????????end
????end
????%?The?particle?filter?estimate?is?the?mean?of?the?particles.
????xhatPart?=?mean（xpart）;
????%?Plot?the?estimated?pdf‘s?at?a?specific?time.
????if?k?==?20
????????%?Particle?filter?pdf
????????pdf?=?zeros（811）;
????????for?m?=?-40?:?40
????????????for?i?=?1?:?N
????????????????if?（m?<=?xpart（i））?&&?（xpart（i）?????????????????????pdf（m+41）?=?pdf（m+41）?+?1;
????????????????end
????????????end
????????end
????????figure;
????????m?=?-40?:?40;
????????plot（m?pdf?/?N?‘r‘）;
????????hold;
????????title（‘Estimated?pdf?at?k=20‘）;
????????disp（[‘min?max?xpart（i）?at?k?=?20:?‘?num2str（min（xpart））?‘?‘?num2str（max（xpart））]）;
????????%?Kalman?filter?pdf
????????pdf?=?（1?/?sqrt（P）?/?sqrt（2*pi））?.*?exp（-（m?-?xhat）.^2?/?2?/?P）;
????????plot（m?pdf?‘b‘）;
????????legend（‘Particle?filter‘?‘Kalman?filter‘）;
????end
????%?Save?data?in?arrays?for?later?plotting
????xArr?=?[xArr?x];
????yArr?=?[yArr?y];
????xhatArr?=?[xhatArr?xhat];
????PArr?=?[PArr?P];
????xhatPartArr?=?[xhatPartArr?xhatPart];
end

t?=?0?:?tf;

%figure;
%plot（t?xArr）;
%ylabel（‘true?state‘）;

figure;
plot（t?xArr?‘b.‘?t?xhatArr?‘k-‘?t?xhatArr-2*sqrt（PArr）?‘r:‘?t?xhatArr+2*sqrt（PArr）

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

?????文件???????3636??2007-01-14?17:09??粒子濾波?目標跟蹤?一維情況下?非線性非高斯\ParticleEx1.m

?????文件??????28672??2008-07-04?10:52??粒子濾波?目標跟蹤?一維情況下?非線性非高斯\Doc1.doc

?????目錄??????????0??2008-07-04?09:50??粒子濾波?目標跟蹤?一維情況下?非線性非高斯

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

????????????????32526????????????????????4