SSM问题（State space models）即：
z为隐变量，y为观测量
% 状态转移函数：z(k) = f(z(k-1)) + v
% 状态观测函数：y(k) = h(z(k)) + n
% 已知：y(1:k)
% 目标：计算z的估计值
当函数f和h为线性变换时，此为线性高斯SSM（LG-SSM）问题或线性动态系统（LDS）问题，详见MLaPP式（18.3）-（18.6），可用卡尔曼滤波（KF：Kalman filter）解析求解。详见：
http://blog.csdn.net/foreseerwang/article/details/77883616
http://blog.csdn.net/foreseerwang/article/details/78982386
当f和h为非线性，无解析解，可用EKF（extended Kalman filter）、UKF（unscented Kalman filter）和PF（particle filter,粒子滤波）近似或数值求解。在此分别介绍个人理解并给出简单范例。
EKF：理念非常简单。把函数f和h用一阶泰勒展开，形成关于z(t-1)和z(t)的线性公式，代入MLaPP（18.3）节中的卡尔曼滤波推导过程，即可得到EKF的迭代结果，式（18.88）-（18.92）。
UKF：按照我的理解，UKF和粒子滤波有相似之处，同样是找一些抽样点（UKF中用sigma点，而PF中是蒙特卡洛抽样），不同之处是UKF在每次状态转移或观测之后仍然假设是高斯分布，而PF则不做任何分布上的假设，只是根据新的分布重新做蒙特卡洛抽样。UKF的抽样点经过非线性函数变换后，仍然假设为高斯分布，其均值和方差可由非线性变换后的抽样点计算得到，状态转移过程见MLaPP书中的式（18.99）-（18.102）；观测过程见MLaPP书中的式（18.103）-（18.106）。由于各节点均为高斯分布，即可通过贝叶斯公式得到z的后验分布，同样为高斯分布，详见MLaPP书中的式（18.107）-（18.110）。
PF：详见http://blog.csdn.net/foreseerwang/article/details/78889888 （这个文章号还挺吉利）。
下面给出上述三种方法针对 http://blog.csdn.net/foreseerwang/article/details/78889888 中的非线性SSM问题的求解。需要注意的是，这是一个简单的一维问题，下述示例程序没有针对可能的高维问题给出通用代码，如果需要实际应用，请小心改写。此外：UKF中sigma采样点的选取涉及一些参数的设定，MLaPP中没有细说，如有必要，请自行参考相关书籍或文章。
结果输出：很明显，粒子滤波可以达到更好的效果；但所需的计算时间更长。
==============================================================
EKF: Extended Kalman filtering (MLaPP, 18.5.1)
EKF预测误差均值： 5.282；方差： 8.515
EKF耗时： 0.036
==============================================================
UKF: Unscented Kalman filtering (MLaPP, 18.5.2)
UKF预测误差均值： 5.799；方差： 7.697
UKF耗时： 0.035
==============================================================
PF: Particle filtering (MLaPP, 23.5)
PF预测误差均值： 2.093；方差： 3.789
PF耗时： 0.552
代码：
%% SSM Inference using EKF, UKF and PF:
% 2018.01.08
% QQ: 50834
% http://blog.csdn.net/foreseerwang
%
% 针对SSM (State space models)，z为隐变量，y为观测量
% 状态转移函数：z(k) = f(z(k-1)) + v
% 状态观测函数：y(k) = h(z(k)) + n
% 已知：y(1:k)
% 目标：分别使用如下方法计算z的估计值
% 1 EKF: extended Kalman filter
% 2 UKF: unscented Kalman filter
% 3 PF: particle filter
close all;
clear all;
rng(0);
% 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_f_H = @(z)(25./(z.^2 + 1) - (50*z.^2)./(z.^2 + 1).^2 + 1/2); % 函数f的一阶导
fun_h = @(z)(z.^2/20); % 观测函数
fun_h_H = @(z)(z/10); % 观测函数h的一阶导
fun_h_I = @(y)(sqrt(abs(20*y))); % 观测函数h的反函数
fun_p = @(x,mu,sig)(normpdf(x,mu,sig)); % 概率函数
fun_n = @(N,mu,sig)(randn(1,N)*sig+mu); % 高斯噪声
% 参数初始化
T = 75; % 共进行75次迭代
% 真实高斯误差参数
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 tt = 2:T,
z(tt) = fun_f(z(tt-1),tt)+fun_n(1,v_mu,v_sig);
end;
y = fun_h(z) + fun_n(T,n_mu,n_sig);
% 后面除了打印显示，不应该再用到z和v/n的均值和方差
%% 1. Extended Kalman Filter
tic;
fprintf('==============================================================\n');
fprintf('EKF: Extended Kalman filtering (MLaPP, 18.5.1)\n');
muEKF = zeros(1, T);
muEKF(1) = fun_h_I(y(1));
siEKF = zeros(1, T);
siEKF(1) = rand;
for tt=2:T,
mu_bar = fun_f(muEKF(tt-1), tt); % MLaPP 18.88
Gt = fun_f_H(muEKF(tt-1));
si_bar = Gt*siEKF(:,tt-1)*Gt'+qv_sig.^2; % MLaPP 18.89
%计算卡尔曼增益
Ht = fun_h_H(mu_bar);
K = si_bar*Ht'*pinv(Ht*si_bar*Ht'+ qn_sig.^2); % MLaPP 18.90
% 根据卡尔曼增益和当前观测值修订预测值，获得当前最优预测值
muEKF(:,tt) = mu_bar + K*(y(:,tt)-fun_h(mu_bar)); % MLaPP 18.91
siEKF(:,:,tt) = (1-K*Ht)*si_bar; % MLaPP 18.92
end;
errEKF = muEKF-z;
fprintf('EKF预测误差均值：%6.3f；方差：%6.3f\n', mean(abs(errEKF)), std(errEKF));
fprintf('EKF耗时：%6.3f\n', toc);
%% 2. Unscented Kalman Filter
tic;
fprintf('==============================================================\n');
fprintf('UKF: Unscented Kalman filtering (MLaPP, 18.5.2)\n');
% 计算（18.96）-（18.98）
% 此处参考书中给出的d=1情况下的alpha/beta/kappa值
% 注意：书中这三个公式中w的上标有误，其中（18.96）和（18.97）的上标应为i=0
% （18.98）的上标应为i~=0
d = 1;
alpha = 1;
beta = 0;
kappa = 2;
lambda = alpha^2*(d+kappa) - d;
w0 = lambda/(d+lambda);
wi = 1.0/2/(d+lambda);
w = [w0, wi, wi];
muUKF = zeros(1, T);
muUKF(1) = fun_h_I(y(1));
siUKF = zeros(1, T);
siUKF(1) = rand;
for tt = 2:T,
z0 = [muUKF(tt-1), muUKF(tt-1)+sqrt(3*siUKF(tt-1)), ...
muUKF(tt-1)-sqrt(3*siUKF(tt-1))]; % (18.99)
z_bar_star = fun_f(z0, tt); % (18.100)
mubar = w*z_bar_star'; % (18.101)
sibar = w*((z_bar_star - mubar).*(z_bar_star - mubar))'...
+ qv_sig.^2; % (18.102)
% (18.103) - (18.106)
z0bar = [mubar, mubar+sqrt(3*sibar), mubar-sqrt(3*sibar)];
y_bar_star = fun_h(z0bar);
y_caret = w*y_bar_star';
St = w*((y_bar_star-y_caret).*(y_bar_star-y_caret))' + qn_sig.^2;
% (18.107) - (18.110)
sibar_zy = w*((z_bar_star-mubar).*(y_bar_star-y_caret))';
Kt = sibar_zy*pinv(St);
muUKF(tt) = mubar + Kt*(y(tt)-y_caret);
siUKF(tt) = sibar - Kt*St*Kt;
end;
errUKF = muUKF-z;
fprintf('UKF预测误差均值：%6.3f；方差：%6.3f\n', mean(abs(errUKF)), std(errUKF));
fprintf('UKF耗时：%6.3f\n', toc);
%% 3. Particle filter
tic;
fprintf('==============================================================\n');
fprintf('PF: Particle filtering (MLaPP, 23.5)\n');
S = 1000; % 粒子数，越大效果越好，计算量也越大
zs = fun_n(S, 0, qv_sig); % 粒子
zPF = zeros(size(z));
zPF(1) = mean(zs); % 真实位置估计
for tt = 2:T,
zs_new = fun_f(zs,tt)+fun_n(S,v_mu,qv_sig);
ys_new = fun_h(zs_new);
% MLaPP (23.37)，重采样之后，ws相等，因此不再*ws(t-1)
ws = fun_p(y(tt),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的权重相等了，因此期望值直接取均值即可
% figure; hist(zs,100);
zPF(tt) = mean(zs);
end;
errPF = zPF-z;
fprintf('PF预测误差均值：%6.3f；方差：%6.3f\n', mean(abs(errPF)), std(errPF));
fprintf('PF耗时：%6.3f\n', toc);
%% 结果打印
figure( 'name', 'Particle Filtering Demo'); hold on;
plot(z,'-.k','linewidth',3);
plot(muEKF, '-.r','linewidth',3);
plot(muUKF, '-.b','linewidth',3);
plot(zPF, '-.g','linewidth',3);
set(gca,'FontSize',12); set(gcf,'Color','White');
xlabel('Time step'); ylabel('Flight position');
h=legend('True flight position', 'Extended Kalman Filter',...
'Unscented Kalman filtering', 'Particle filter estimate');
set(h,'Fontsize',20);