贝叶斯滤波在机器人领域的应用

本片文章主要依据我本人在学习贝叶斯滤波器的体会，总结记录贝叶斯滤波（Bayes Filter）、卡尔曼滤波（Kalman Filter）、粒子滤波（Particle Filter）的基本原理、公式推导以及在机器人定位领域的应用。

1 贝叶斯滤公式

1.1 贝叶斯公式的推导

先回顾一下大学时期学习的《概率论与数理统计》知识：

1. 概率： 设在一次随机试验中，$x$为其中任一事件，则定义$p(x)$表示事件$x$发生的概率。

2. 联合概率： 设$x$、$y$是随机试验的任意两事件，则两事件同时成立的概率可以表示为：
   $$p(x,y)=p(xy)=p(x\cap y)=p(y)p(x|y)=p(x)p(y|x)$$
   此时，如果$x$、$y$相互独立，则：$p(x,y)=p(x)p(y)$

3. 条件概率： 设$x$、$y$是随机试验的任意两事件，则在$y$事件发生的条件下$x$事件发生的概率可以表达为：
   $$p(x|y)=\frac{p(x,y)}{p(y)}$$
   此时，如果$x$、$y$相互独立，则：$p(x|y)=p(x)$

4. 全概率公式：
   连续的情况下：
   $$p(y)=\int p(y|x)\ast p(x)dx$$
   离散的情况下：
   $$p(y)=\sum _{x}p(y|x)\ast p(x)$$

5. 基于条件概率公式和全概率公式，我们可以导出贝叶斯公式：
   $$p(x|y)=\frac{p(y,x)}{p(y)}=\frac{p(x)p(y|x)}{p(y)}=\frac{p(x)p(y|x)}{\sum _{x}p(y|x)p(x)}$$
   以及条件贝叶斯公式：
   $$p(x|y,z)=\frac{p(x,y,z)}{p(y,z)}=\frac{p(y|x,z)p(x|z)}{p(y|z)}$$

1.2 贝叶斯公式的运用

例子1：根据以往的记录，某种诊断产品是否合格的传感器有如下效果：对合格产品的测试呈阳性的概率为0.95；不合格产品的测试呈阴性的概率为0.95。对产品进行普查的结果为：仅有千分之五的产品合格，现有某产品被检测结果为阳性，问该产品的合格概率为多少？

解：设$y$={某产品检测结果为阳性}，$x$={某产品确实合格}；根据已知条件：$p(y|x)=0.95$，$p(\overline{y}|\overline{x})=0.95$，$p(x)=0.005$，$p(\overline{x})=0.995$，$p(y|\overline{x})=1-p(\overline{y}|\overline{x})=0.05$，由贝叶斯公式，有
$$p(x|y)=\frac{p(x,y)}{p(y)}=\frac{p(x)p(y|x)}{p(x)p(y|x)+p(\overline{x})p(y|\overline{x})}\approx 0.087$$

验算：假设在100000产品中，确有不合格产品99500件，测试得到阳性4975件；确有合格产品500件，测试得到阳性475件。因此，$p(x|y)=\frac{475}{475+4975}\approx 0.087$，说明若将该实验用于普查，则正确性只有8.7%。

启发：在贝叶斯公式中，$p(x)$是在没有进一步的信息（不知道$y$是否发生）的情况下人们对$x$发生可能性大小的认识，称为先验概率（priori probability）；现在有了新的信息（知道$y$的发生），人们对$x$发生的可能性有了新的估计，得到条件概率$p(x|y)$，称为后验概率（posterior probability）。在例子1中，对于产品不合格率的认识从先验概率的0.5%，通过传感器测试，得到了后验概率8.7%。

2 贝叶斯滤波

2.1 概率分布

上文在推导贝叶斯公式时，计算得到的结果都是概率，概率代表着某一件事情发生的可能性。而接下来我们要讨论的概念是概率分布。概率分布是指某一次试验的全部可能结果及各种可能结果发生的概率。

例如：假设某物体的角速度恒定为1rad/s，我们使用某传感器测量该物体的角速度，假设测量噪声服从正态分布，那么该次传感器的所有测量结果的概率分布就可以表现为如下图形式，如果测量噪声的方差为0.1，那么可以得出结论：传感器该次测量完全准确的概率为40%，有5%的概率测量误差为0.2。

2.2 贝叶斯滤波的推导

由于本人的水平问题，以及为方便推导方便理解和说明，贝叶斯滤波的推导过程都假设在机器人的系统中。

在机器人系统中：
设$x$代表机器人的状态数据，比如坐标、角度数据；
设$z$代表传感器的观测数据，比如速度、角速度；
设$u$代表机器人系统的控制量，比如角加速度、线加速度；
使用下标$A_{1:t}$表示在1到t时刻，A的所有状态；

在机器人系统运行的过程中，1到t时刻的观测$z_{1:t}$和输入$u_{1:t}$都是已知的，而如何依据这两个量计算出当前时刻机器人状态$x_t$的概率分布就是贝叶斯滤波要做的事，即求解：
$$P(x_t|z_{1:t},u_{1:t})$$
根据上文推导的条件贝叶斯公式，该式子可以扩展为：
$$P(x_t|z_{1:t},u_{1:t})=P(x_t|z_t,z_{1:t-1},u_{1:t})=\frac{P(z_t|x_t,z_{1:t-1},u_{1:t})P(x_t|z_{1:t-1},u_{1:t})}{P(z_t|z_{1:t-1},u_{1:t})}$$
其中：

1. 分母$P(z_t|z_{1:t-1},u_{1:t})$没有意义,因为现在的测量值和现在的位置有关系，与之前的测量值没什么关系，所以把它作为一个归一化参数$\eta$，即$P(z_t|z_{1:t-1},u_{1:t})=\eta$；
2. 根据隐式马尔可夫模型，在给定现在状态时，它与过去状态是条件独立的，当前时刻机器人的观测只与当前机器人的状态有关，因此$P(z_t|x_t,z_{1:t-1},u_{1:t})=P(z_t|x_t)$；
3. 根据全概率公式，可得
   $$P(x_t|z_{1:t-1},u_{1:t})=\int P(x_t|x_{t-1},z_{1:t-1},u_{1:t})P(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1}$$
   而该式子可以进一步化简：
   在式子$P(x_t|x_{t-1},z_{1:t-1},u_{1:t})$中，显然当前机器人的状态只与上一时刻的状态以及当前系统输入有关，即$P(x_t|x_{t-1},z_{1:t-1},u_{1:t})=P(x_t|x_{t-1},u_t)$；
   在式子$P(x_{t-1}|z_{1:t-1},u_{1:t})$中，上一时刻的状态与当前时刻的系统输入无关，即$P(x_{t-1}|z_{1:t-1},u_{1:t})=P(x_{t-1}|z_{1:t-1},u_{1:t-1})$

最终，贝叶斯滤波的公式的化简过程如下：
$$\begin{gathered} P(x_t|z_{1:t},u_{1:t})=\frac{P(z_t|x_t,z_{1:t-1},u_{1:t})P(x_t|z_{1:t-1},u_{1:t})}{P(z_t|z_{1:t-1},u_{1:t})} \\ =\eta P(z_t|x_t,z_{1:t-1},u_{1:t})P(x_t|z_{1:t-1},u_{1:t}) \\ =\eta P(z_t|x_t)\int P(x_t|x_{t-1},z_{1:t-1},u_{1:t})P(x_{t-1}|z_{1:t-1},u_{1:t})d{x}_{t-1} \\ =\eta P(z_t|x_t)\int P(x_t|x_{t-1},u_t)P(x_{t-1}|z_{1:t-1},u_{1:t-1})d{x}_{t-1} \end{gathered}$$

式中各个部分的含义：

1. $P(x_t|z_{1:t},u_{1:t})$指贝叶斯滤波的计算结果，通常用$bel(x_t)$，是t时刻机器人状态的后验概率分布；
2. $P(z_t|x_t)$指观测与机器人状态的关系，其实就是传感器的观测精度；
3. $P(x_t|x_{t-1},u_t)$指机器人的运动模型，通常是状态转移函数或者状态转移矩阵；
4. $P(x_{t-1}|z_{1:t-1},u_{1:t-1})$上一时刻贝叶斯滤波计算得到的后验概率分布，因此贝叶斯滤波的迭代形式可以表示为：
   $$bel(x_t)=\eta P(z_t|x_t)\int P(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}$$
5. 积分部分通常被成为预测概率分布（或者先验概率分布），该概率分布是指使用上一时刻的测量和当前时刻的系统输入预测出机器人的状态，预测概率分布可以表示为：
   $$\overline{bel}(x_t)=P(x_t|z_{1:t-1},u_{1:t})=\int P(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}$$
   因此，贝叶斯滤波也可以被表示为：
   $$bel(x_t)=\eta P(z_t|x_t)\overline{bel}(x_t)$$

2.2 贝叶斯滤波的过程

通过上面的推导，总结一下，贝叶斯滤波的过程如下图所示：

贝叶斯滤波的计算只需要上一时刻的计算结果、当前时刻的系统输入和当前时刻的测量三个量。计算过程只有两步：第一步，根据上一时刻贝叶斯滤波对状态的滤波结果以及系统的状态转移关系计算当前时刻系统状态的估计；第二步，根据系统的观测对估计结果进行调整，得到当前时刻状态的滤波结果。

2.3 为什么贝叶斯滤波无法实现

贝叶斯滤波仅仅只停留在公式上，并没有具体的实现，这是因为在实际生活中，很多概率分布都是无法使用数学公式描述的。比如上文提到的$P(z_t|x_t)$指传感器测量的概率分布，然而在实际生活中，这个分布函数也许是高度复杂的、非线性的、甚至无法用数学公式表示的。

针对这个问题，卡尔曼滤波和粒子滤波都做了一定的简化。在卡尔曼滤波中，算法假设系统是线性的，所有的概率分布都服从高斯分布，因此卡尔曼滤波只需要一个数学期望$\mu$和方差${\delta }^2$就可以表示一个概率分布；而粒子滤波则是使用很多粒子，通过它们的分布和权重来拟合系统的概率分布。

尽管描述概率分布的方法不同，但是卡尔曼滤波和粒子滤波在计算步骤上和贝叶斯滤波一致，所以称这些算法是基于贝叶斯滤波的算法。

3 卡尔曼滤波

3.1 卡尔曼滤波的基本原理

卡尔曼滤波的公式推导网上有很多，不再复述，可以参考：https://baijiahao.baidu.com/s?id=1604230489177143048&wfr=spider&for=pc

3.2 贝叶斯滤波和卡尔曼滤波

在贝叶斯滤波器中，当$P(z_t|x_t)$和$P(x_t|x_{t-1},u_t)$都使用高斯分布来表示时，这种特殊情况就叫卡尔曼滤波。

标准卡尔曼滤波的过程可以分为预测阶段和更新阶段，其中预测阶段的公式如下，式中：$x$为系统的状态；$u$为系统的输入；$P$为$x$的协方差矩阵（卡尔曼滤波假设系统状态服从高斯分布，因此$x$和$P$分布代表高斯分布的期望和方差）；$Q$为状态转移过程的噪声，服从高斯分布。
$\{\begin{array}{l}{x}_{k|k-1}=F{x}_{k-1|k-1}+B{u}_k\\ P_{k|k-1}=F_k{P}_{k-1|k-1}{F}_k^T+Q_k\end{array}$
第一个公式即为状态转移公式，第二个公式表示协方差矩阵经过状态转移矩阵后发生的变化；这两个公式根据上一时刻的系统状态$(x_{k-1|k-1},P_{k-1|k-1})$得到当前时刻系统状态的预测$(x_{k|k-1},P_{k|k-1})$。因此，这两个公式即实现了贝叶斯滤波中的$\overline{bel}(x_t)=\int P(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}$过程。

更新阶段的公式如下，式中：$z$为测量；$H$为测量矩阵；$R$为测量噪声；$K$为卡尔曼增益。
$\{\begin{array}{l}{K}_k=P_{k|k-1}{H}_k^T{(H_k{P}_{k|k-1}{H}_k^T+R_k)}^{-1}\\ x_{k|k}=x_{k|k-1}+K_k(z_k-H{x}_{k|k-1})\\ P_{k|k}=(I-K_k{H}_k)P_{k|k-1}\end{array}$
第一个公式用来计算卡尔曼增益$K$，在第二个公式中，$z_k-H{x}_{k|k-1}$是观测与预测之间的差距，这个差距乘上补偿系数$K$后补偿到预测$x_{k|k-1}$上，最终得到卡尔曼滤波的计算结果。这三个公式通过观测补偿预测结果$(x_{k|k-1},P_{k|k-1})$得到$(x_{k|k},P_{k|k})$。因此，这三个公式即实现了贝叶斯滤波中的$bel(x_t)=\eta P(z_t|x_t)\overline{bel}(x_t)$过程。

3.3 卡尔曼滤波在机器人定位系统上的应用

在使用卡尔曼滤波前需要对系统进行分析建模，得到系统的状态传递模型和观测模型，即：
$$\{\begin{array}{l}{x}_k=f(x_{k-1},u_k)+w_k\\ z_k=h(x_k)+v_k\end{array}$$
假设有一个机器人，它能测量自身的加速度$A$、线速度$V$、角速度$V\theta$、姿态角$\theta$，要求卡尔曼滤波输出更加准确的位移$XY$和姿态角$\theta$数据，只考虑它在2D平面上的运动，则系统可以建模成：
$$\{\begin{array}{l}{x}_k=\left[\begin{array}{c}{X}_k\\ Y_k\\ {\theta }_k\\ V{x}_k\\ V{y}_k\\ V{\theta }_k\\ A{x}_k\\ A{y}_k\end{array}\right]=\left[\begin{array}{cccccccc}1& 0& 0& \cos ({\theta }_{k-1})△t& -\sin ({\theta }_{k-1})△t& 0& 0.5\cos ({\theta }_{k-1})△t^2& -0.5\sin ({\theta }_{k-1})△t^2\\ & 1& 0& \sin ({\theta }_{k-1})△t& \cos ({\theta }_{k-1})△t& 0& 0.5\sin ({\theta }_{k-1})△t^2& 0.5\cos ({\theta }_{k-1})△t^2\\ & & 1& 0& 0& △t& 0& 0\\ & & & 1& 0& 0& △t& 0\\ & & & & 1& 0& 0& △t\\ & & & & & 1& 0& 0\\ & & & & & & 1& 0\\ & & & & & & & 1\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ Y_{k-1}\\ {\theta }_{k-1}\\ V{x}_{k-1}\\ V{y}_{k-1}\\ V{\theta }_{k-1}\\ A{x}_{k-1}\\ A{y}_{k-1}\end{array}\right]+w_k\\ z_k=\left[\begin{array}{cccccccc}0& & & & & & & \\ & 0& & & & & & \\ & & 1& & & & & \\ & & & 1& & & & \\ & & & & 1& & & \\ & & & & & 1& & \\ & & & & & & 1& \\ & & & & & & & 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ \theta \\ Vx\\ Vy\\ V\theta \\ Ax\\ Ay\end{array}\right]+v_k\end{array}$$
但是绝大部分时候，廉价的加速度传感器对加速度值测量具有较大的噪声，因此并不会在系统中使用，所以，上面的系统可以简化为：
$$\{\begin{array}{l}{x}_k=\left[\begin{array}{c}{X}_k\\ Y_k\\ {\theta }_k\\ V{x}_k\\ V{y}_k\\ V{\theta }_k\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& \cos ({\theta }_{k-1})△t& -\sin ({\theta }_{k-1})△t& 0\\ 0& 1& 0& \sin ({\theta }_{k-1})△t& \cos ({\theta }_{k-1})△t& 0\\ 0& 0& 1& 0& 0& △t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_{k-1}\\ Y_{k-1}\\ {\theta }_{k-1}\\ V{x}_{k-1}\\ V{y}_{k-1}\\ V{\theta }_{k-1}\end{array}\right]+w_k\\ z_k=\left[\begin{array}{cccccc}0& & & & & \\ & 0& & & & \\ & & 1& & & \\ & & & 1& & \\ & & & & 1& \\ & & & & & 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ \theta \\ Vx\\ Vy\\ V\theta \end{array}\right]+v_k\end{array}$$
无论系统如何简化，可以看到系统状态转移矩阵中都出现了$\sin (\theta )$和$\sin (\theta )$函数，说明该矩阵是非线性的，因此普通的卡尔曼滤波不再适用，需要用到扩展卡尔曼滤波（Extended Kalman Filter），扩展卡尔曼滤波对非线性系统做出改进，但是主要的公式和原理基本不变，这里不再赘述。
传感器每次测量得到数据后，通过扩展卡尔曼滤波的预测公式和更新公式不断迭代，逐渐准确地估计动态系统的状态。

4 粒子滤波

4.1 粒子滤波的基本原理

卡尔曼滤波的公式推导网上有很多，不再复述，可以参考：https://heyijia.blog.csdn.net/article/details/41122125

4.2 粒子滤波和贝叶斯滤波

粒子滤波采用蒙特卡洛采样思想，使用粒子集来描述概率分布$P(z_t|x_t)$和$P(x_t|x_{t-1},u_t)$，以此实现贝叶斯滤波。

在粒子滤波中，粒子集可以表示为：
X = { ( x t i , w t i ) ∣ i = 1 , ⋯ n } X=\{(x_t^i,w_t^i)\vert i=1,\cdots n\} X={(xti​,wti​)∣i=1,⋯n}
式中：$i$表示粒子的索引；$t$表示时刻；$x_t^i$表示某粒子代表的一个状态的假设；$w_t^i$表示该假设的权重；

粒子滤波的过程分为三步，分别为：

1. 粒子的状态传播：
   $$x_t^i\sim p(x_t|u_t,x_{t-1}^i)$$
   该公式将上一时刻的粒子集$x_{t-1}^i$通过状态转移公式计算得到预测的粒子集$x_t^i$。因此，这两个公式即实现了贝叶斯滤波中的$\overline{bel}(x_t)=\int P(x_t|x_{t-1},u_t)bel(x_{t-1})d{x}_{t-1}$过程。
2. 评估每一个粒子的权重：
   $$w_t^i=\eta p(z_t|x_t)$$
   通过观测来评估预测粒子集中各个粒子的权重，实现了计算贝叶斯滤波中的$\eta P(z_t|x_t)$过程。
3. 根据权重进行重采样：
   将粒子根据各自的权重进行重新分布，使得粒子的分布符合后验概率分布，实现了贝叶斯滤波中的$bel(x_t)=\eta P(z_t|x_t)\overline{bel}(x_t)$过程。

5 程序实现

最后，贴上扩展卡尔曼滤波、无迹卡尔曼滤波和粒子滤波的MATLAB程序实现。

%% 改写自https://heyijia.blog.csdn.net/article/details/41142679
%% 一维扩展卡尔曼滤波、无迹卡尔曼滤波、粒子滤波效果比对 
clear all    
close all    
clc

f=@(x,t)(0.5*x + 25*x/(1 + x^2) + 8*cos(1.2*(t-1))); % 状态传递函数
h=@(x)(x^2/20); % 测量函数

% 共同参数  
x = 0.1; % initial actual state
x_N = 1; % 系统过程噪声的协方差  (由于是一维的，这里就是方差)    
x_R = 1; % 测量的协方差    
T = 80;  % 共进行80次

% PF参数
N = 100; % 粒子数，越大效果越好，计算量也越大    

% EKF参数
p_ekf_E = 1;

% UKF参数
ki = 0;
alpha = 1;
beta = 2;
p_ukf_E = 1;

% 公共初始化
z_out = [x^2 / 20 + sqrt(x_R) * randn];  %实际测量值    
x_out = [x];  %the actual output vector for measurement values.  
  
% PF初始化
V = 2; %初始分布的方差    
x_P = []; % 粒子    
% 用一个高斯分布随机的产生初始的粒子    
for i = 1:N    
    x_P(i) = x + sqrt(V) * randn;
end    
x_est = [x]; % time by time output of the particle filters estimate    
x_est_out = [x_est]; % the vector of particle filter estimates.

% EKF初始化
x_ekf = x;  % time by time output of the kalman filters estimate 
x_ekf_out = [x_ekf]; % the vector of kalman filter estimates.

% UKF初始化
L = 1; % x矩阵维度
lambda = alpha^2 * (L + ki) - L;
Wm = zeros(1, 2*L+1);
Wc = zeros(1, 2*L+1);
for j=1:2*L+1 % 计算比重
    Wm(j)=1/(2*(L+lambda));
    Wc(j)=1/(2*(L+lambda));
end
Wm(1)=lambda/(L+lambda);
Wc(1)=lambda/(L+lambda)+1-alpha^2+beta;
x_ukf = x;
x_ukf_out = [x_ukf];


%% 算法开始    
for t = 1:T    
    x = f(x,t) +  sqrt(x_N) * randn;    % 系统状态转换
    z = h(x) + sqrt(x_R)*randn;    % 系统状态测量
    
    %% SIR粒子滤波的应用，算法流程参见博客http://blog.csdn.net/heyijia0327/article/details/40899819 
    %%%%%%%%%%%%%%%%%% particle filter %%%%%%%%%%%%%%%%%%
    
    for i = 1:N    
        %从先验p(x(k)|x(k-1))中采样     % 把所有粒子都通过状态转换函数进行更新
        x_P_update(i) = f(x_P(i),t) + sqrt(x_N)*randn;    
        %计算采样粒子的值，为后面根据似然去计算权重做铺垫    
        z_update(i) = h(x_P_update(i));      % 将所有更新后的粒子做测量
        %对每个粒子计算其权重，这里假设量测噪声是高斯分布。所以 w = p(y|x)对应下面的计算公式    
        P_w(i) = (1/sqrt(2*pi*x_R)) * exp(-(z - z_update(i))^2/(2*x_R));    % 通过正态分布函数计算粒子权重，标志差为x_R，平均值为z_update，当z_update=z的时候，权重最大
    end    
    % 归一化.    
    P_w = P_w./sum(P_w);    
    
    for i = 1 : N
        x_P(i) = x_P_update(find(rand <= cumsum(P_w),1));   % 粒子权重大的将多得到后代    
    end                                                     % find( ,1) 返回第一个 符合前面条件的数的 下标    
        
    %状态估计，重采样以后，每个粒子的权重都变成了1/N    
    x_est = mean(x_P);    
    
    %% 扩展卡尔曼滤波实现
    %%%%%%%%%%%%%%%%%% extend kalman filter %%%%%%%%%%%%%%%%%%
    x_ekf_es =  f(x_ekf, t);  % 计算状态转移
    z_ekf_es = h(x_ekf_es); % 计算状态转移后的观测值
    f_ekf = 25/(x_ekf^2 + 1) - (50*x_ekf^2)/(x_ekf^2 + 1)^2 + 1/2; % 计算状态转移矩阵
    p_ekf_es = f_ekf * p_ekf_E * f_ekf + x_N; % 计算先验协方差矩阵
    h_ekf = x_ekf_es /10; % 计算测量矩阵
    k_ekf = p_ekf_es * h_ekf / (h_ekf * p_ekf_es * h_ekf + x_R); % 计算卡尔曼增益
    x_ekf = x_ekf_es + k_ekf * (z - z_ekf_es); % 计算状态预测值
    p_ekf_E = (1 - k_ekf * h_ekf) * p_ekf_es; % 计算后验协方差矩阵
    
    %% 无迹卡尔曼滤波实现，参考自https://blog.csdn.net/caokaifa/article/details/83041371
    %%%%%%%%%%%%%%%%%% unscented kalman filter %%%%%%%%%%%%%%%%%%

    % 对状态转移进行UT变环
    cho = (chol(p_ukf_E * (L + lambda)))';
    sigmas = [x_ukf,x_ukf+cho,x_ukf-cho]; % 计算sigma点
    sigmas_x = zeros(1, 2*L+1); % sigma点经过状态转移后的结果
    x_ukf_es = 0; % 计算状态转移后的期望
    for i = 1 : 2*L+1
        sigmas_x(i) = f(sigmas(i), t);
        x_ukf_es = x_ukf_es + sigmas_x(i) * Wm(i);
    end
    p_ukf_es = x_N; % 计算状态转移后的方差(同时保留状态转移误差)
    for i = 1 : 2*L+1
        p_ukf_es = p_ukf_es + Wc(i) * (sigmas_x(i) - x_ukf_es)*(sigmas_x(i) - x_ukf_es)';
    end
    
    % 对测量进行UT变换
    cho = (chol(p_ukf_es * (L + lambda)))';
    sigmas = [x_ukf_es,x_ukf_es+cho,x_ukf_es-cho]; % 计算sigma点
    sigmas_z = zeros(1, 2*L+1); % sigma点经过状态转移后的结果
    z_ukf_es = 0; % 计算状态转移后的期望
    for i = 1 : 2*L+1
        sigmas_z(i) = h(sigmas(i)); % sigmas_z(i) = h(sigmas_x(i));
        z_ukf_es = z_ukf_es + sigmas_z(i) * Wm(i);
    end
    p_ukf_ZZ = x_R; % 计算测量后的方差(同时保留测量误差)
    p_ukf_XZ = 0;
    for i = 1 : 2*L+1
        p_ukf_ZZ = p_ukf_ZZ + Wc(i) * (sigmas_z(i) - z_ukf_es)*(sigmas_z(i) - z_ukf_es)';
        p_ukf_XZ = p_ukf_XZ + Wc(i) * (sigmas_x(i) - x_ukf_es)*(sigmas_z(i) - z_ukf_es)';
    end
    
    k_ukf = p_ukf_XZ * inv(p_ukf_ZZ); % 计算卡尔曼增益
    x_ukf = x_ukf_es + k_ukf * (z - z_ukf_es); % 计算后延概率的期望
    p_ukf_E = p_ukf_es - k_ukf * p_ukf_ZZ * k_ukf'; % 计算后延概率的方差
    
    
    % Save data in arrays for later plotting    
    x_out = [x_out x];    % 保存 通过状态转换函数得到的x
    z_out = [z_out z];    % 保存 通过状态测量函数得到的z
    x_est_out = [x_est_out x_est];    % 保存粒子滤波结果
    x_ekf_out = [x_ekf_out x_ekf];    % 保存扩展卡尔曼滤波结果
    x_ukf_out = [x_ukf_out x_ukf];    % 保存无迹卡尔曼滤波结果
        
end
    
std_pf = std(x_est_out - x_out)
std_ekf = std(x_ekf_out - x_out)
std_ukf = std(x_ukf_out - x_out)

t = 0:T;    
figure(1);    
clf    
plot(t, x_out, 'b', t, x_est_out, 'r', t, x_ekf_out, 'y', t, x_ukf_out, 'k', 'linewidth',1);    
set(gca,'FontSize',12); set(gcf,'Color','White');    
xlabel('time step'); ylabel('flight position');    
legend('True flight position', 'Particle Filter', 'Extended Kalman Filter', 'Unscented Kalman Filter');

实现效果：

参考

1. https://zhuanlan.zhihu.com/p/31870182
2. https://www.cnblogs.com/ycwang16/p/5995702.html
3. https://heyijia.blog.csdn.net/article/details/41122125
4. https://baijiahao.baidu.com/s?id=1604230489177143048&wfr=spider&for=pc
5. 深蓝学院SLAM教程

文中有任何错谬之处，还望指正。
-2020-11-21