Rao+ Blackwellized PF

Rao+ Blackwellized PF 混合粒子滤波程序matlab代码

% PURPOSE : PF and RBPF for conditionally Gaussian JMLS.

% COPYRIGHT : Nando de Freitas    
% DATE      : June 2001

clear;
echo off;

% =======================================================================
%              INITIALISATION AND PARAMETERS
% =======================================================================

N = 100;                     % Number of particles.
T = 50;                     % Number of time steps.

% Here we give you the choice to try three different types of
% resampling algorithms: multinomial （select 3） residual （1） and 
% deterministic （2）. Note that the code for these O（N） algorithms is generic.

resamplingScheme = 2;    

n_x = 1;                    % Continuous state dimension.
n_z = 3;                    % Number of discrete states.
n_y = 2;                    % Dimension of observations.

par.A = zeros（n_xn_xn_z）;
par.B = zeros（n_xn_xn_z）;
par.C = zeros（n_yn_xn_z）;
par.D = zeros（n_yn_yn_z）;
par.E = zeros（n_xn_xn_z）;
par.F = zeros（n_x1n_z）;
par.G = zeros（n_y1n_z）;
for i=1:n_z
  par.A（::i） = i*randn（n_xn_x）;
  par.C（::i） = i*randn（n_yn_x）;
  par.B（::i） = 0.01*eye（n_xn_x）;    
  par.D（::i） = 0.01*eye（n_yn_y）;    
  par.F（::i） = （1/n_x）*zeros（n_x1）;
  par.G（::i） = （1/n_y）*zeros（n_y1）;   
end;

par.T = unidrnd（10n_zn_z）;           % Transition matrix.
for i=1:n_z
  par.T（i:） = par.T（i:）./sum（par.T（i:））; 
end;

par.pz0 = unidrnd（10n_z1）;            % Initial discrete distribution. Random numbers from the discrete uniform distribution
par.pz0 = par.pz0./sum（par.pz0）; 
par.mu0 = zeros（n_x1）;                 % Initial Gaussian mean.
par.S0  = 0.1*eye（n_xn_x）;             % Initial Gaussian covariance.  



% I sometimes set some of the following matrices by hand:
%par.T = [.1 .9; 
%	 .1 .9];
%par.pz0 = [.5 .5]‘;   

%par.T = [.1 .5 .4; 
%	 .1 .6 .3
%	 .1 .3 .6];
%par.pz0 = [.5 .5 .5]‘;   



% =======================================================================
%                          GENERATE THE DATA
% =======================================================================

x = zeros（n_xT）;
z = zeros（1T）;
y = zeros（n_yT）;
u = zeros（1T）;           % Control signals.

x（:1） = par.mu0 + sqrtm（par.S0）*randn（n_x1）;
z（1） = length（find（cumsum（par.pz0‘）for t=2:T
  z（t） = length（find（cumsum（par.T（z（t-1）:）‘）  x（:t） = par.A（::z（t））*x（:t-1） + par.B（::z（t））*randn（n_x1） + par.F（::z（t））*u（:t）; 
  y（:t） = par.C（::z（t））*x（:t） + par.D（::z（t））*randn（n_y1） + par.G（::z（t））*u（:t）; 
end;

figure（1）
clf
subplot（311）
plot（1:Tz‘r‘‘linewidth‘2）;
ylabel（‘z_t‘‘fontsize‘15）;
axis（[0 T+1 0 n_z+1]）
grid on;
subplot（312）
plot（1:Tx‘r‘‘linewidth‘2）;
ylabel（‘x_t‘‘fontsize‘15）;
grid on;
subplot（313）
plot（1:Ty‘r‘‘linewidth‘2）;
ylabel（‘y_t‘‘fontsize‘15）;
xlabel（‘t‘‘fontsize‘15）;
grid on;

fprintf（‘\n‘）
fprintf（‘\n‘）
fprintf（‘Estimation has started‘）
fprintf（‘\n‘）


% =================================================================

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      10528  2008-03-18 19:44  demo_rbpf_gauss\demo_rbpf_gauss\demo_rbpf.asv

     文件      10528  2008-03-18 19:47  demo_rbpf_gauss\demo_rbpf_gauss\demo_rbpf.m

     文件       1099  2001-11-27 04:10  demo_rbpf_gauss\demo_rbpf_gauss\deterministicR.m

     文件       1134  2001-11-27 04:10  demo_rbpf_gauss\demo_rbpf_gauss\multinomialR.m

     文件       1342  2001-11-27 04:10  demo_rbpf_gauss\demo_rbpf_gauss\residualR.m

     目录          0  2008-04-23 19:30  demo_rbpf_gauss\demo_rbpf_gauss

     目录          0  2008-04-23 19:30  demo_rbpf_gauss

----------- ---------  ---------- -----  ----

                24849                    8