# 粒子滤波（particle filtering）梳理

1. Importance sampling

Importance sampling主要是为了解决 I = E[f] = integral(f(x)*p(x)) 的问题，其中p(x)不能直接抽样，但可以直接计算。

2. Sampling importance resampling (SIR)

3. Particle filtering（粒子滤波）

%% Monte Carlo Inference (MLaPP, Chapter 23):
% 2017.12.22
% QQ: 50834
% http://blog.csdn.net/foreseerwang

% 1. Importance sampling
% 1.1 Basic idea (MLaPP, 23.4.1)
% 1.2 Unnormalized distributions (MLaPP, 23.4.2)
% 1.3 SIR: Sampling importance resampling (MLaPP, 23.4.4)
%
% 2. Particle filter
% 2.1 Sequential importance sampling
% 2.2 Resampling
% 2.3 Proposal distribution

close all;
clear all;
rng(0);

%% 1. Importance sampling
fprintf('==============================================================\n');
fprintf('Importance sampling (MLaPP, 23.4)\n');
% 各种函数定义
fun_px_pdf = @(x)(normpdf(x, 5, 0.4)*0.7+normpdf(x, -7, 2)*0.3);
fun_qx_smp = @(x)(randn(x,1)*5);
fun_qx_pdf = @(x)(normpdf(x, 0, 5));
fun_fx = @(x)(1./(1+exp(-x)));

% 初始化
S = 10000;          % 抽样数量（粒子数目）
N = 1;              % 重复次数
S_SIR = round(S/10);    % SIR数量，MLaPP (23.34)中的S'，远小于S

% f(x)相对于p(x)的理论均值
dx = 0.1;
x = -20:dx:20;
px = fun_px_pdf(x);
fx = fun_fx(x);

E_f = sum(fx.*px)*dx;
fprintf('Expectation of f(x) is: %5.3f\n', E_f);
figure; scatter(x,px); hold on; scatter(x,fx);
legend('p(x)', 'f(x)');
set(gca,'FontSize',12); set(gcf,'Color','White');

% 1.1和1.2，对应MLaPP 23.4.1和23.4.2
E_f1 = 0;
E_f2 = 0;
for ii = 1:N,
% 引入已知分布q(x)，其核心思想是要用q(x)抽样
% 本例中q(x) = norm(x|0,5)
x_samp = fun_qx_smp(S);
px_samp = fun_px_pdf(x_samp);
qx_samp = fun_qx_pdf(x_samp);

% Basic idea
ws = px_samp./qx_samp;
fx_samp = fun_fx(x_samp);
E_f1 = E_f1*(ii-1)/ii+sum(ws.*fx_samp)/S/ii;        % MLaPP (23.20)

% Handling unnormalized distributions
ws2 = ws/sum(ws);                                   % MLaPP (23.27)
E_f2 = E_f2*(ii-1)/ii+sum(ws2.*fx_samp)/ii;         % MLaPP (23.26)
end;
% 这两种方法的计算结果应该一样
fprintf('Expectation of f(x) using IS (basic idea) is: %5.3f\n', E_f1);
fprintf('Expectation of f(x) using IS (normalized) is: %5.3f\n', E_f2);

% SIR: Sampling importance resampling，对应MLaPP 23.4.4
x_SIR = zeros(1,S_SIR);
p_SIR = zeros(1,S_SIR);
for ii = 1 : S_SIR,
tmp_idx = find(rand <= cumsum(ws2),1);
x_SIR(ii) = x_samp(tmp_idx);
p_SIR(ii) = ws2(tmp_idx);
end
p_SIR = p_SIR/sum(p_SIR);

figure( 'name', 'SIR: Sampling importance resampling');
set(gca,'FontSize',12); set(gcf,'Color','White');
subplot(1,2,1);
hist(x_SIR,100);
xlabel('x'); ylabel('p(x)');
title('Resampling Results');
subplot(1,2,2);
scatter(x_SIR, p_SIR);
xlabel('x'); ylabel('p(x)');
title('p(x) after resampling');

%% 2. Particle filter
% 针对SSM (State space models)，z为隐变量，y为观测量
% 状态转移函数：z(k) = f(z(k-1)) + v
% 状态观测函数：y(k) = h(z(k)) + n
% 已知：y(1:k)
% 目标：计算p(z(k)|y(1:k))，进而求得z的估计值

fprintf('\n==============================================================\n');
fprintf('Particle filtering (MLaPP, 23.5)\n');
clear all;
% function definition
% Ref. to: http://blog.csdn.net/liujiakuino1/article/details/54343527
% 在真实的应用中，f和h应该是知道的，但v、n未知
fun_f = @(z,t)(0.5*z + 25*z./(1 + z.^2) + 8*cos(1.2*(t-1)));
fun_v = @(N,mu,sig)(randn(1,N)*sig+mu);
fun_h = @(z)(z.^2/20);
fun_n = @(N,mu,sig)(randn(1,N)*sig+mu);
fun_p = @(x,mu,sig)(normpdf(x,mu,sig));

% 参数初始化
T = 75;     % 共进行75次迭代
S = 1000;   % 粒子数，越大效果越好，计算量也越大

% 真实高斯误差参数
v_mu = 0; v_sig = 1;
n_mu = 0; n_sig = 1;
% 预测高斯误差参数
qv_mu = 0; qv_sig = 2;
qn_mu = 0; qn_sig = 2;

% 生成并显示状态序列z和观测序列y
z = zeros(1,T);
z(1) = 0.1;
for ii = 2:T,
z(ii) = fun_f(z(ii-1),ii)+fun_v(1,v_mu,v_sig);
end;
y = fun_h(z) + fun_n(T,n_mu,n_sig);

% 后面除了打印显示，不应该再用到z和v/n的均值和方差
zs = fun_v(S, y(1), qv_sig);                            % 粒子
z_est = zeros(size(z));
z_est(1) = mean(zs);                                    % 真实位置估计

for ii = 2:T,
zs_new = fun_f(zs,ii)+fun_v(S,v_mu,qv_sig);
ys_new = fun_h(zs_new);

% MLaPP (23.37)，重采样之后，ws相等，因此不再*ws(t-1)
ws = fun_p(y(ii),ys_new,qn_sig);
ws = ws/sum(ws);                            % MLaPP (23.38)

for i = 1 : S
zs(i) = zs_new(find(rand <= cumsum(ws),1)); % 根据ws重采样
end

% 重采样之后，各zs的权重相等了，因此期望值直接取均值即可
z_est(ii) = mean(zs);
end;

figure( 'name', 'Particle Filtering Demo'); hold on;
plot(z,'-.b','linewidth',3); plot(z_est, '-.g','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time step'); ylabel('Flight position');
legend('True flight position', 'Particle filter estimate');