EKF slam

xiaoma_bk

已于 2022-03-31 20:11:34 修改

阅读量814

点赞数 1

分类专栏： SLAM 技术相关文章标签：人工智能

于 2022-03-22 20:31:23 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/123670629

版权

SLAM 技术相关专栏收录该内容

3 篇文章 1 订阅

订阅专栏

文章目录

- EKF SLAM

EKF SLAM

PPT 教程地址：http://ais.informatik.uni-freiburg.de/teaching/

在估计理论中，扩展卡尔曼滤波器 (EKF) 是卡尔曼滤波器的非线性版本，它对当前均值和协方差的估计进行线性化。在定义明确的过渡模型的情况下，EKF 已被视为非线性状态估计、导航系统和 GPS 理论中的事实标准。
缺点：与其线性对应物不同，扩展卡尔曼滤波器通常不是最优估计器（如果测量和状态转换模型都是线性的，则它是最优的，因为在这种情况下扩展卡尔曼滤波器与常规卡尔曼滤波器相同）。此外，如果状态的初始估计是错误的，或者如果过程建模不正确，则滤波器可能会由于其线性化而迅速发散。扩展卡尔曼滤波器的另一个问题是估计的协方差矩阵倾向于低估真实的协方差矩阵，因此在不添加“稳定噪声”的情况下可能会在统计意义上变得不一致[8]。
迭代扩展卡尔曼滤波器：通过递归修改泰勒展开的中心点来改进扩展卡尔曼滤波器的线性化。这以增加计算要求为代价减少了线性化误差。 [13]
鲁棒扩展卡尔曼滤波器：
- 扩展卡尔曼滤波器是通过对当前状态估计的信号模型进行线性化并使用线性卡尔曼滤波器来预测下一个估计而产生的。这试图产生一个局部最优的滤波器，然而，它不一定是稳定的，因为基础里卡蒂方程的解不能保证是正定的。提高性能的一种方法是仿代数 Riccati 技术 [14]，它以最优性换取稳定性。扩展卡尔曼滤波器的熟悉结构被保留，但通过为增益设计选择伪代数 Riccati 方程的正定解来实现稳定性。
- 提高扩展卡尔曼滤波器性能的另一种方法是使用鲁棒控制的 H 无穷大结果。通过在设计 Riccati 方程中添加一个正定项来获得稳健的滤波器。 [15] 附加项由标量参数化，设计人员可以调整该标量以在均方误差和峰值误差性能标准之间取得平衡。
不变扩展卡尔曼滤波器：不变扩展卡尔曼滤波器 (IEKF) 是 EKF 的修改版本，适用于具有对称性（或不变性）的非线性系统。它结合了 EKF 和最近引入的保持对称滤波器的优点。 IEKF 不使用基于线性输出误差的线性校正项，而是使用基于不变输出误差的几何自适应校正项；同样，增益矩阵不是从线性状态误差更新，而是从不变状态误差更新。主要好处是增益和协方差方程在比平衡点大得多的轨迹集上收敛到常数值，因为 EKF 就是这种情况，这导致估计的更好收敛。

EKF

EKF 线性化：一阶泰勒展开
- 预测： ${g(u_t,x_{t-1})\approx g(u_t,\mu_{t-1}) + \frac{\partial g(u_t,\mu_{t-1})}{\partial x_{t-1}}(x_{t-1}-\mu_{t-1})}$
- 修正： ${h(x_t)\approx h(\bar{\mu_t})+\frac{\partial h(\bar{\mu_t})}{\partial x_t}(x_t-\bar{\mu_t})}$
- 两偏导用 $G_t$ 和 ${H_t}$ 表示，Jacobian matrices
Jacobian Matrix
- 他是一个非线性矩阵： ${m\times n}$
- 给一个向量值函数： $\begin{pmatrix} g_1(x)\\g_2(x)\\ \vdots \\ g_m(x) \end{pmatrix}}$
- 则雅克比矩阵被定义为：
  
  ${G_x=\begin{pmatrix} \frac{\partial g_1}{\partial x_1} &\frac{\partial g_1}{\partial x_2}& \cdots& \frac{\partial g_1}{\partial x_n} \\ \frac{\partial g_2}{\partial x_1} &\frac{\partial g_2}{\partial x_2}& \cdots& \frac{\partial g_2}{\partial x_n} \\ \vdots & \vdots & \vdots & \vdots \\ \frac{\partial g_m}{\partial x_1} &\frac{\partial g_m}{\partial x_2}& \cdots& \frac{\partial g_m}{\partial x_n} \end{pmatrix}}$
- 它是给定点处向量值函数的切平面的方向
- 一个标量值函数的梯度

Extended Kalman filter Algorithm

Extended_kalman_filter ( ${\mu_{t-1}}$ ， ${\Sigma_{t-1}}$ ， ${u_t}$ ， ${z_t}$ )
predict:
- ${\bar{\mu_t}=g(u_t,\mu_{t-1})}$
- ${\bar{\Sigma_t}=G_t\Sigma_{t-1}G_t^T+R^T}$
update:
- ${K_t = \bar{\Sigma_t}H_t^T(H_t\bar{\Sigma_t}H_t^T+Q_t)^{-1}}$
- ${\mu_t = \bar{\mu_t}+K_t(z_t-h(\bar{\mu_t}))}$
- ${\Sigma_t=(I-K_tH_t)\bar{\Sigma_t}}$
return ${\mu_t}$ ， ${\Sigma_t}$

EKF to SLAM

对于2D空间，状态
- ${x_t = (x,y,\theta,m_{1,x},m_{1,y},\cdots,m_{n,x},m_{n,y})^T}$
  - 机器人位姿，landmark 1，…，landmark n
地图有N个landmark时，有 ${3+2n}$ 维高斯分布，其置信度为：
- ${\mu =\begin{pmatrix} x&y&\theta&m_{1,x}&m_{1,y}& \cdots& m_{n,x}&m_{n,y}\end{pmatrix}^T}$
- ${\Sigma =\begin{pmatrix} \sigma_{xx} & \sigma_{xy} & \sigma_{x\theta} & \sigma_{xm_{1,x}} & \sigma_{xm_{1,y}} & \cdots & \sigma_{xm_{n,x}} & \sigma_{xm_{n,y}} \\ \sigma_{yx} & \sigma_{yy} & \sigma_{y\theta} & \sigma_{ym_{1,x}} & \sigma_{ym_{1,y}} & \cdots & \sigma_{ym_{n,x}} & \sigma_{ym_{n,y}} \\ \sigma_{\theta x} & \sigma_{\theta y} & \sigma_{\theta \theta} & \sigma_{\theta m_{1,x}} & \sigma_{ym_{1,y}} & \cdots & \sigma_{\theta m_{n,x}} & \sigma_{\theta m_{n,y}} \\ \sigma_{m_{1,x}x} & \sigma_{m_{1,x}y} & \sigma_{m_{1,x}\theta } & \sigma_{m_{1,x}m_{1,x}} & \sigma_{m_{1,x}m_{1,y}} & \cdots & \sigma_{m_{1,x} m_{n,x}} & \sigma_{m_{1,x} m_{n,y}} \\ \sigma_{m_{1,y}x} & \sigma_{m_{1,y}y} & \sigma_{m_{1,y}\theta } & \sigma_{m_{1,y}m_{1,x}} & \sigma_{m_{1,y}m_{1,y}} & \cdots & \sigma_{m_{1,y} m_{n,x}} & \sigma_{m_{1,y} m_{n,y}} \\ \vdots &\vdots &\vdots & \vdots & \vdots &\vdots & \vdots & \vdots \\ \sigma_{m_{n,x}x} & \sigma_{m_{n,x}y} & \sigma_{m_{n,x}\theta } & \sigma_{m_{n,x}m_{1,x}} & \sigma_{m_{n,x}m_{1,y}} & \cdots & \sigma_{m_{n,x} m_{n,x}} & \sigma_{m_{n,x} m_{n,y}} \\ \sigma_{m_{n,y}x} & \sigma_{m_{n,y}y} & \sigma_{m_{n,y}\theta } & \sigma_{m_{n,y}m_{1,x}} & \sigma_{m_{n,y}m_{1,y}} & \cdots & \sigma_{m_{n,y} m_{n,x}} & \sigma_{m_{n,y} m_{n,y}} \end{pmatrix}}$
- 简写：
  - ${\mu=(x,m)^T}$
  - ${\begin{pmatrix} \Sigma_{xx} & \Sigma_{xm} \\ \Sigma_{mx} & \Sigma_{mm} \end{pmatrix}}$
状态预测 – 红色部分
- ${\mu=({\color{red}x},m)^T}$
- ${\Sigma = \begin{pmatrix} {\color{red}\Sigma_{xx}}& {\color{red}\Sigma_{xm}} \\ {\color{red}\Sigma_{mx} }& \Sigma_{mm} \end{pmatrix}}$
初始化：
- ${\mu= \begin{pmatrix}0 &0& 0& \cdots & 0\end{pmatrix}^T}$
- ${\Sigma = \begin{pmatrix} 0& 0 \\ 0 & \infty \end{pmatrix}}$
预测：
- ${\bar{\mu_t}}$ 预测：
  - 目标：根据机器人的运动更新状态空间
  - 已知线速度和角速度，时间：
    - ${\begin{pmatrix} x'\\ y'\\ \theta'\end{pmatrix}= \begin{pmatrix} x \\ y \\ \theta \end{pmatrix}+ \begin{pmatrix} -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ \frac{v_t}{w_t}cos(\theta) -\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\ w_t \Delta _t \end{pmatrix}}$
  - 到 ${2N+3}$ 空间：
  - ${\begin{pmatrix} x'\\ y'\\ \theta' \\ \vdots \end{pmatrix}= \begin{pmatrix} x \\ y \\ \theta \\ \vdots \end{pmatrix} + \underset{F_x^T}{\underbrace{\begin{pmatrix} 1&0&0 & 0...0 \\ 0&1&0 & 0...0 \\ 0&0&1 & \underset{2N cols}{\underbrace{0...0}}\end{pmatrix}^T}}\begin{pmatrix} -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ \frac{v_t}{w_t}cos(\theta) -\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\ w_t \Delta _t \end{pmatrix}}$
- ${\bar{\Sigma_t}}$ 预测：
  - 该功能 g 仅影响机器人运动，不影响landmark
  - ${G_t = \begin{pmatrix} G_t^x &0\\0 &I \end{pmatrix}}$
    - ${G_t^x}$ ： Jacobian of the motion ${3\times 3}$
    - $I$ ：Identity ${2N\times 2N}$
  - ${G_t^x=\frac{\partial}{\partial{(x,y,\theta)^T}}\begin{bmatrix} \begin{pmatrix} x'\\ y'\\ \theta'\end{pmatrix}= \begin{pmatrix} x \\ y \\ \theta \end{pmatrix}+ \begin{pmatrix} -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ \frac{v_t}{w_t}cos(\theta) -\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\ w_t \Delta _t \end{pmatrix} \end{bmatrix}}$
  - ${G_t^x= \begin{pmatrix} 1& 0& \frac{v_t}{w_t}cos(\theta)+\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\0&1& -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ 0& 0& 1\end{pmatrix}}$
  - ${\bar{\Sigma_t}=G_t\Sigma_{t-1}G_t^T+R^T= \begin{pmatrix} G_t^x & 0 \\ 0 & I\end{pmatrix} \begin{pmatrix} \Sigma_{xx} & \Sigma_{xm} \\ \Sigma_{mx} & \Sigma_{mm}\end{pmatrix} \begin{pmatrix} {G_t^x}^T & 0 \\ 0 & I\end{pmatrix} +R_t = \begin{pmatrix} G_t^x\Sigma_{xx}{G_t^x}^T & G_t^x\Sigma_{xm} \\ {G_t^x\Sigma_{mx}}^T & \Sigma_{mm}\end{pmatrix}+R_t}$
- 《自主机器人导论》中里程计模型。
  - ${\begin{pmatrix} x'\\ y'\\ \theta'\end{pmatrix}= \begin{pmatrix} x \\ y \\ \theta \end{pmatrix}+ \begin{pmatrix} \Delta{s} \ cos(\theta+\Delta \theta/2) \\ \Delta{s} \ sin(\theta+\Delta \theta/2) \\ \Delta \theta \end{pmatrix}}$
  - 其中：
    - ${\Delta \theta = \frac{\Delta s_r - \Delta s_l}{b}}$
    - ${\Delta s = \frac{\Delta s_r + \Delta s_l}{2}}$
    - ${\Delta s_{l/r}=k_{l/r} \cdot \Delta e_{l/r}}$
    - ${k_{l/r}}$ 左右轮系数，把编码器增量 ${\Delta e_{l/r}}$ 转换为左右轮的位移， $b$ 是轮间距。左右轮位移的增量 ${\Delta s_{l/r}}$ 服从高斯分布，均值就是编码器计算出的位移增量，标准差与增量大小成正比。如果 t-1 时刻机器人位姿的协方差为 ${\Sigma_{\xi,t}}$ ，控制的协方差也就是编码器的协方差为： ${\Sigma_u}$ ，那么机器人位姿在 t 时刻的协方差就是： ${\Sigma_{\xi,t}=G_{\xi}\Sigma_{\xi,t-1}G_{\xi}^T+G_u'\Sigma_uG_u'^T}$
  - ${G_\xi}$ ：为状态更新中对状态变量的偏导。
  - ${G_u}$ ：为状态更新中对编码器变量的偏导。
预测总结：
- ${F_x = \begin{pmatrix} 1&0&0 & 0...0 \\ 0&1&0 & 0...0 \\ 0&0&1 & \underset{2N cols}{\underbrace{0...0}}\end{pmatrix}}$
- ${\bar{\mu_t}=\mu_{t-1}+F_x^T\begin{pmatrix} -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ \frac{v_t}{w_t}cos(\theta) -\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\ w_t \Delta _t \end{pmatrix}}$
- ${G_t=I+F_x^T \begin{pmatrix} 0& 0& \frac{v_t}{w_t}cos(\theta)+\frac{v_t}{w_t}cos(\theta+w_t\Delta _t) \\0&0& -\frac{v_t}{w_t}sin(\theta)+\frac{v_t}{w_t}sin(\theta+w_t\Delta _t) \\ 0& 0& 0\end{pmatrix} F_x}$
- ${\bar{\Sigma_t}=G_t\Sigma_{t-1}G_t^T+ F_x^TR_t^xF_x}$
测量模型
- ${c_t^i=j}$ 在时间 $t$ 的第 $i$ 次测量观测到landmark 的下表为 $j$
- 步骤：若第一次观测到landmark时初始化，计算观测期望+雅克比，再计算卡尔曼
- 测距模型： ${z_t^i=\begin{pmatrix}r_t^i& \phi_t^i \end{pmatrix}^T}$
- 若landmark未被观测到时：
  - landmark $j$ 的位置 = 机器人的预估位姿 + 相对测量
  - ${\begin{pmatrix} \bar{\mu_{j,x}} \\ \bar{\mu_{j,y}} \end{pmatrix} = \begin{pmatrix} \bar{\mu_{t,x}} \\ \bar{\mu_{t,y}} \end{pmatrix} + \begin{pmatrix} r_t^i cos(\phi_t^i+\bar{\mu_{t,\theta}}) \\r_t^i sin(\phi_t^i+\bar{\mu_{t,\theta}}) \end{pmatrix}}$
- 通过当前估计计算期望观测：
  - ${\delta=\begin{pmatrix}\delta_x \\ \delta_y \end{pmatrix}= \begin{pmatrix} \bar{\mu_{j,x}}-\bar{\mu_{t,x}} \\\bar{\mu_{j,y}}-\bar{\mu_{t,y}} \end{pmatrix}}$
  - ${q=\delta^T\delta}$
  - ${\hat{z_t^i}=\begin{pmatrix} \sqrt q \\ atan2(\delta_y,\delta_x)- \bar{\mu_{t,\theta}} \end{pmatrix} = h(\bar{\mu_t})}$
- 通过观测期望，计算 jacobian
  - $^{low}H_t^i=\frac{\partial h(\bar{\mu_t})}{\partial \bar{\mu_t}}}$
  - 涉及到的低维空间： ${(x,y,\theta,m_{j,x},m_{j,y})}$
  - $\frac{1}{1+x^2}}$
  - 故有： ${^{low}H_t^i=\frac{\partial h(\bar{\mu_t})}{\partial \bar{\mu_t}} = \frac{1}{q}\begin{pmatrix} -\sqrt{q}\delta_x & -\sqrt{q}\delta_y & 0 & \sqrt{q}\delta_x & \sqrt{q}\delta_y \\ \delta_y & -\delta_x & -q & -\delta_y & \delta_x \end{pmatrix}}$
  - 其映射到高维空间：
    - ${H_t^i=^{low}H_t^i F_{x,j}}$
    - ${F_{x,j} = \begin{pmatrix} 1 & 0 & 0 & 0...0 & 0 & 0 & 0...0 \\ 0 & 1 & 0 & 0...0 & 0 & 0 & 0...0 \\ 0 & 0 & 1 & 0...0 & 0 & 0 & 0...0 \\ 0 & 0 & 0 & 0...0 & 1 & 0 & 0...0 \\ 0 & 0 & 0 & \underset{2j-2}{\underbrace{0...0}} & 0 & 1 & \underset{2N -2}{\underbrace{0...0}} \end{pmatrix}}$
修正总结：
- ${Q_t=\begin{pmatrix} \delta_r^2 & 0 \\ 0& \sigma_{\phi}^2 \end{pmatrix}}$
- 对于所有观测特征 ${z_t^i=\begin{pmatrix}r_t^i& \phi_t^i \end{pmatrix}^T}$ 作：
  - ${j = c_t^i}$
  - 如果 landmark $j$ 在此之前没有看到过：
    - ${\begin{pmatrix} \bar{\mu_{j,x}} \\ \bar{\mu_{j,y}} \end{pmatrix} = \begin{pmatrix} \bar{\mu_{t,x}} \\ \bar{\mu_{t,y}} \end{pmatrix} + \begin{pmatrix} r_t^i cos(\phi_t^i+\bar{\mu_{t,\theta}}) \\r_t^i sin(\phi_t^i+\bar{\mu_{t,\theta}}) \end{pmatrix}}$
  - ${\delta=\begin{pmatrix}\delta_x \\ \delta_y \end{pmatrix}= \begin{pmatrix} \bar{\mu_{j,x}}-\bar{\mu_{t,x}} \\\bar{\mu_{j,y}}-\bar{\mu_{t,y}} \end{pmatrix}}$
  - ${q=\delta^T\delta}$
  - ${\hat{z_t^i}=\begin{pmatrix} \sqrt q \\ atan2(\delta_y,\delta_x)- \bar{\mu_{t,\theta}} \end{pmatrix} = h(\bar{\mu_t})}$
  - ${F_{x,j} = \begin{pmatrix} 1 & 0 & 0 & 0...0 & 0 & 0 & 0...0 \\ 0 & 1 & 0 & 0...0 & 0 & 0 & 0...0 \\ 0 & 0 & 1 & 0...0 & 0 & 0 & 0...0 \\ 0 & 0 & 0 & 0...0 & 1 & 0 & 0...0 \\ 0 & 0 & 0 & \underset{2j-2}{\underbrace{0...0}} & 0 & 1 & \underset{2N -2}{\underbrace{0...0}} \end{pmatrix}}$
  - ${H_t^i= \frac{1}{q}\begin{pmatrix}-\sqrt{q}\delta_x & -\sqrt{q}\delta_y & 0 & \sqrt{q}\delta_x & \sqrt{q}\delta_y \\ \delta_y & -\delta_x & -q & -\delta_y & \delta_x \end{pmatrix}F_{x,j}}$
  - ${K_t = \bar{\Sigma_t}H_t^T(H_t\bar{\Sigma_t}H_t^T+Q_t)^{-1}}$
  - ${\bar \mu_t = \bar{\mu_t}+K_t(z_t-h(\bar{\mu_t}))}$
  - ${\bar\Sigma_t=(I-K_tH_t)\bar{\Sigma_t}}$
- ${\mu_t = \bar{\mu_t}}$
- ${\Sigma_t=\bar\Sigma_t}$
执行时注意：
- 单个步骤中的测量更新只需要一次完整的置信度更新
- 始终归一化角度分量
- 你可能不需要创建 $F$ 矩阵

Loop Closures

Notes：
- 闭环意味着识别已有的地图。
- 数据关联：高度模糊，尽可能的环境相似。关注：闭环是否正确
- 闭环减少了机器人与landmark之间的不确定性，提高建图效率，但错误的闭环导致滤波器发散
相关性：
- 机器人的姿态与地标之间的相关性不容忽略(识别精度)
- 假设独立性会对不确定性的影响过于乐观估计
不确定性：
- 任何子矩阵都是地图协方差单调递减的决定因素
- 新的landmark以最大不确定初始化
限制：
- 在限制中，与任何单个地标位置估计相关联的协方差仅由车辆位置估计中的初始协方差确定。
EKF 的复杂度：
- 复杂度仅取决于测量的维度
- 每一步的代价：有地标的数量决定 $O(n^2)$
- 内存消耗 $O(n^2)$
- 对于大型地图，EKF变得难以计算