Marginalized （Rao-Blackwellized） Particle Filter

Marginalized (Rao-Blackwellized) Particle Filter的matlab代码，及对应的论文。

% 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