%?Illustrating?the?use?of?the?marginalized?（a.k.a.?the?Rao-Blackwellized）
%?particle?filter?by?solving?a?specific?example.
%
%?All?the?references?to?equations?within?this?code?are?with?respect?to?the?
%?paper
%?
%?Thomas?Sch鰊?Fredrik?Gustafsson?and?Per-Johan?Nordlund.?Marginalized?
%?Particle?Filters?for?Mixed?Linear/Nonlinear?State-Space?Models.?IEEE?
%?Transactions?on?Signal?Processing?53（7）:2279-2289?Jul.?2005.
%
%?If?you?use?this?code?for?academic?work?please?reference?the?above?paper.?
%?
%?Note?that?this?code?has?been?written?in?order?to?facilitate?the?
%?understanding?of?the?Rao-Blackwellized?particle?filter?via?solving?a?
%?specific?example.?Hence?the?code?is?not?written?not?to?obtain?a?general
%?implementation?nor?is?it?written?to?obtain?the?fastest?and?most?optimized?
%?code?possible.
%
%?Written?by:
%??????????????Thomas?Sch鰊?（schon@isy.liu.se）
%??????????????Division?of?Automatic?Control
%??????????????link鰌ing?University
%??????????????www.control.isy.liu.se/~schon
%??????????????Last?revised?on?August?19?2011
%

clear;
%============================
%===???Define?the?model???===
%============================
m.f??=?inline（‘[1*atan（x（1:））?+?1*x（2:）;?1*x（2:）?+?0.3*x（3:）;?0.92*x（3:）-0.3*x（4:）;?0.3*x（3:）+0.92*x（4:）]‘）;??%?Dynamic?model
m.h??=?inline（‘[（0.1*x（1:）.^2）.*sign（x（1:））;?x（2:）?-?x（3:）?+?x（4:）]‘）;???????????%?Measurement?model
m.x0?=?zeros（41）;?????????????%?Initial?state
m.P0?=?（1e-6）*eye（4）;??????????%?Covariance?for?the?initial?state
m.P0（11）?=?1;
m.R??=?0.1*eye（2）;?????????????%?Measurement?noise?covariance
m.Q??=?0.01*eye（4）;????????????%?Process?noise?covariance

m.nx?=?4;??????????????????????%?State?dimension
m.nxn?=?1;?????????????????????%?Nonlinear?state?dimension
m.nxl?=?3;?????????????????????%?Linear?state?dimension
m.ny?=?2;??????????????????????%?Measurement?dimension

%?Define?model?to?be?used?in?the?MPF?see?eq.?（18-19）.
m.An???=?[1?0?0];
m.Al???=?[1??0.3????0;
??????????0??0.92?-0.3;?
??????????0??0.3??0.92];
m.C????=?[0?0??0;
??????????1?-1?1];

%==============================
%===???Simulate?the?model???===
%==============================
Tfinal?=?200;??????????????????%?Number?of?samples
MC?????=?10;???????????????????%?Number?of?Monte?Carlo?simulations
for?i=1:MC
??x?=?zeros（m.nxTfinal+1）;
??y?=?zeros（m.nyTfinal）;
??x（:1）?=?m.x0?+?sqrtm（m.P0）*randn（m.nx1）;
??for?t=1:Tfinal
????x（:t+1）?=?feval（m.fx（:t））+sqrtm（m.Q）*randn（m.nx1）;
????y（:t）???=?feval（m.hx（:t））+sqrtm（m.R）*randn（m.ny1）;
??end
??z{i}.xTrue?=?x（:1:Tfinal）;?????%?Store?the?true?states
??z{i}.y?????=?y（:1:Tfinal）;?????%?Store?the?measurements
end;

%==========================================================
%===???Compute?the?estimates?using?the?PF?and?the?MPF???===
%==========================================================
Npf??=?200;???????????%?Number?of?particles?in?the?particle?filter
Nmpf?=?Npf;???????????%?Number?of?particles?in?the?MPF
for?i=1:MC
??disp（[‘Monte?Carlo?iteration:?‘?num2str（i）]）
??gPF{i}?

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

?????文件???????4240??2011-08-19?15:58??RBPF實例\mpf\experiment.m

?????文件???????3585??2011-08-19?16:02??RBPF實例\mpf\mpf.m

?????文件???????1636??2010-11-04?19:23??RBPF實例\mpf\pf.m

?????文件????????176??2008-08-23?20:50??RBPF實例\mpf\sysresample.m

?????文件?????136086??2018-04-24?15:08??RBPF實例\MPFexample.pdf

?????文件?????588535??2018-04-24?15:15??RBPF實例\schongn2005.pdf

?????目錄??????????0??2018-04-24?15:20??RBPF實例\mpf

?????目錄??????????0??2018-04-24?15:21??RBPF實例

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

???????????????734258????????????????????8