EKF-SLAM: Simulataneous localization and mapping with the extended Kalman filterr.

• EKF-SLAM
  • Setting up an EKF for SLAM
    • Model
      • 1. state vector
      • 2. covariance matrix
  • Process
    • 1. Initialization
    • 2. Robot motion (Predict)
      • 2.0 Motion func
      • 2.1 Simplify
    • 3. Observation of mapped landmarks (Update)
      • 3.0 Observation function
      • 3.1 old landmark
      • 3.2 new landmark

---

Created 2021.05.18 by William Yu; Last modified: 2022.08.08-V1.2.2

Contact: windmillyucong@163.com

Copyleft! 2022 William Yu. Some rights reserved.

---

EKF-SLAM

Simulataneous localization and mapping with the extended Kalman filterr（拓展卡尔曼滤波SLAM）

refitem:

• paper: https://www.iri.upc.edu/people/jsola/JoanSola/objectes/curs_SLAM/SLAM2D/SLAM%20course.pdf
• blog: https://zhuanlan.zhihu.com/p/399365812
• courseware:
  • http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/
  • http://ais.informatik.uni-freiburg.de/teaching/ws12/mapping/pdf/slam04-ekf-slam.pdf
  • http://ais.informatik.uni-freiburg.de/teaching/ws17/mapping/pdf/slam09-kf-wrapup.pdf

Setting up an EKF for SLAM

Model

${x, P}$: a Gaussian variable using the mean and the covariances matrix of the state vector, denoted respectively by x and P:

1. state vector

\[x = [R \ \ M] ^T= [R \ L_1 \ ... \ L_n]^T \tag1\]

• where:
  • R: robot state $R=[x \ y \ \theta]^T$
  • M: map, the set of landmark states $M=[L_1 \ … \ L_n]^T$ 2n个数据

2. covariance matrix

\[P = \left[ \begin{matrix} P_{RR} & P_{RM} \\ P_{MR} & P_{MM} \end{matrix} \right] = \left[ \begin{matrix} P_{RR} & P_{RL_1} & ... & R_{R L_n} \\ P_{L_1R} & P_{L_1L_1} & ... & R_{L_1 L_n} \\ ... & ... & ... & ... \\ P_{L_nR} & P_{L_nL_1} & ... & p_{L_n L_n} \end{matrix} \right] \tag2\]

Process

1. Initialization

0! set everything to 0!

2. Robot motion (Predict)

2.0 Motion func

\[x^-_k = f(x_{k-1},u_k,n)\tag 3\] \[P^-_k = F_xP_{k-1}F_x^T + F_nNF_n^T \tag4\]

• where:

  • f 是非线性函数
  • F_x F_n 为线性化后的结果
  \[F_x = \frac{\partial f }{\partial x} , \ \ \ F_n = \frac{\partial f }{\partial n}\]

2.1 Simplify

• the motion model changes robot R, but not map M.
• so that we only need to predict the R releated part in $x$ as well as P.
• F_x 矩阵虽然大，但是异常空旷
• F_xx 是充实的，F_xm以及F_mx是空旷的，F_mm是0
• 而且F_xx维度很小
• 所以简化3 4 两表达式，可以只考虑其中的R相关的部分：

\[R^-_k = f_R(R_{k-1}, u_k, n) \tag5\] \[P^-_{k_{RR}} = \\ P^-_{k_{RM}} = \\ P^-_{k_{MR}} = \tag6\]

//todo(congyu)

• where:
  • R: robot
  • M: map

3. Observation of mapped landmarks (Update)

3.0 Observation function

\[y_k = h(x) + v \tag7\]

线性化得$H_x = \frac {\partial h} {\partial x}$.

3.1 old landmark

对于旧的路标点

step 1. 计算卡尔曼增益 \(K_k = P_k^- H_k^T (H_kP^-_kH_k^T + R )^{-1} \tag 8\)

• where
  • R: 观测噪声的协方差矩阵
  • H: 即$H_x$

step 2. 计算 error

1. 投影模型，对某一个路标点，预测在当前机器状态下，应该得到的观测值 e.g.{角度，距离}（机器坐标系下）$h(x_k^-)$

2. 然后进行实际观测，得到当前实际路标点的观测结果{角度，距离}（机器坐标系下）

3. 做差得error \(z_k^- = y_k - h(x^-_k) \tag9\)

step 3. 更新结果 \(x_k := x_k^- + Kz_k^- \\ P_k := (I-K_kH_k)P_k^- \tag {10}\)

大致流程总结：表达式5,6 -> 8 -> 9 -> 10

3.2 new landmark

对于新增的路标点

//todo(congyu)

---

• Previous
  Error-State Kalman Filter
• Next
  MSCKF

---