>

## SLAM 中的概率建模

$$p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k})$$


\begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) &= \frac{p(\boldsymbol{z}_k | \boldsymbol{x}_k, \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})}{p(\boldsymbol{z}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})} \end{aligned}


\begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) &=\eta p(\boldsymbol{z}_k | \boldsymbol{x}_k, \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})\\ &= \eta p(\boldsymbol{z}_k | \boldsymbol{x}_k)p(\boldsymbol{x}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1}) \end{aligned}


\begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1}) &= \int p(\boldsymbol{x}_k, \boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})d\boldsymbol{x}_{k-1}\\ &= \int p(\boldsymbol{x}_k| \boldsymbol{x}_{k-1}, \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})p(\boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})d\boldsymbol{x}_{k-1}\\ &= \int p(\boldsymbol{x}_k| \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k})p(\boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})d\boldsymbol{x}_{k-1} \end{aligned}


\begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) &= \eta p(\boldsymbol{z}_k | \boldsymbol{x}_k)\int p(\boldsymbol{x}_k| \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k})p(\boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})d\boldsymbol{x}_{k-1} \end{aligned}


• $p(\boldsymbol{z}_k | \boldsymbol{x}_k)$：当前时刻，当前状态下对环境的观测
• $p(\boldsymbol{x}_k| \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k})$：从前一时刻到当前时刻的状态变化预测
• $p(\boldsymbol{x}_{k-1} | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})$，上一时刻的状态后验概率

• 首先获得上一时刻状态的后验概率分布
• 对上一时刻所有可能的状态根据运动模型对当前状态进行预测，获得当前状态的先验分布
• 结合当前状态的先验分布和当前观测结果的概率分布计算得到当前状态的后验分布

$$\boldsymbol{x}_k = f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_k, \boldsymbol{w}_k)$$


$$\boldsymbol{z}_k = f(\boldsymbol{x}_k, m, \boldsymbol{v}_k)$$


• 假设每一时刻的位姿和观测服从高斯分布，应用高斯分布的性质进行求解，为卡尔曼滤波
• 不对位姿和观测的分布作假设，应用蒙特卡洛的思路进行大量模拟，对位姿进行估计，为粒子滤波

## 卡尔曼滤波 Kalman Filter

### 原始卡尔曼滤波推导

\begin{aligned} \boldsymbol{x}_k &= \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{G}_k\boldsymbol{u}_k + \boldsymbol{w}_k, \qquad &\boldsymbol{w} \sim \mathcal{N}(0, \boldsymbol{W})\\ \boldsymbol{z}_k &= \boldsymbol{H}_k\boldsymbol{x}_k + \boldsymbol{v}_k, \qquad &\boldsymbol{v} \sim \mathcal{N}(0, \boldsymbol{V}) \end{aligned}


$$p(\boldsymbol{x}_{k-1}|\boldsymbol{x}_0, \boldsymbol{u}_{1:k-1}, \boldsymbol{z}_{0:k-1}) = \mathcal{N}(\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1})$$


$$\check{\boldsymbol{x}}_k \sim \mathcal{N}(\check{\boldsymbol{\mu}}_k, \check{\boldsymbol{P}}_k)$$


\begin{aligned} \check{\boldsymbol{\mu}}_k &= \check{\boldsymbol{x}}_k = \boldsymbol{f}(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_k, \boldsymbol{0}) = \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{G}_k\boldsymbol{u}_k\\ \check{\boldsymbol{P}}_k &= \boldsymbol{F}_{k-1}\hat{\boldsymbol{P}}_{k-1}\boldsymbol{F}_{k-1}^T + \boldsymbol{W}_k \end{aligned}


\begin{aligned} &p(\boldsymbol{x}_k, \boldsymbol{z}_k | \boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k-1})\\ =& \mathcal{N} \left( \begin{bmatrix} \boldsymbol{\mu}_{x_k}\\ \boldsymbol{\mu}_{z_k} \end{bmatrix}, \begin{bmatrix} \boldsymbol{\Sigma}_{x_kx_k} & \boldsymbol{\Sigma}_{x_kz_k}\\ \boldsymbol{\Sigma}_{z_kx_k} & \boldsymbol{\Sigma}_{z_kz_k}\\ \end{bmatrix} \right) \end{aligned}


$$\boldsymbol{\Sigma}_{x_kx_k} = \check{\boldsymbol{P}}_k$$


\begin{aligned} \boldsymbol{\Sigma}_{z_kz_k} &= E\left[(\boldsymbol{z}_k - E[\boldsymbol{z}_k])(\boldsymbol{z}_k - E[\boldsymbol{z}_k]^T\right]\\ &= E\left[(\boldsymbol{H}\boldsymbol{x}_k + \boldsymbol{v}_k - E[\boldsymbol{H}\boldsymbol{x}_k + \boldsymbol{v}_k)(\boldsymbol{H}\boldsymbol{x}_k + \boldsymbol{v}_k - E[\boldsymbol{H}\boldsymbol{x}_k + \boldsymbol{v}_k]^T\right]\\ &= E\left[(\boldsymbol{H}\boldsymbol{x}_k - E[\boldsymbol{H}\boldsymbol{x}_k])(\boldsymbol{H}\boldsymbol{x}_k - E[\boldsymbol{H}\boldsymbol{x}_k]^T\right] + E\left[(\boldsymbol{v}_k - E[\boldsymbol{v}_k])(\boldsymbol{v}_k - E[\boldsymbol{v}_k])^T\right]\\ &= \boldsymbol{H}E\left[(\boldsymbol{x}_k - E[\boldsymbol{x}_k])(\boldsymbol{x}_k - E[\boldsymbol{x}_k]^T\right]\boldsymbol{H}^T + E\left[(\boldsymbol{v}_k - E[\boldsymbol{v}_k])(\boldsymbol{v}_k - E[\boldsymbol{v}_k])^T\right]\\ &= \boldsymbol{H}\check{\boldsymbol{P}}_k\boldsymbol{H}^T + \boldsymbol{V}\\ \end{aligned}


\begin{aligned} \boldsymbol{\Sigma}_{x_kz_k} &= E\left[(\boldsymbol{x}_k - E[\boldsymbol{x}_k])(\boldsymbol{z}_k - E[\boldsymbol{z}_k]^T\right]\\ &= E\left[(\boldsymbol{x}_k - \check{\boldsymbol{\mu}}_{x_k})(\boldsymbol{H}\boldsymbol{x}_k - \boldsymbol{H}\check{\boldsymbol{\mu}}_{x_k} + \boldsymbol{v}_k)^T\right]\\ &= E\left[(\boldsymbol{x}_k - \check{\boldsymbol{\mu}}_{x_k})(\boldsymbol{x}_k - \check{\boldsymbol{\mu}}_{x_k})^T\boldsymbol{H}^T\right] + E\left[(\boldsymbol{x}_k - \check{\boldsymbol{\mu}}_{x_k})\boldsymbol{v}_k^T\right]\\ &= \check{\boldsymbol{P}}_k\boldsymbol{H}^T\\ \end{aligned}


$$\boldsymbol{\Sigma}_{z_kx_k} = \boldsymbol{\Sigma}_{x_kz_k} = \boldsymbol{H}\check{\boldsymbol{P}}_k^T = \boldsymbol{H}\check{\boldsymbol{P}}_k$$


\begin{aligned} p(\boldsymbol{x}_k|\boldsymbol{x}_0, \boldsymbol{u}_{1:k}, \boldsymbol{z}_{0:k}) &= \mathcal{N}(\hat{\boldsymbol{\mu}}_k, \hat{\boldsymbol{P}}_k)\\ \hat{\boldsymbol{\mu}}_k = \hat{\boldsymbol{x}}_k &= \check{\boldsymbol{x}}_k + (\check{\boldsymbol{P}}_k\boldsymbol{H}^T)(\boldsymbol{H}\check{\boldsymbol{P}}_k\boldsymbol{H}^T + \boldsymbol{V})^{-1}(\boldsymbol{z}_k - \boldsymbol{H}\check{\boldsymbol{x}}_k)\\ \hat{\boldsymbol{P}}_k &= \check{\boldsymbol{P}}_k - (\check{\boldsymbol{P}}_k\boldsymbol{H}^T)(\boldsymbol{H}\check{\boldsymbol{P}}_k\boldsymbol{H}^T + \boldsymbol{V})^{-1}(\boldsymbol{H}\check{\boldsymbol{P}}_k)\\ &= (\boldsymbol{I} - (\check{\boldsymbol{P}}_k\boldsymbol{H}^T)(\boldsymbol{H}\check{\boldsymbol{P}}_k\boldsymbol{H}^T + \boldsymbol{V})^{-1}\boldsymbol{H})\check{\boldsymbol{P}}_k \end{aligned}


• 利用实际观测值和估计观测值的偏差对当前时刻状态量的估计值进行了一定修正
• 利用观测值的约束降低了当前时刻状态量的不确定度（协方差矩阵）

\begin{aligned} \check{\boldsymbol{\mu}}_k &= \check{\boldsymbol{x}}_k = \boldsymbol{F}_k\boldsymbol{x}_{k-1} + \boldsymbol{G}_k\boldsymbol{u}_k\\ \check{\boldsymbol{P}}_k &= \boldsymbol{F}_{k-1}\hat{\boldsymbol{P}}_{k-1}\boldsymbol{F}_{k-1}^T + \boldsymbol{W}_k \end{aligned}


$$\boldsymbol{K}_k = (\check{\boldsymbol{P}}_k\boldsymbol{H}^T)(\boldsymbol{H}\check{\boldsymbol{P}}_k\boldsymbol{H}^T + \boldsymbol{V})^{-1}$$


\begin{aligned} \hat{\boldsymbol{\mu}}_k = \hat{\boldsymbol{x}}_k &= \check{\boldsymbol{x}}_k + \boldsymbol{K}_k(\boldsymbol{z}_k - \boldsymbol{H}\check{\boldsymbol{x}}_k)\\ \hat{\boldsymbol{P}}_k &= (\boldsymbol{I} - \boldsymbol{K}_k\boldsymbol{H})\check{\boldsymbol{P}}_k \end{aligned}


### 扩展卡尔曼滤波 Extended Kalman Filter (EKF)

\begin{aligned} \boldsymbol{x}_k &= f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_k, \boldsymbol{w}_k)\\ &\approx f(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_k, \boldsymbol{0}) + \left.\frac{\partial f}{\partial \boldsymbol{x}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1}) + \left.\frac{\partial f}{\partial \boldsymbol{w}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}(\boldsymbol{w}_k - \boldsymbol{0})\\ \boldsymbol{z}_k &= h(\boldsymbol{x}_{k}, \boldsymbol{v}_k)\\ &\approx h(\check{\boldsymbol{x}}_{k}, \boldsymbol{0}) + \left.\frac{\partial h}{\partial\boldsymbol{x}}\right|_{\check{\boldsymbol{x}}_{k}, \boldsymbol{0}}(\boldsymbol{x}_{k} - \check{\boldsymbol{x}}_{k}) + \left.\frac{\partial h}{\partial\boldsymbol{v}}\right|_{\check{\boldsymbol{x}}_k, \boldsymbol{0}}(\boldsymbol{v}_k - \boldsymbol{0}) \end{aligned}


\begin{aligned} \boldsymbol{F}_k &= \left.\frac{\partial f}{\partial \boldsymbol{x}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}\\ \boldsymbol{Q}_k &= \left.\frac{\partial f}{\partial \boldsymbol{w}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}\\ \boldsymbol{H}_k &= \left.\frac{\partial h}{\partial\boldsymbol{x}}\right|_{\check{\boldsymbol{x}}_{k}, \boldsymbol{0}}\\ \boldsymbol{R}_k &= \left.\frac{\partial h}{\partial\boldsymbol{v}}\right|_{\check{\boldsymbol{x}}_k, \boldsymbol{0}} \end{aligned}


\begin{aligned} \boldsymbol{x}_k &\approx \check{\boldsymbol{x}}_{k} + \boldsymbol{F}_{k-1}(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1}) + \boldsymbol{Q}_k\boldsymbol{w}_k\\ \boldsymbol{z}_k &\approx \check{\boldsymbol{z}}_k + \boldsymbol{H}_k(\boldsymbol{x}_{k} - \check{\boldsymbol{x}}_{k}) + \boldsymbol{R}_k\boldsymbol{v}_k \end{aligned}


\begin{aligned} E[\boldsymbol{x}_k] &= \check{\boldsymbol{\mu}}_k = E[\check{\boldsymbol{x}}_{k} + \boldsymbol{F}_{k-1}(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1}) + \boldsymbol{Q}_k\boldsymbol{w}_k]\\ &= \check{\boldsymbol{x}}_{k} + \boldsymbol{F}_{k-1}(\boldsymbol{x}_{k-1} - \hat{\boldsymbol{x}}_{k-1}) + \hat{\boldsymbol{x}}_{k-1})\\ E[(\boldsymbol{x}_k - E[\boldsymbol{x}_k])(\boldsymbol{x}_k - E[\boldsymbol{x}_k])^T] &= \check{\boldsymbol{P}}_k \approx E[\boldsymbol{Q}_k\boldsymbol{w}_k\boldsymbol{w}_k^T\boldsymbol{Q}_{k}^T]\\ &= \boldsymbol{Q}_k\boldsymbol{W}\boldsymbol{Q}^T \end{aligned}


\begin{aligned} E(\boldsymbol{z}_k) &\approx \check{\boldsymbol{z}}_k + \boldsymbol{H}_k(\boldsymbol{x}_k - \check{\boldsymbol{x}}_k)\\ E((\boldsymbol{z}_k - E(\boldsymbol{z}_k))(\boldsymbol{z}_k - E(\boldsymbol{z}_k))^T) &\approx E[\boldsymbol{R}\boldsymbol{v}_k\boldsymbol{v}_k^T\boldsymbol{R}^T] = \boldsymbol{R}\boldsymbol{V}\boldsymbol{R}^T \end{aligned}


### 迭代扩展卡尔曼滤波 Iterative Extended Kalman Filter (IEKF)

IEKF 是在 EKF 上考虑了线性化误差的问题，EKF 的线性化点选择了在当前时刻的先验估计 $\check{\boldsymbol{x}}_k$ 进行。过程中线性化误差的会随着线性化点和工作点和差距而别大。因此一个很直观的想法是当我们通过这个线性化点得到当前时刻的后验估计 $\hat{\boldsymbol{x}}_k$ ，我们能不能以这个作为线性化点重新对观测模型进行线性化并重新估计当前时刻状态的后验分布。这个就是 IEKF 的思想。

\begin{aligned} \boldsymbol{x}_{op, k} &= \hat{\boldsymbol{x}}_k\\ \boldsymbol{z}_k &\approx \boldsymbol{z}_{op, k} + \boldsymbol{H}_{op, k}(\boldsymbol{x}_{k} - \boldsymbol{x}_{op, k}) + \boldsymbol{R}_k\boldsymbol{v}_k\\ \boldsymbol{K}_k &= (\check{\boldsymbol{P}}_k\boldsymbol{H}_{op, k}^T)(\boldsymbol{H}_{op, k}\check{\boldsymbol{P}}_k\boldsymbol{H}_{op, k}^T + \boldsymbol{R}\boldsymbol{V}\boldsymbol{R}^T)^{-1}\\ \hat{\boldsymbol{\mu}}_k = \hat{\boldsymbol{x}}_k &= \check{\boldsymbol{x}}_k + \boldsymbol{K}_k(\boldsymbol{z}_k - \boldsymbol{H}_{op, k}(\check{\boldsymbol{x}}_k-\boldsymbol{x}_{op,k}))\\ \hat{\boldsymbol{P}}_k &= (\boldsymbol{I} - \boldsymbol{K}_k\boldsymbol{H}_{op, k})\check{\boldsymbol{P}}_k \end{aligned}


### 基于误差状态的卡尔曼滤波 Error State Extended Kalman Filter (ES-EKF)

ES-EKF 将载体的真实状态分为：名义状态（Nomimal State, 不考虑任何误差得出的理想状态）和误差状态（Error State, 名义状态和真实状态的差值），如下所示:

$$\boldsymbol{x} = \check{\boldsymbol{x}} + \delta\boldsymbol{x}$$


EK-EKF 的思想就是不对真实状态进行滤波，而改为对误差状态滤波。基本流程是：对名义状态使用不含误差的运动模型一直进行更新，对误差状态进行滤波（包括预测和更新），并将其添加至名义状态中对其进行修正，使其更接近真实值。如下所示：

\begin{aligned} \boldsymbol{x}_k &= f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_k, \boldsymbol{w}_k)\\ &\approx f(\check{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_k, \boldsymbol{0}) + \left.\frac{\partial f}{\partial \boldsymbol{x}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}(\boldsymbol{x}_{k-1} - \check{\boldsymbol{x}}_{k-1}) + \left.\frac{\partial f}{\partial \boldsymbol{w}}\right|_{\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{v}_{k}, \boldsymbol{0}}(\boldsymbol{w}_k - \boldsymbol{0})\\ &= \check{\boldsymbol{x}}_{k} + \boldsymbol{F}_{k-1}(\boldsymbol{x}_{k-1} - \check{\boldsymbol{x}}_{k-1}) + \boldsymbol{Q}_{k}\boldsymbol{w}_k \\ \end{aligned}


\begin{aligned} \boldsymbol{x}_k - \check{\boldsymbol{x}}_{k} &= \boldsymbol{F}_{k-1}(\boldsymbol{x}_{k-1} - \check{\boldsymbol{x}}_{k-1}) + \boldsymbol{Q}_{k}\boldsymbol{w}_{k}\\ \Rightarrow\delta\boldsymbol{x}_k &= \boldsymbol{F}_{k-1}\delta\boldsymbol{x}_{k-1} + \boldsymbol{Q}_{k}\boldsymbol{w}_{k} \end{aligned}


\begin{aligned} \boldsymbol{z}_k &= h(\boldsymbol{x}_{k}, \boldsymbol{v}_k)\\ &\approx h(\check{\boldsymbol{x}}_{k}, \boldsymbol{0}) + \left.\frac{\partial h}{\partial\boldsymbol{x}}\right|_{\check{\boldsymbol{x}}_{k}, \boldsymbol{0}}(\boldsymbol{x}_{k} - \check{\boldsymbol{x}}_{k}) + \left.\frac{\partial h}{\partial\boldsymbol{v}}\right|_{\check{\boldsymbol{x}}_k, \boldsymbol{0}}(\boldsymbol{v}_k - \boldsymbol{0})\\ &= h(\check{\boldsymbol{x}}_{k}, \boldsymbol{0}) + \boldsymbol{H}_{k}(\boldsymbol{x}_{k} - \check{\boldsymbol{x}}_{k}) + \boldsymbol{R}_k\boldsymbol{v}_k\\ &= h(\check{\boldsymbol{x}}_{k}, \boldsymbol{0}) + \boldsymbol{H}_{k}\delta\boldsymbol{x}_k + \boldsymbol{R}_k\boldsymbol{v}_k\\ \end{aligned}


\begin{aligned} \check{\boldsymbol{\mu}}_k &= \check{\boldsymbol{x}}_k = f(\boldsymbol{x}_{k-1}, \boldsymbol{u}_k, \boldsymbol{0})\\ \check{\boldsymbol{P}}_k &= \boldsymbol{F}_{k-1}\boldsymbol{P}_{k-1}\boldsymbol{F}_{k-1}^T + \boldsymbol{Q}_k\boldsymbol{W}\boldsymbol{Q}^T \end{aligned}


\begin{aligned} \boldsymbol{K}_k &= \check{\boldsymbol{P}}_{k}\boldsymbol{H}_k^T(\boldsymbol{H}_k\check{\boldsymbol{P}}_k\boldsymbol{H}_k^T + \boldsymbol{R})^{-1}\\ \delta\boldsymbol{x}_k &= \boldsymbol{K}_k(\boldsymbol{z}_k - h(\check{\boldsymbol{x}}_k, \boldsymbol{0}))\\ \end{aligned}


\begin{aligned} \hat{\boldsymbol{\mu}}_x = \hat{\boldsymbol{x}}_k &= \check{\boldsymbol{x}}_k + \delta\boldsymbol{x}_k\\ \hat{\boldsymbol{P}}_k &= (\boldsymbol{I} - \boldsymbol{K}_k\boldsymbol{H}_k)\check{\boldsymbol{P}}_k \end{aligned}


• 旋转的误差状态可以避免过参数化的问题，比较好操作。假如我们用四元数作为估计状态量，那么实际上它只有三个自由度，因此在更新的时候需要做一点特殊处理。如果使用误差状态则没有这个问题
• 误差状态通常比较小，可以有效控制线性化误差
• 误差状态通常变化比较缓慢，因此更新频率的容忍度较高，可以以较慢的频率更新

### 无迹卡尔曼滤波 Unscented Kalman Filter (UKF)

UKF 的思想是按照高斯分布的性质进行采样选出若干个 sigma 点，用这些点来模拟当前的先验分布，对这些点通过运动和观测模型进行非线性转换。再分析转换后的点的分布近似当前时刻的后验分布。具体过程如下：

• 预测步骤：

\begin{aligned} \boldsymbol{\mu}_y &= \begin{bmatrix} \hat{\boldsymbol{x}}_{k-1}\\ \boldsymbol{0} \end{bmatrix}\\ \boldsymbol{\Sigma}_{yy} &= \begin{bmatrix} \hat{\boldsymbol{P}}_{k-1}& \boldsymbol{0}\\ 0 & \boldsymbol{W}_k \end{bmatrix} \end{aligned}


\begin{aligned} \boldsymbol{LL}^T &= \boldsymbol{\Sigma}_{yy}\\ \boldsymbol{y}_0 &= \boldsymbol{\mu}_y\\ \boldsymbol{y}_i &= \boldsymbol{\mu}_y + \sqrt{L + \kappa}\text{col}_i\boldsymbol{L}, &\qquad i = 1, 2, ..., L\\ \boldsymbol{y}_{i+L} &= \boldsymbol{\mu}_y - \sqrt{L + \kappa}\text{col}_i\boldsymbol{L}, &\qquad i = 1, 2, ..., L \end{aligned}


$$\boldsymbol{y}_i = \begin{bmatrix} \hat{\boldsymbol{x}}_{k-1, i}\\ \boldsymbol{w}_{k, i} \end{bmatrix}$$


$$\check{\boldsymbol{x}}_{k, i} = f(\hat{\boldsymbol{x}}_{k-1, i}, \boldsymbol{u}_k, \boldsymbol{w}_{k, i}) \qquad i = 0, 1, ..., 2L$$


\alpha_i = \left\{ \begin{aligned} &\frac{\kappa}{L+\kappa}, &\qquad i = 0\\ &\frac{1}{2}\frac{1}{L + \kappa}&\qquad\text{others} \end{aligned} \right.


\begin{aligned} \check{\boldsymbol{x}}_k &= \check{\boldsymbol{\mu}}_x = \sum_{i = 0}^{2L}\alpha_{i}\check{\boldsymbol{x}}_{k, i}\\ \check{\boldsymbol{P}}_k &= \boldsymbol{\Sigma}_{xx} = \sum_{i = 0}^{2L}\alpha_i(\check{\boldsymbol{x}}_{k, i}-\check{\boldsymbol{\mu}}_x)(\check{\boldsymbol{x}}_{k, i}-\check{\boldsymbol{\mu}}_x)^T \end{aligned}

• 校正步骤：

\begin{aligned} \boldsymbol{\mu}_y &= \begin{bmatrix} \check{\boldsymbol{x}}_{k}\\ \boldsymbol{0} \end{bmatrix}\\ \boldsymbol{\Sigma}_{yy} &= \begin{bmatrix} \check{\boldsymbol{P}}_{k}& \boldsymbol{0}\\ 0 & \boldsymbol{V}_k \end{bmatrix} \end{aligned}


\begin{aligned} \boldsymbol{LL}^T &= \boldsymbol{\Sigma}_{yy}\\ \boldsymbol{y}_0 &= \boldsymbol{\mu}_y\\ \boldsymbol{y}_i &= \boldsymbol{\mu}_y + \sqrt{L + \kappa}\text{col}_i\boldsymbol{L}, &\qquad i = 1, 2, ..., L\\ \boldsymbol{y}_{i+L} &= \boldsymbol{\mu}_y - \sqrt{L + \kappa}\text{col}_i\boldsymbol{L}, &\qquad i = 1, 2, ..., L \end{aligned}


$$\boldsymbol{y}_i = \begin{bmatrix} \check{\boldsymbol{x}}_{k, i}\\ \boldsymbol{v}_{k, i} \end{bmatrix}$$


$$\check{\boldsymbol{z}}_{k, i} = h(\check{\boldsymbol{x}}_{k, i}, \boldsymbol{v}_{k, i}) \qquad i = 0, 1, ..., 2L$$


\begin{aligned} \boldsymbol{\mu}_{z, k} &= \sum_{i = 0}^{2L}\alpha_{i}\check{\boldsymbol{z}}_{k, i}\\ \boldsymbol{\Sigma}_{zz, k} &= \sum_{i = 0}^{2L}\alpha_i(\check{\boldsymbol{z}}_{k, i}-\boldsymbol{\mu}_{z, k})(\check{\boldsymbol{z}}_{k, i}-\boldsymbol{\mu}_{z, k})^T\\ \boldsymbol{\Sigma}_{xz, k} &= \sum_{i = 0}^{2L}\alpha_i(\check{\boldsymbol{x}}_{k, i}-\check{\boldsymbol{x}}_{k})(\check{\boldsymbol{z}}_{k, i}-\boldsymbol{\mu}_{z, k})^T \end{aligned}


## 粒子滤波 Particle Filter (PF)

$$\begin{bmatrix} \hat{\boldsymbol{x}}_{k-1, m}\\ \boldsymbol{w}_{k, m} \end{bmatrix}\leftarrow p(\boldsymbol{x}_{k-1}|\boldsymbol{0}_0, \boldsymbol{u}_{1:k-1}, \boldsymbol{z}_{1:k-1})p(\boldsymbol{w})$$


$$\check{\boldsymbol{x}}_{k, m} = f(\hat{\boldsymbol{x}}_{k-1, m}, \boldsymbol{u}_k, \boldsymbol{w}_{k, m})$$


$$\alpha_{k, m} = \eta p(\boldsymbol{z}_k | \check{\boldsymbol{x}}_{k, m})$$


$$\check{\boldsymbol{z}}_{k, m} = h(\check{\boldsymbol{x}}_{k, m}, \boldsymbol{0})$$


## 参考资料

Built with Hugo
Theme Stack designed by Jimmy