KF EKF
早期SLAM的优化方式一般用滤波器,计算资源少,适合小场景,计算资源不足的场合.
状态估计
将路标和位姿写在一起
$\boldsymbol{x}_{k} \triangleq\left\{\boldsymbol{x}_{k}, \boldsymbol{y}_{1}, \ldots, \boldsymbol{y}_{m}\right\}$
运动方程: $\left\{\begin{array}{l}{\boldsymbol{x}_{k}=f\left(\boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)+\boldsymbol{w}_{k}} \\ {\boldsymbol{z}_{k}=h\left(\boldsymbol{x}_{k}\right)+\boldsymbol{v}_{k}}\end{array}\right. \quad k=1, \ldots, N$
其中 w v 是噪声,u运动输入,x位姿,路标,z观测
用开始0到现在k的数据估计当前的状态xk
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k}\right)$
按照贝叶斯法则可以将zk和xk位置调换
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k}\right) \propto P\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right) P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)$
其中$P\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right)$ 为似然
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)$为先验,缺少zk,只是通过运动输入uk从xk-1估计而来
就是先验通过似然矫正后得到后验
KF
全概率公式可得先验部分
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)=\int P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right) P\left(\boldsymbol{x}_{k-1} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right) \mathrm{d} \boldsymbol{x}_{k-1}$
假设马尔科夫性
左边 $P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)=P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{k-1}, \boldsymbol{u}_{k}\right)$ 即当前状态只与上一状态有关
右边 $P\left(\boldsymbol{x}_{k-1} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)=P\left(\boldsymbol{x}_{k-1} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k-1}, \boldsymbol{z}_{1 : k-1}\right)$ 上一状态与前面状态有关
假设线性高斯系统
$\left\{\begin{array}{l}{\boldsymbol{x}_{k}=\boldsymbol{A}_{k} \boldsymbol{x}_{k-1}+\boldsymbol{u}_{k}+\boldsymbol{w}_{k}} \\ {\boldsymbol{z}_{k}=\boldsymbol{C}_{k} \boldsymbol{x}_{k}+\boldsymbol{v}_{k}}\end{array}\right. \quad k=1, \ldots, N$
$\boldsymbol{w}_{k} \sim N(\mathbf{0}, \boldsymbol{R}) . \quad \boldsymbol{v}_{k} \sim N(\mathbf{0}, \boldsymbol{Q})$
预测,从上一个时刻的状态根据运动输入推断信息（但是有噪声）,当前时刻的状态分布
先验部分
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{1 : k-1}\right)=N\left(\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{T}+\boldsymbol{R}\right)$
记
$\overline{\boldsymbol{x}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \quad \overline{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{T}+\boldsymbol{R}$ --> $N\left(\overline{\boldsymbol{x}}_{k}, \overline{\boldsymbol{P}}_{k}\right)$
似然部分
$P\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right)=N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right)$
后验部分 设$\boldsymbol{x}_{k} \sim N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)$
$N\left(\hat{\boldsymbol{x}}_{k}, \hat{\boldsymbol{P}}_{k}\right)=N\left(\boldsymbol{C}_{k} \boldsymbol{x}_{k}, \boldsymbol{Q}\right) \cdot N\left(\overline{\boldsymbol{x}}_{k}, \overline{\boldsymbol{P}}_{k}\right)$
指数部分展开
$\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)^{T} \hat{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)=\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)^{T} \boldsymbol{Q}^{-1}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \boldsymbol{x}_{k}\right)+\left(\boldsymbol{x}_{k}-\overline{\boldsymbol{x}}_{k}\right)^{T} \overline{\boldsymbol{P}}_{k}^{-1}\left(\boldsymbol{x}_{k}-\overline{\boldsymbol{x}}_{k}\right)$
推导
二次项部分
$\hat{\boldsymbol{P}}_{k}^{-1}=\boldsymbol{C}_{k}^{T} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k}+\overline{\boldsymbol{P}}_{k}^{-1}$
定义一个中间变量：
$\boldsymbol{K}=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{T} \boldsymbol{Q}^{-1}$
左右各乘$\hat{\boldsymbol{P}}_{k}$
$I=\hat{P}_{k} C_{k}^{T} Q^{-1} C_{k}+\hat{P}_{k} \overline{P}_{k}^{-1}=K C_{k}+\hat{P}_{k} \overline{P}_{k}^{-1}$ --->$\hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \overline{\boldsymbol{P}}_{k}$
一次项部分
$-2 \hat{\boldsymbol{x}}_{k}^{T} \hat{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k}=-2 \boldsymbol{z}_{k}^{T} \boldsymbol{Q}^{-1} \boldsymbol{C}_{k} \boldsymbol{x}_{k}-2 \overline{\boldsymbol{x}}_{k}^{T} \overline{\boldsymbol{P}}_{k}^{-1} \boldsymbol{x}_{k}$
整理（取系数并转置）
$\hat{\boldsymbol{P}}_{k}^{-1} \hat{\boldsymbol{x}}_{k}=\boldsymbol{C}_{k}^{T} \boldsymbol{Q}^{-1} \boldsymbol{z}_{k}+\overline{\boldsymbol{P}}_{k}^{-1} \overline{\boldsymbol{x}}_{k}$
两侧乘以 P^k 并代入
$\begin{aligned} \hat{\boldsymbol{x}}_{k} &=\hat{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{T} \boldsymbol{Q}^{-1} \boldsymbol{z}_{k}+\hat{\boldsymbol{P}}_{k} \overline{\boldsymbol{P}}_{k}^{-1} \overline{\boldsymbol{x}}_{k} \\ &=\boldsymbol{K} \boldsymbol{z}_{k}+\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \overline{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \overline{\boldsymbol{x}}_{k}\right) \end{aligned}$
1.预测
$\overline{\boldsymbol{x}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{x}}_{k-1}+\boldsymbol{u}_{k}, \quad \overline{\boldsymbol{P}}_{k}=\boldsymbol{A}_{k} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{A}_{k}^{T}+\boldsymbol{R}$
2. 更新：
先计算 K，它又称为卡尔曼增益：
$\boldsymbol{K}=\overline{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{T}\left(\boldsymbol{C}_{k} \overline{\boldsymbol{P}}_{k} \boldsymbol{C}_{k}^{T}+\boldsymbol{Q}\right)^{-1}$
计算后验概率的分布：
$\hat{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}\left(\boldsymbol{z}_{k}-\boldsymbol{C}_{k} \overline{\boldsymbol{x}}_{k}\right)$
$\hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K} \boldsymbol{C}_{k}\right) \overline{\boldsymbol{P}}_{k}$
EKF
SLAM 中的运动方程和观测方程通常 是非线性函数.一个高斯分布，经过非线性变换后，往往不再是高斯 分布，所以在非线性系统中，我们必须取一定的近似，将一个非高斯的分布近似成一个高斯分布。
在某个点附近考虑运动方程以及观测方程的 一阶泰勒展开，只保留一阶项，即线性的部分，然后按照线性系统进行推导。
k−1 时刻的 均值与协方差矩阵为 $\hat{\boldsymbol{x}}_{k-1}, \hat{\boldsymbol{P}}_{k-1}$
运动方程和观测方程线性化
$\boldsymbol{x}_{k} \approx f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right)+\left.\frac{\partial f}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}}\left(\boldsymbol{x}_{k-1}-\hat{\boldsymbol{x}}_{k-1}\right)+\boldsymbol{w}_{k}$ 记$\boldsymbol{F}=\left.\frac{\partial f}{\partial \boldsymbol{x}_{k-1}}\right|_{\hat{\boldsymbol{x}}_{k-1}}$
观测方程线性化
$\boldsymbol{z}_{k} \approx h\left(\overline{\boldsymbol{x}}_{k}\right)+\left.\frac{\partial h}{\partial \boldsymbol{x}_{k}}\right|_{\overline{\boldsymbol{x}}_{k}}\left(\boldsymbol{x}_{k}-\hat{\boldsymbol{x}}_{k}\right)+\boldsymbol{n}_{k}$ 记$\boldsymbol{H}=\left.\frac{\partial h}{\partial \boldsymbol{x}_{k}}\right|_{\overline{\boldsymbol{x}}_{k}}$
预测 根据运动方程
$P\left(\boldsymbol{x}_{k} | \boldsymbol{x}_{0}, \boldsymbol{u}_{1 : k}, \boldsymbol{z}_{0 : k-1}\right)=N\left(f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right), \boldsymbol{F} \hat{\boldsymbol{P}}_{k-1} \boldsymbol{F}^{\mathrm{T}}+\boldsymbol{R}_{k}\right)$
记这里先验和协方差的均值为
$\overline{\boldsymbol{x}}_{k}=f\left(\hat{\boldsymbol{x}}_{k-1}, \boldsymbol{u}_{k}\right), \quad \overline{\boldsymbol{P}}_{k}=\boldsymbol{F} \hat{\boldsymbol{P}}_{k} \boldsymbol{F}^{T}+\boldsymbol{R}_{k}$
1.观测
$P\left(\boldsymbol{z}_{k} | \boldsymbol{x}_{k}\right)=N\left(h\left(\overline{\boldsymbol{x}}_{k}\right)+\boldsymbol{H}\left(\boldsymbol{x}_{k}-\overline{\boldsymbol{x}}_{k}\right), \boldsymbol{Q}_{k}\right)$
2.更新
卡尔曼增益
$\boldsymbol{K}_{k}=\overline{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}\left(\boldsymbol{H} \overline{\boldsymbol{P}}_{k} \boldsymbol{H}^{\mathrm{T}}+\boldsymbol{Q}_{k}\right)^{-1}$
在卡尔曼增益的基础上，后验概率
$\hat{\boldsymbol{x}}_{k}=\overline{\boldsymbol{x}}_{k}+\boldsymbol{K}_{k}\left(\boldsymbol{z}_{k}-h\left(\overline{\boldsymbol{x}}_{k}\right)\right), \hat{\boldsymbol{P}}_{k}=\left(\boldsymbol{I}-\boldsymbol{K}_{k} \boldsymbol{H}\right) \overline{\boldsymbol{P}}_{k}$
