KF&EKF&UKF&PF

1. 基础知识概要
- 协方差矩阵
- 多维高斯分布
- 状态空间表达式
2. 什么是卡尔曼滤波器
3. 五个重要的公式
- 公式介绍
- 公式推导过程
4. 卡尔曼滤波的变种
- KF（Kalman Filter）
- EKF（Extended Kalman Filter）
- UKF（Unscented Kalman Filter）
- PF（Particle Filter）
5. 卡尔曼滤波器参数

1. 基础知识概要

协方差矩阵

协方差矩阵可视为方差和协方差组合而成，方差构成了对角线上的元素，协方差构成了非对角线上的元素。
例如，有d个随机变量x，记为: x1,x2,...,xdx_1,x_2,...,x_dx1,x2,...,xd，则协方差矩阵为：
P=[σ(x1,x1)σ(x1,x2)...σ(x1,xd)σ(x2,x1)σ(x2,x2)...σ(x2,xd)............σ(xd,x1)σ(xd,x2)...σ(xd,xd)]P = \left[ \begin{matrix} \sigma(x_1, x_1) & \sigma(x_1, x_2) & ... & \sigma(x_1, x_d) \\ \sigma(x_2, x_1) & \sigma(x_2, x_2) & ... & \sigma(x_2, x_d) \\ ... & ... & ...& ... \\ \sigma(x_d,x_1) & \sigma(x_d,x_2) & ... & \sigma(x_d,x_d) \end{matrix} \right] P=⎣⎢⎢⎡σ(x1,x1)σ(x2,x1)...σ(xd,x1)σ(x1,x2)σ(x2,x2)...σ(xd,x2)............σ(x1,xd)σ(x2,xd)...σ(xd,xd)⎦⎥⎥⎤
其中PPP为协方差矩阵，为对称矩阵。

多维高斯分布

一个随机变量XXX服从均值为μμμ，协方差矩阵为PPP的多维高斯分布，则其概率密度函数为：
P(X∣μ,P)=1(2π)Ndet(P)e−12(x−μ)TP−1(x−μ)P(X|μ,P)=\frac{1}{\sqrt{(2\pi)^Ndet(P)}}e^{-\frac{1}{2}(x-μ)^TP^{-1}(x-μ)} P(X∣μ,P)=(2π)Ndet(P)1e−21(x−μ)TP−1(x−μ)

状态空间表达式

离散状态下的状态方程与观测方程：
xk=Axk−1+Buk−1+wk−1zk=Hxk+vkx_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Hx_k+v_k xk=Axk−1+Buk−1+wk−1zk=Hxk+vk
其中：

xk−1x_{k-1}xk−1为上一时刻(k-1)状态，
AAA为状态转移矩阵，
BBB将输入转换为状态的矩阵，是输入矩阵，
wk−1w_{k-1}wk−1为过程噪声，服从均值为0，方差为Q的高斯分布，
xkx_kxk为k时刻状态，
HHH为观测矩阵，
zkz_kzk是测量值，
vkv_kvk为传感器的测量噪声，服从矩阵为0，方差为R的高斯分布。

2. 什么是卡尔曼滤波器

现实世界中存在很多的不确定性，当我们去描述一个系统的时候，不确定性主要体现在三个方面：

不存在完美的数学模型
系统扰动是不可控的，往往很难建模
测量的传感器本身存在误差

卡尔曼滤波是一种优化估计算法，可以在任何含有不确定信息的动态系统中使用卡尔曼滤波，对系统下一步的走向做出有根据的预测，即使伴随着各种干扰，卡尔曼滤波总是能指出真实发生的情况。

下面举一个简单例子来具体说明一下卡尔曼滤波器解决什么问题。

假设我们要研究一个房间的温度，根据经验判断，我们认为这个房间的温度是一直不变的，即当前时刻的温度等于上一时刻的温度。假如你对你的经验不是百分百相信，温度可能上下偏差几度，我们把这些偏差看做高斯白噪声。此时，房间里还有一个温度计，得到一个测量的温度，这个温度计的测量值也是不准确的，存在一个测量噪声，也是高斯白噪声。

现在，我们得到了某一时刻房间里的两种温度，人为估计的温度（系统的预测值）和温度计测量的温度（传感器的测量值）。那么房间的温度到底是多少呢？这时候就需要用到卡尔曼滤波器了，通过系统预测值和传感器观测值，我们对这两个数据进行融合，会得到一个最优的估计值。

假设人为估计的温度为23度，偏差的标准差为5度，温度计的测量温度为25度，偏差为4度。由于我们用于估算k时刻的实际温度有两个温度值，分别是23度和25度。究竟实际温度是多少呢？相信自己还是相信温度计呢？究竟相信谁多一点，我们可以用他们的covariance来判断。因为Kg=5252+42K_g=\frac{5^2}{5^2+4^2}Kg=52+4252，所以Kg=0.78，我们可以估算出k时刻的实际温度值是：23+0.78*(25-23)=24.56度。可以看出，因为温度计的covariance比较小（比较相信温度计），所以估算出的最优温度值偏向温度计的值。

3. 五个重要的公式

公式介绍

离散状态下的状态方程与观测方程：
xk=Axk−1+Buk−1+wk−1zk=Hxk+vkx_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k=Hx_k+v_kxk=Axk−1+Buk−1+wk−1zk=Hxk+vk

其中：

AAA：状态转移矩阵，对目标状态转换的一种猜想模型；
BBB：将输入转换为状态的矩阵；
HHH：是状态变量到测量（观测）的转换矩阵，表示将状态和观测连接起来的关系；
zkz_kzk：测量值（观测值），是滤波的输入。

卡尔曼滤波分为两步：预测和更新。

状态预测方程根据前一时刻的状态估计值推算当前时刻的状态变量的先验估计值和误差协方差的先验估计值。
状态更新方程负责将先验估计和新的测量变量结合起来，构造改进的后验估计。

状态预测方程：
x^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q\hat{x}_k^-=A\hat{x}_{k-1}+B{u_{k-1}}\\ {P_k^-}=A{P_{k-1}}A^T+Qx^k−=Ax^k−1+Buk−1Pk−=APk−1AT+Q

状态更新方程：
Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−K_k={P_k^-}H^T(H{P_k^-}H^T+R)^{-1}\\ \hat{x}_k=\hat{x}_k^-+K_k(z_k-H\hat{x}_k^-)\\ P_k=(I-K_kH)P_k^-Kk=Pk−HT(HPk−HT+R)−1x^k=x^k−+Kk(zk−Hx^k−)Pk=(I−KkH)Pk−

其中:

x^k−1\hat{x}_{k-1}x^k−1和x^k\hat{x}_kx^k：k-1时刻和k时刻的后验状态估计值，更新后的结果，也叫最优估计。
x^k−\hat{x}_k^-x^k−：k时刻的先验状态估计值，是滤波的中间计算结果，即根据上一时刻（k-1时刻）的最优估计预测的k时刻的结果，是预测方程的结果。
Pk−1{P_{k-1}}Pk−1和PkP_kPk: k-1时刻和k时刻的后验估计协方差（即x^k−1\hat{x}_{k-1}x^k−1和xkx_kxk的误差的协方差，表示状态的不确定度），是滤波的结果之一。
Pk−P_k^-Pk−：k时刻的先验误差估计协方差（x^k−\hat{x}_k^-x^k−的协方差），是滤波的中间计算结果。
KkK_kKk：滤波的增益矩阵，是滤波的中间计算结果，卡尔曼增益。
QQQ：过程激励噪声的协方差（系统过程协方差）。该参数被用来表示状态转换矩阵与实际过程之间的误差。
RRR：测量噪声协方差。
(zk−Hx^k−)(z_k-H\hat{x}_k^-)(zk−Hx^k−)：实际观测和预测观测的残差，和卡尔曼增益一起修正先验（预测）得到的后验。

公式推导过程

首先看系统的状态方程和观测方程，其中分别包含wk−1w_{k-1}wk−1和vkv_kvk两个不确定的量，在现实建模的过程中，这两个量是无法确定的，我们只能用到除了它们以外的部分来求一个状态估计值，即得到第一个式子：
x^k−=Ax^k−1+Buk−1\hat{x}_k^-=A\hat{x}_{k-1}+B{u_{k-1}}x^k−=Ax^k−1+Buk−1

对于观测方程，测量结果是已知的，可以计算出一个x的测量值x^kmea\hat{x}_{kmea}x^kmea:
zk=HXk⇒x^kmea=H−1zkz_k=HX_k \Rightarrow \hat{x}_{kmea}=H^{-1}z_kzk=HXk⇒x^kmea=H−1zk

这样我们得到了两个结果，一个是先验结果，计算出来的，另一个是测量结果，但是这两个结果都不具备噪声的影响，这两个结果都是不准确的，这时就用卡尔曼滤波对这两个数据进行融合，即得到下式：
x^k=x^k−+G(H−1zk−x^k−)\hat{x}_k=\hat{x}_k^-+G(H^{-1}z_k-\hat{x}_k^-)x^k=x^k−+G(H−1zk−x^k−)

后验估计值等于状态的先验估计值（算出来的结果）加上一个权重系数G乘上测量结果与先验估计值的差值。可以看出当G=0时，x^k=x^k−\hat{x}_k=\hat{x}_k^-x^k=x^k−，当G=1时，x^k=H−1zk\hat{x}_k=H^{-1}z_kx^k=H−1zk。

这个结果和一般教科书上的结果不同，我们做个变换，令G=KkHG=K_kHG=KkH,即得到第二个式子：
x^k=x^k−+Kk(zk−Hx^k−)\hat{x}_k=\hat{x}_k^-+K_k(z_k-H\hat{x}_k^-)x^k=x^k−+Kk(zk−Hx^k−)

此时KkK_kKk的范围为0到H−1H^{-1}H−1。

接下来的目标就是去寻找一个卡尔曼增益KkK_kKk，使得后验估计值无限接近于系统的实际状态值，即x^k→xk(实际值)\hat{x}_k \rightarrow x_k(实际值)x^k→xk(实际值)。

此时我们定义ek=xk−x^ke_k=x_k-\hat{x}_kek=xk−x^k,服从均值为0的高斯分布。当我们估计的x^k\hat{x}_kx^k越接近实际的值，即估计值与实际值的误差越小，即误差的方差越小，即误差的值越接近期望0，所以我们希望选择一个KkK_kKk，使得协方差矩阵P的迹（主对角线上元素之和）最小，则方差最小。

先计算协方差矩阵：
Pk=E[eeT]Pk=E[(xk−x^k)((xk−x^k)T]P_k = E[ee^T]\\ P_k = E[(x_k-\hat{x}_k)((x_k-\hat{x}_k)^T]Pk=E[eeT]Pk=E[(xk−x^k)((xk−x^k)T]

将x^k\hat{x}_kx^k的公式（将公式中的zkz_kzk用zk=Hxk+vkz_k=Hx_k+v_kzk=Hxk+vk替换）带入上式中，就得到了关于KkK_kKk的等式，整理得到：
Pk=E[[(I−KkH)(xk−x^k−)−Kkvk][(I−KkH)(xk−x^k−)−Kkvk]T]P_k = E[[(I-K_kH)(x_k-\hat{x}_k^-)-K_kv_k][(I-K_kH)(x_k-\hat{x}_k^-)-K_kv_k]^T]Pk=E[[(I−KkH)(xk−x^k−)−Kkvk][(I−KkH)(xk−x^k−)−Kkvk]T]

我们记ek−=xk−x^k−e_k^-=x_k-\hat{x}_k^-ek−=xk−x^k−,定义为误差的先验。将上式展开，去掉转置，去掉整体期望变为各自期望的和，又因ek−e_k^-ek−和vkT{v_k}^TvkT相互独立，整理得到：
Pk=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(vkvkT)KkTP_k = (I-K_kH)E(e_k^-{e_k^-}^T)(I-K_kH)^T+K_kE(v_k{v_k}^T){K_k}^TPk=(I−KkH)E(ek−ek−T)(I−KkH)T+KkE(vkvkT)KkT

可以看出上式中的E(ek−ek−T)E(e_k^-{e_k^-}^T)E(ek−ek−T)等于Pk−P_k^-Pk−,E(vkvkT)E(v_k{v_k}^T)E(vkvkT)等于R，接着展开得到：
Pk=Pk−−Pk−HTKkT−KkHPk−+KkHPk−HTKkT+KkRKkTP_k = P_k^--P_k^-H^T{K_k}^T-K_kHP_k^-+K_kHP_k^-H^T{K_k}^T+K_kR{K_k}^TPk=Pk−−Pk−HTKkT−KkHPk−+KkHPk−HTKkT+KkRKkT

接下来求协方差矩阵的迹：即tr(Pk)tr(P_k)tr(Pk)，可以看出PkP_kPk第二项和第三项互为转置，故它们的迹相等。我们要求KkK_kKk使得迹最小，则对KkK_kKk求导，一阶导数等于零，求解则可以得到KkK_kKk。在求导的过程中用到了以下两个恒等式：
dtr(AB)dA=BTdtr(ABAT)dA=2AB\frac{dtr(AB)}{dA}=B^T\\ \ \\ \frac{d tr(ABA^T)}{dA}=2ABdAdtr(AB)=BT dAdtr(ABAT)=2AB

最终我们得到了卡尔曼增益的表达式：
Pk−=APk−1AT+QP_k^-=AP_{k-1}A^T+QPk−=APk−1AT+Q

Pk−P_k^-Pk−的更新需要用到Pk−1P_{k-1}Pk−1,故需要更新协方差矩阵的后验估计，通过之前推导的公式，将KkK_kKk带入，整理可得：
Pk=(I−KkH)Pk−P_k=(I-K_kH)P_k^-Pk=(I−KkH)Pk−

卡尔曼滤波的五个公式到这里就推导完成了，这是从增益最优化的角度来推导的，还可以通过MAP和贝叶斯推断来推导卡尔曼滤波，其他的推导过程参考《机器人学中的状态估计》一书。

4. 卡尔曼滤波的变种

现实是我们的处理和测量模型都是非线性的，结果就是一个不规则分布，KF能够使用的前提就是所处理的状态是满足高斯分布的，为了解决这个问题，EKF是寻找一个线性函数来近似这个非线性函数，而UKF就是去找一个与真实分布近似的高斯分布。

EKF存在两个问题：

通过泰勒展开线性化后忽略了高阶项,对于强非线性系统其误差会比较大;
计算 Jacobian 矩阵比较复杂,甚至有时候无法得到 Jacobian 矩阵。

UKF则可以对应的改善以上两个问题:

UKF 相对 EKF 精度更高一些,相当于二阶 Taylor 展开,但速度会略慢一点;
不需要计算 Jacobian 矩阵。

KF 和 PF 的思想类似,但是 UKF 中的粒子是明确的, PF 中的粒子是随机的,所以 UKF 相对于 PF 来讲，计算量小很多。

State Estimator	Model	Assumed Distribution	Assumed Distribution	Application
Kalman Filter(KF)	Linear	Gaussian	Low
Extended Kalman Filter(EKF)	Locally Linear	Gaussian	Low(if the Jacobians need to be computed analytically) Medium(if the Jacobians can be computed numerically)	Tracking
Unscented Kalman Filter(UKF)	Nolinear	Gaussian	Medium	Tracking
Particle Filter(PF)	Nolinear	Non-Gaussian	High	Localization

注: EKF 是靠非线性函数线性化,泰勒级数取前几项近似; UKF 是靠对非线性函数的概率密度分布进行近似,用一系列确定样本来逼近状态的后验概率密度;粒子滤波是靠频率代替概率;神经网络直接不管这些,三层结构用数据训练模型等等。

KF（Kalman Filter）

卡尔曼滤波是一种高效率的递归滤波器, 它能够从一系列的不完全包含噪声的测量中，估计动态系统的状态，然而简单的卡尔曼滤波必须应用在符合高斯分布的系统中。

KF处理线性模型：
xk=Axk−1+Buk−1+wk−1zk=Hxk+vkx_k=Ax_{k-1}+Bu_{k-1}+w_{k-1}\\ z_k =Hx_k+v_kxk=Axk−1+Buk−1+wk−1zk=Hxk+vk

KF的假设：

高斯分布的随机变量预测后的不确定性仍服从高斯分布；
高斯分布的随机变量变换到测量空间后不确定性仍服从高斯分布；
A和H矩阵，线性变换应用在线性系统。

KF 的优点:

卡尔曼滤波非常适合不断变化的系统；
它的优点还有内存占用较小(只需保留前一个状态),速度快,是实时问题和嵌入式系统的理想选择。

应用 KF 所面临的问题是非线性处理模型和非线性测量模型的处理。

卡尔曼滤波的本质是参数化的贝叶斯模型，通过对下一时刻系统的初步状态估计（即状态的先验估计）以及测量得出的反馈相结合，最终得到该时刻较为准确的的状态估计（即状态的后验估计），其核心思想即为预测+测量反馈，而这两者是通过一个变化的权值相联系使得最后的状态后验估计无限逼近系统准确的状态真值，这个权值即为大名鼎鼎的卡尔曼增益。

卡尔曼增益 K 首先权衡预测状态协方差和观测值矩阵 R 的大小 , 并以此来决定更相信谁 ; 如果信预测 ,则 K 小一点 , 如果信观测 , 则 K 大一点。

卡尔曼增益 K 还负责把残差的表现形式从观测域转换到了状态域。

在小⻋运动的例子中 , 我们只观察了汽⻋的位置 , 但 K 里面已经包含了协方差矩阵 P 的信息 (P 里面就给出了位置和速度的相关性 ), 所以它利用速度和位置这两个维度的相关性 , 从位置的残差中推算出了速度的残差 . 从而让我们可以对状态值 X 的两个维度同时进行修正。

EKF（Extended Kalman Filter）

EKF，Extended Kalman Filter，扩展卡尔曼滤波。在现实世界中，许多系统都不是线性的，状态和噪声也不是高斯分布的。一个高斯分布经过非线性变换后，不再是高斯分布。EKF则是用高斯分布去近似非线性变换后的分布，并且，在工作点附近对系统进行线性化。但是我们在对非线性系统进行线性化的过程中,只有被线性化的那个点附近的线性化模型和真实的模型相近,远的误差就大了,那么这个时候卡尔曼滤波器的效果就不好。决定一个线性化滤波器成功与否的关键就在于这个滤波器系统模型线性化得好不好。

非线性系统的状态方程一般可表示为：
xk=f(xk−1)+Wk−1zk=h(xk)+Vkx_k = f(x_{k-1})+W_{k-1}\\ z_k=h(x_k)+V_kxk=f(xk−1)+Wk−1zk=h(xk)+Vk

EKF与KF的区别在于：

需要使用非线性函数fff、hhh来表示状态方程和输出方程；
系统矩阵AAA、输出矩阵HHH需要用fff、hhh函数求偏导后的雅克比矩阵表示。

EKF的缺点：

高斯分布经过一个非线性变换后不是高斯分布，EKF 只计算均值与协方差，是在用高斯近似这个非线性变换后的结果 ( 实际中这个近似可能很差 )；
系统本身线性化过程中，丢掉了高阶项；
线性化的工作点往往不是输入状态真实的均值，而是一个估计的均值，因此，在这个工作点下计算的 AAA、HHH，也不是最好的；
在估计非线性输出的均值时，EKF 算的是非线性函数的形式，这个结果几乎不会是输出分布的真正期望值，协方差也是同理。

如何克服EKF的缺点：

为了克服第 3 条工作点的问题，可以以 EKF 估计的结果为工作点，重新计算一遍 EKF，直到这个工作点变化够小。此为迭代EKF（Iterated EKF，IEKF）；
为了克服第 4 条，我们除了计算函数的结果，再计算其他几个精心挑选的采样点，然后用这几个点估计输出的高斯分布。此为 SPKF(Sigma Point KF) 或 UKF(Unscented Kalman Filter)；或者丢掉高斯分布，用足够多的采样点，来表达输出的分布。这种蒙特卡洛的粗暴方式，即为粒子滤波 PF(Particle Filter) 的思路；
丢掉滤波器思路，可以把所有状态看成变量，把运动方程和观测方程看成变量间的约束，构造误差函数，然后最小化这个误差的二次型,此为非线性优化的方法。不过，非线性优化也需要对误差函数不断地求梯度，并根据梯度方向迭代，因而局部线性化是不可避免的。

UKF（Unscented Kalman Filter）

UKF，Unscented Kalman Filter，无迹卡尔曼滤波主要解决一个高斯分布经过非线性变换后，怎么用另一个高斯分布近似它。

UKF使用的是统计线性化技术，我们把这种线性化的方法叫做无损变换（unscented transformation）。假设状态是nnn，UT变换是通过2n+12n+12n+1个在先验分布中采集的点（sigma points）的线性回归来线性化随机变量的非线性函数。

基本思路：近似非线性函数的概率分布要比近似非线性函数本身更容易。通过寻找一个与真实分布有着相同的均值和协方差的高斯分布，来实现无损变换。

UKF Predict
无损滤波预测步骤主要分为三步：
1.需要知道选择sigma点的好办法，
2.需要知道如何预测sigma点，
3.需要知道如何根据预测的sigma点计算预测、均值和协方差。

选择预测sigma点：
sigma点集的均值的计算公式为：
x[1]=μx[i]=μ+((n+λ)P)ii=2,3,...,n+1x[i]=μ−((n+λ)P)i−ni=n+2,...,2n+1x^{[1]}=\mu\\ \ \\ x^{[i]}=\mu +(\sqrt{(n+\lambda)P})_i\qquad i=2,3,...,n+1\\ \ \\ x^{[i]}=\mu -(\sqrt{(n+\lambda)P})_{i-n}\qquad i=n+2,...,2n+1x[1]=μ x[i]=μ+((n+λ)P)ii=2,3,...,n+1 x[i]=μ−((n+λ)P)i−ni=n+2,...,2n+1

2n+12n+12n+1个粒子值由上述公式可求得。其中λ\lambdaλ是一个超参数，值越大，越远离。

sigma点集的预测：
xk+1=g(xk,μk)x_{k+1}=g(x_k,\mu_k)xk+1=g(xk,μk)

将粒子输入到非线性函数中，得到下一时刻的先验估计。
根据预测的sigma点预测状态均值和协方差矩阵：
计算各点的权重值：
w[1]=λλ+nw[i]=λ2(λ+n)i=2,3,...,2n+1w^{[1]}=\frac{\lambda}{\lambda+n}\\ w^{[i]}=\frac{\lambda}{2(\lambda+n)}\qquad i=2,3,...,2n+1w[1]=λ+nλw[i]=2(λ+n)λi=2,3,...,2n+1

通过计算出的先验估计和各点的权重矩阵，我们计算新的分布的均值和方差。
μ′=∑i=12n+1w[i]xk+1[i]P′=∑i=12n+1w[i](xk+1[i]−μ′)(xk+1[i]−μ′)T\mu^\prime=\sum_{i=1}^{2n+1}w^{[i]}x_{k+1}^{[i]}\\ \ \\ P^\prime=\sum_{i=1}^{2n+1}w^{[i]}(x_{k+1}^{[i]}-\mu^\prime)(x_{k+1}^{[i]}-\mu^\prime)^Tμ′=i=1∑2n+1w[i]xk+1[i] P′=i=1∑2n+1w[i](xk+1[i]−μ′)(xk+1[i]−μ′)T

UKF Update
xk+1=f(xk,vk)zk+1=h(xk+1)+wk+1x_{k+1}=f(x_k,v_k)\\ \ \\ z_{k+1}=h(x_{k+1})+w_{k+1}xk+1=f(xk,vk) zk+1=h(xk+1)+wk+1

将预测状态转换为测量空间，定义转换的模型叫测量模型。

再一次，我们使用无损转换来解决，但是这里，我们可以不用再产生sigma points了，我们可以直接使用预测出来的sigma点集，并且可以忽略掉处理噪声部分。

首先，将预测到的sigma点转换到测量空间；
其次，利用这些转换后的点计算预测测量值的均值和协方差矩阵。

具体步骤如下：

首先计算出sigma点集在状态空间和测量空间的互相关函数Tk+1∣kT_{k+1|k}Tk+1∣k
Tk+1∣k=∑i=02n+1wi(Xk+1∣k,i−xk+1∣k)(Zk+1∣k,i−zk+1∣k)TT_{k+1|k}=\sum_{i=0}^{2n+1}w_i(X_{k+1|k,i}-x_{k+1|k})(Z_{k+1|k,i}-z_{k+1|k})^TTk+1∣k=i=0∑2n+1wi(Xk+1∣k,i−xk+1∣k)(Zk+1∣k,i−zk+1∣k)T
计算卡尔曼增益Kk+1∣kK_{k+1|k}Kk+1∣k
Kk+1∣k=Tk+1∣k∗Sk+1∣k−1K_{k+1|k}=T_{k+1|k}*S_{k+1|k}^{-1}Kk+1∣k=Tk+1∣k∗Sk+1∣k−1
更新状态，计算Sk+1∣kS_{k+1|k}Sk+1∣k
Sk+1∣k=∑i=02n+1wi(Zk+1∣k,i−zk+1∣k)(Zk+1∣k,i−zk+1∣k)T+RS_{k+1|k}=\sum_{i=0}^{2n+1}w_i(Z_{k+1|k,i}-z_{k+1|k})(Z_{k+1|k,i}-z_{k+1|k})^T+RSk+1∣k=i=0∑2n+1wi(Zk+1∣k,i−zk+1∣k)(Zk+1∣k,i−zk+1∣k)T+R
zk+1∣k=∑i=12n+1w[i]Zk+1∣k[i]z_{k+1|k}=\sum_{i=1}^{2n+1}w^{[i]}Z_{k+1|{k}}^{[i]}zk+1∣k=i=1∑2n+1w[i]Zk+1∣k[i]
更新状态协方差矩阵，计算Pk+1∣k+1P_{k+1|k+1}Pk+1∣k+1
Pk+1∣k+1=Pk+1∣k−Kk+1∣k(zk+1−zk+1∣k)P_{k+1|k+1}=P_{k+1|k}-K_{k+1|k}(z_{k+1}-z_{k+1|k})Pk+1∣k+1=Pk+1∣k−Kk+1∣k(zk+1−zk+1∣k)

UKF的好处有：

sigma点近似非线性转换的效果比线性化更好；
可以省去计算雅可比矩阵的过程；
计算量随维度增长是线性的。

PF（Particle Filter）

粒子滤波，Particle Filter，它利用粒子集来表示概率，可以用在任何形式的状态空间模型上。其核心思想是通过从后验概率中抽取的随机状态粒子来表达其分布，是一种顺序重要性采样法。简单来说，粒子滤波是指寻找一组在状态空间传播的随机样本对概率密度函数进行近似，以样本均值代替积分运算，从而获得状态最小方差分布的过程。这里的样本即指粒子，当样本数量N趋近于无穷时，可以逼近任何形式的概率密度分布。

尽管算法中的概率分布只是真实分布的一种近似，但由于非参数化的特点，它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，能表达比高斯模型更广泛的分布，也对变量参数的非线性特性有更强的建模能力。因此，粒子滤波能够比较精确地表达基于观测量和控制量的后验概率分布，可以用于解决 SLAM问题。

粒子滤波的应用：粒子滤波技术在非线性、非高斯系统表现出来的优越性，决定了它的应用范围非常广泛。另外，粒子滤波器的多模态处理能力，也是它应用广泛的原因之一。国际上，粒子滤波已被应用于各个领域。在经济学领域，它被应用在经济数据预测；在军事领域已经被应用于雷达跟踪空中飞行物，空对空、空对地的被动式跟踪；在交通管制领域它被应用在对车或人视频监控；它还用于机器人的全局定位。

粒子滤波的缺点：虽然粒子滤波算法可以作为解决SLAM问题的有效手段，但是该算法仍然存在着一些问题。其中最主要的问题是需要用大量的样本数量才能很好地近似系统的后验概率密度。

机器人面临的环境越复杂，描述后验概率分布所需要的样本数量就越多，算法的复杂度就越高。因此，能够有效地减少样本数量的自适应采样策略是该算法的重点。
另外，重采样阶段会造成样本有效性和多样性的损失，导致样本贫化现象。如何保持粒子的有效性和多样性，克服样本贫化，也是该算法研究重点。

5. 卡尔曼滤波器参数

卡尔曼滤波器在设计的过程中，有一些参数需要初始化，包括A，B，H，PA，B，H，PA，B，H，P，过程噪声的协方差矩阵QQQ，测量噪声的协方差矩阵RRR。

针对我们的实际系统搭建相应的数学模型，我们可以得到AAA与BBB的表达形式。若是线性系统可以直接得到A，BA，BA，B；若是非线性系统，需要进行线性化。

针对HHH矩阵，根据传感器来建立数学模型，HHH矩阵是状态变量到观测的转换矩阵，表示将状态和观测连接起来的关系。HHH矩阵可以根据要观测的状态量，用0,1矩阵来初始化。

针对估计值的协方差矩阵PPP，表示我们对当前预测状态的信任度，它越小说明我们越相信当前预测状态。协方差矩阵是在不断更新的，只要确定了一个初始值，后面就可以递推出来，而且初始的协方差矩阵只要不是为0，它的取值对滤波效果影响很小，都能很快收敛。

针对过程噪声协方差矩阵QQQ，在不同的系统中，要结合物理情况确定QQQ矩阵。比如，在温度测量系统中，过程噪声由于人体干扰、阳光照射、风等因素造成的。要准确获取QQQ是比较困难的，可以通过对比试验获得。例如，机器人小车在光滑的玻璃上行驶与在粗糙的路面上行驶，两者对比就可以获得在路面上的阻力因素，从而测得阻力噪声方差QQQ。

针对测量噪声协方差矩阵RRR，它是一个统计意义上的参数，可以理解为：对传感器测量的数据经过长期的概率统计，得出它的测量方差。例如，用温度计测量100次房间的温度，这100次温度数据的方差为R′R^\primeR′，这与传感器真实方差RRR是非常接近的。

自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍相关推荐

自动驾驶算法-滤波器系列（五）——高级运动模型在UKF中的应用
CTRV_UKF, CTRA_UKF 1. 基于CTRV的UKF 1.1 CTRV模型 1.2 UKF模型 2. 基于CTRA的UKF 2.1 CTRA模型扩展卡尔曼滤波算法是对非线性的系统方程或者 ...
自动驾驶算法-滤波器系列（一）——详解卡尔曼滤波原理
详解卡尔曼滤波原理什么是卡尔曼滤波? 我们能用卡尔曼滤波做什么? 卡尔曼滤波是如何看到你的问题的使用矩阵来描述问题外部控制量外部干扰用测量量来修正估计值融合高斯分布将所有公式整合起来总 ...
自动驾驶算法-滤波器系列（八）——IMM交互多模型介绍
IMM交互多模型介绍 1. 简介 (1)IMM(Interacting Multiple Model) (2)马尔科夫概率转移矩阵 2. 算法流程 (1)输入交互(模型j) (2)卡尔曼滤波(模型j) ...
自动驾驶算法-滤波器系列（三）——不同运动模型（CV、CA、CTRV、CTRA）的建模和推导
CV & CA & CTRV & CTRA 0. 运动模型简介 1. CV模型 2. CA模型 3. CTRV模型 4. CTRA模型上一篇文章主要讲解了不同卡尔曼滤波的原理 ...
自动驾驶算法-滤波器系列（四）——不同运动模型在KF/EKF中的应用
不同运动模型应用于KF/EKF 1. 基于匀速(CV)运动模型的KF 2. 基于匀加速(CA)运动模型的KF 3. 基于CTRV的EKF 4. 基于CTRA的EKF 上一篇文章中,笔者针对四种不同的运 ...
自动驾驶算法-滤波器系列（七）——ESKF(error-state Kalman Filter)介绍
IMM(Interacting Multiple Model) 1. ESKF是什么? 2. ESKF如何演变出来的? 3. ESKF主要解决什么问题? 4. ESKF算法原理 5. 总结 1. ES ...
自动驾驶算法-滤波器系列（六）——10+种经典滤波算法
经典滤波器介绍 1.限幅滤波法(又称程序判断滤波法) 2.中位值滤波法 3.算术平均滤波法 4.递推平均滤波法(又称滑动平均滤波法) 5.中位值平均滤波法(又称防脉冲干扰平均滤波法) 6.限幅平均滤波 ...
自动驾驶采标系列二：车载感知与车载传感器标定
上一篇文章对自动驾驶采标问题中的一系列法律问题进行了浅析,接下来自动驾驶采标系列里面会对自动驾驶相关技术和知识点进行进一步梳理,来帮助小伙伴深入的了解自动驾驶的架构逻辑与数据采标工作的关系. 上文中提 ...
INS/GNSS组合导航（四）卡尔曼滤波比较之KF/EKF/UKF/PF
1.摘要卡尔曼滤波自1960年代发表至今,在各个时间序列估计领域尤其是位置估计.惯性导航等得到了广泛的应用,后续逐渐演化出EKF.UKF以及PF,本文重点对比KF.EKF与UKF及PF的差异及演化来 ...

自动驾驶算法-滤波器系列（二）—— 卡尔曼滤波简介及其变种（EKF、UKF、PF）介绍