EKF-SLAM原理推导

0.引言

• 参考链接。基本是基于概率机器人进行实现的，是一个很好的学习材料。此博客只是个人学习记录。

• ref01.第五课 EKF SLAM

• ref02.EKF_SLAM实践。特别好的一篇文章。

$$AlgorithmExtendedKalmanfilter\left({\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t\right)$$$$\begin{array}{rl}& {\bar{\mu }}_t=g\left(u_t,{\mu }_{t-1}\right)\\ & {\bar{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+W_k{R}_t{W}_k^T\\ & K_t={\bar{\Sigma }}_t{H}_t^T{\left(H_t{\bar{\Sigma }}_t{H}_t^T+V_k{Q}_t{V}_k^T\right)}^{-1}\\ & {\mu }_t={\bar{\mu }}_t+K_t\left(z_t-h\left({\bar{\mu }}_t\right)\right)\\ & {\Sigma }_t=\left(I-K_t{H}_t\right){\bar{\Sigma }}_t\end{array}$$
PR书上的公式有点不完善，我加了一点。然后照着EKF公式推一下. 状态量为：$$\mathbf{X}={\left[\begin{array}{llllllllll}x& y& \theta & m_{x,1}& m_{y,1}& m_{x,2}& m_{y,2}& \cdots & m_{x,N}& m_{y,N}\end{array}\right]}^T$$
代码中状态量，初始化时只有机器人位姿，后续在增广添加marker：

 /* 初始时刻机器人位姿为0，绝对准确, 协方差为0 */
 mu_.resize(3, 1);
 mu_.setZero();
 sigma_.resize(3, 3);
 sigma_.setZero();

前3项是机器人位姿，后2N项是 N个路标点的位置。

1. 运动模型

$$\begin{array}{rl}g\left({\mathbf{u}}_t,{\mathbf{x}}_{t-1}\right)& \approx g\left({\mathbf{u}}_t,{\mathbf{\mu }}_{t-1}\right)+\underset{=:G_t}{g^{\prime }\left({\mathbf{u}}_t,{\mathbf{\mu }}_{t-1}\right)}\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{t-1}\right)\\ & =g\left({\mathbf{u}}_t,{\mathbf{\mu }}_{t-1}\right)+G_t\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{t-1}\right)\end{array}$$

运动模型需要确定的量： 运动模型$g(\ast )$ 、控制输入 $u_t$ 、$g(\ast )$ 对 状态量的 Jacobian 矩阵$G_t$ 、$g(\ast )$ 对过程噪声的 Jacobian 矩阵$W_k$、 噪声协方差矩阵 $R_t$.

1.1.里程计模型

$${\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]}_t={\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]}_{t-1}+\left[\begin{array}{c}\Delta s\cos ({\theta }_{t-1}+\Delta \theta /2)\\ \Delta s\sin ({\theta }_{t-1}+\Delta \theta /2)\\ \Delta \theta \end{array}\right]$$$$\{\begin{array}{l}\Delta \theta =\frac{\Delta s_r-\Delta s_l}{b}\\ \Delta s=\frac{\Delta s_r+\Delta s_l}{2}\end{array},\Delta s_{l/r}=k_{l/r}\cdot \Delta e_{l/r}\text{\hspace{1em}(1)}$$$$\Delta s_{l/r}\sim N\left(\Delta s_{l/r},{\Vert k\Delta s_{l/r}\Vert }^2\right)$$
原文公式(1)中最后一列$\theta$ 应该是${\theta }_{t-1}$ 。 $k_{l/r}$为左右轮系数，把编码器增量$\Delta e_{l/r}$转化为左右轮的位移， $b$ 是轮间距。公式（1）对应EKF公式（1）

 double tmp_th = mu_(2,0) + 0.5 * delta_theta;
 double cos_tmp_th = cos( tmp_th );
 double sin_tmp_th = sin(tmp_th);

 mu_(0, 0) += delta_s * cos_tmp_th;
 mu_(1, 0) += delta_s * sin_tmp_th;
 mu_(2, 0) += delta_theta;
 normAngle(mu_(2, 0)); //norm

左右轮位移的增量$\Delta s_{l/r}$服从高斯分布，均值就是编码器计算出的位移增量，标准差与增量大小成正比。如果t-1时刻机器人位姿的协方差为 ${\Sigma }_{\xi ,t-1}$，控制的协方差也就是左右轮位移增量的协方差为${\Sigma }_u$，那么机器人位姿在t时刻的协方差就是：

$${\bar{\mathbf{\Sigma }}}_{\xi ,t}={\mathbf{G}}_{\xi }{\mathbf{\Sigma }}_{\xi ,t-1}{\mathbf{G}}_{\xi }^T+{\mathbf{G}}_u^{\prime }{\mathbf{\Sigma }}_u{\mathbf{G}}^{\prime }{}_u^T\text{\hspace{1em}(2)}$$

原文对公式（2）称之为位姿在$t$时刻的协方差，感觉不是很对，应该是 $t$ 时刻 先验位姿误差的协方差？不是${\mathbf{\Sigma }}_{\xi ,t}$ 而是 ${\bar{\mathbf{\Sigma }}}_{\xi ,t}$.

$${\mathbf{G}}_{\xi }=\frac{\partial {\xi }_t}{\partial {\xi }_{t-1}}=\left[\begin{array}{ccc}1& 0& -\Delta s\sin ({\theta }_{t-1}+\Delta \theta /2)\\ 0& 1& \Delta s\cos ({\theta }_{t-1}+\Delta \theta /2)\\ 0& 0& 1\end{array}\right]\text{\hspace{1em}(3)}$$

/* 构造 G_xi */
Eigen::Matrix3d G_xi;
G_xi << 1.0, 0.0, -delta_s * sin_tmp_th,
        0.0, 1.0, delta_s * cos_tmp_th,
        0.0, 0.0, 1.0;

控制输入为$$\mathbf{u}={\left[\begin{array}{ll}\Delta s_r& \Delta s_l\end{array}\right]}^T$$
这里的求导有点耐人寻味，按理说应该是求$\frac{\partial {\xi }_t}{\partial {\mathbf{u}}_{\mathbf{t}-1}}$，但可以看出推导过程使用的是$\frac{\partial {\xi }_t}{\partial {\mathbf{u}}_{\mathbf{t}}}$:
$${\mathbf{G}}_u^{\prime }=\frac{\partial {\xi }_t}{\partial \mathbf{u}}=\left[\begin{array}{cc}\frac{1}{2}\cos \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)-\frac{\Delta s}{2b}\sin \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)& \frac{1}{2}\cos \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)+\frac{\Delta s}{2b}\sin \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)\\ \frac{1}{2}\sin \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)+\frac{\Delta s}{2b}\cos \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)& \frac{1}{2}\sin \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)-\frac{\Delta s}{2b}\cos \left({\theta }_{t-1}+\frac{\Delta \theta }{2}\right)\\ \frac{1}{b}& -\frac{1}{b}\end{array}\right]\text{\hspace{1em}(4)}$$

/* 构造 Gu' */
Eigen::Matrix<double, 3, 2> Gup;
Gup <<  0.5  * (cos_tmp_th - delta_s * sin_tmp_th / b_), 0.5  * (cos_tmp_th + delta_s * sin_tmp_th / b_),
        0.5  * (sin_tmp_th + delta_s * cos_tmp_th / b_), 0.5  *(sin_tmp_th - delta_s * cos_tmp_th / b_),
        1.0/b_                                         , -1.0/b_;

1.2.运动更新

可以看出1.1小节中只是对机器人位姿 $\xi$ 进行了求偏导， 而状态量 $\mathbf{X}$ 还包含了路标点。扩展路标点之后，运动方程为：

$$\underset{{\mathbf{X}}_t}{{\left[\begin{array}{c}x\\ y\\ \theta \\ m_{x,1}\\ m_{y,1}\\ ⋮\\ m_{x,N}\\ m_{y,N}\end{array}\right]}_t}=\underset{\mathbf{g}\left({\mathbf{X}}_{\mathbf{t}-1},{\mathbf{u}}_{\mathbf{t}}\right)}{\underset{{\mathbf{x}}_{l-1}}{{\left[\begin{array}{c}x\\ y\\ \theta \\ m_{x,1}\\ m_{y,1}\\ ⋮\\ m_{x,N}\\ m_{y,N}\end{array}\right]}_{t-1}}+\underset{\mathbf{F}}{\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\\ 0& 0& 0\\ 0& 0& 0\\ ⋮& ⋮& ⋮\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]}\left[\begin{array}{c}\Delta s\cos ({\theta }_{t-1}+\Delta \theta /2)\\ \Delta s\sin ({\theta }_{t-1}+\Delta \theta /2)\\ \Delta \theta \end{array}\right]}\text{\hspace{1em}(5)}$$

接下来是求解(不太明白这里为什么是 ${\mathbf{u}}_t$,而不是 ${\mathbf{u}}_{t-1}$)：

$${\overline{\mathbf{\Sigma }}}_t={\mathbf{G}}_t{\mathbf{\Sigma }}_{t-1}{\mathbf{G}}_t^T+{\mathbf{G}}_u{\mathbf{\Sigma }}_u{\mathbf{G}}_u^T\text{\hspace{1em}(6)}$$

$${\mathbf{G}}_t=\frac{\partial g({\mathbf{X}}_{t-1},{\mathbf{u}}_t)}{\partial {\mathbf{X}}_{t-1}}=\left[\begin{array}{cc}{\mathbf{G}}_{\xi }& 0\\ 0& \mathbf{I}\end{array}\right]\text{\hspace{1em}(7)}$$

$${\mathbf{G}}_u=\frac{\partial g({\mathbf{X}}_{t-1},{\mathbf{u}}_t)}{\partial {\mathbf{u}}_t}=\mathbf{F}{\mathbf{G}}_u^{\prime }\text{\hspace{1em}(8)}$$

$$\begin{array}{rl}{\overline{\mathbf{\Sigma }}}_t& =\left[\begin{array}{cc}{\mathbf{G}}_{\xi }& 0\\ 0& \mathbf{I}\end{array}\right]{\mathbf{\Sigma }}_{t-1}\left[\begin{array}{cc}{\mathbf{G}}_{\xi }^T& 0\\ 0& \mathbf{I}\end{array}\right]+\mathbf{F}{\mathbf{G}}^{\prime }{}_u{\mathbf{\Sigma }}_u{\mathbf{G}}^{\prime }{}_u^T{\mathbf{F}}^T\\ & =\left[\begin{array}{cc}{\mathbf{G}}_{\xi }{\mathbf{\Sigma }}_{xx}{\mathbf{G}}_{\xi }^T& {\mathbf{G}}_{\xi }{\mathbf{\Sigma }}_{xm}\\ {\left({\mathbf{G}}_{\xi }{\mathbf{\Sigma }}_{xm}\right)}^T& {\mathbf{\Sigma }}_{mm}\end{array}\right]+\mathbf{F}{\mathbf{G}}_u^{\prime }{\mathbf{\Sigma }}_u{\mathbf{G}}^{\prime }{}_u^T{\mathbf{F}}^T\end{array}\text{\hspace{1em}(9)}$$
可以看出，运动更新同时影响了机器人位姿的协方差，以及位姿与地图之间的协方差。

// 根据此时状态量的维数，构建F\Gt矩阵
int N = mu_.rows(); // N = 3
Eigen::MatrixXd F(N, 3); F.setZero();
F.block(0,0, 3, 3) = Eigen::Matrix3d::Identity();

Eigen::MatrixXd Gt = Eigen::MatrixXd::Identity(N, N);
Gt.block(0, 0, 3, 3) = G_xi;

在代码中，${\mathbf{\Sigma }}_u$ 的计算如下所示：

double k_; // 里程计协方差参数
sigma_u <<  k_ * k_ * delta_sr * delta_sr,  0.0, 
            0.0,   k_ * k_* delta_sl * delta_sl;

对于协方差，查了一下，网上相同的内容好多，不知道谁是原创，贴一个参考链接.从参考可知，变量 $X$ 按列排列时有：

$$\Sigma =\frac{1}{m}\sum _{i=1}^m\left(X^i\right)\cdot {\left(X^i\right)}^T$$里程计协方差参数选取蛮重要。

因此，最后的先验估计误差协方差矩阵，即公式（9）为：

/* 更新协方差, 代码中显然是 sigma_{t-1} , 原文公式表示有点不准确*/
sigma_ = Gt * sigma_ *Gt.transpose() + F * Gup * sigma_u * Gup.transpose() * F.transpose();

---

我计算出来的公式（9）是这样的：

$$\begin{array}{rl}{\overline{\mathbf{\Sigma }}}_t& =\left[\begin{array}{cc}{\mathbf{G}}_{\xi }& 0\\ 0& \mathbf{I}\end{array}\right]{\mathbf{\Sigma }}_{t-1}\left[\begin{array}{cc}{\mathbf{G}}_{\xi }^T& 0\\ 0& \mathbf{I}\end{array}\right]+\mathbf{F}{\mathbf{G}}^{\prime }{}_u{\mathbf{\Sigma }}_u{\mathbf{G}}^{\prime }{}_u^T{\mathbf{F}}^T\\ & =\left[\begin{array}{cc}{\mathbf{G}}_{\xi }{\mathbf{\Sigma }}_{t-1}{\mathbf{G}}_{\xi }^T& 0\\ 0& {\mathbf{\Sigma }}_{t-1}\end{array}\right]+\mathbf{F}{\mathbf{G}}_u^{\prime }{\mathbf{\Sigma }}_u{\mathbf{G}}^{\prime }{}_u^T{\mathbf{F}}^T\end{array}\text{\hspace{1em}(9)}$$
我不太明白，这里作者是怎么展开的，请求各位看官大佬帮我解答一下。公式（9）对应EKF公式（2）

哦，傻逼了，${\mathbf{\Sigma }}_{t-1}$ 要展开乘，但即便是展开乘，也应该是将${\mathbf{\Sigma }}_{t-1}$展开，而不是${\mathbf{\Sigma }}_t$

---

至此，EKF的前两个公式基本完成了。

2.测量模型

测量模型需要确定的量： 测量模型$h(\ast )$ 、$h(\ast )$ 对 ${\tilde{X}}_t$ 的 Jacobian 矩阵$H_t$ 、$h(\ast )$ 对测量噪声的 Jacobian 矩阵$V_k$、 测量噪声协方差矩阵 $Q_t$.

注意这里的${\tilde{X}}_t=f\left({\hat{X}}_{t-1},u_{t-1},0\right)$, ${\hat{X}}_{t-1}$ 为上一轮的后验估计。

首先解决测量值的问题。虽然可以获得ArUco码相对于机器人的6自由度位姿信息，但是为了与书上的观测统一，本文还是把相机作为Range-bearing传感器使用，也就是转换成距离 $r$ 和角度 $\varphi$ 。1个ArUco码作为一个路标点 $m$，坐标为 $[m_x{m}_y{]}^T$。

码与相机的相对位姿为 ${\mathbf{T}}_{cm}$，相机与机器人的相对位姿为 ${\mathbf{T}}_{rc}$，那么码相对于机器人的位姿为 ${\mathbf{T}}_{rm}={\mathbf{T}}_{rc}{\mathbf{T}}_{cm}$ 。${\mathbf{T}}_{rm}$ 的平移项 $x$ 和 $y$ 就是码的原点在机器人坐标系下的坐标。转化成距离信息就是 $r=\sqrt{x^2+y^2}$，角度就是 $\varphi =\mathrm{atan}2(y,x)$。这样就得到了测量值$z={\left[\begin{array}{ll}r& \varphi \end{array}\right]}^T$。这里的箭头方向和自己的表达习惯依然是反的，注意不要混淆。

对于测量噪声协方差矩阵$\mathbf{Q}$, 这里再做一个近似假设，认为观测的方差与距离和角度成线性关系：

$$\mathbf{Q}=\left[\begin{array}{cc}{\Vert k_{r}r\Vert }^2& \\ & {\Vert k_{\varphi }\varphi \Vert }^2\end{array}\right]\text{\hspace{1em}(10)}$$

这里的$k_r$、$k_{\varphi }$ 怎么计算得来的？从代码中得到答案：初始化给定的。

/* 计算观测方差 */
Eigen::Matrix2d Q;
// 对应公式（10），这里的参数依据什么来设定？  测量为 z= [r, phi]
Q << k_r_ * k_r_* fabs(ob.r_ * ob.r_), 0.0, 
     0.0, k_phi_ * k_phi_ * fabs(ob.phi_ * ob.phi_);       

第 $i$个路标点的观测模型为：

$${\mathbf{z}}_t^i=h\left({\mathbf{X}}_t\right)+{\delta }_t^i,{\delta }_t\sim \mathcal{N}\left(0,{\mathbf{Q}}_t\right)\text{\hspace{1em}(11)}$$

展开来看：
$$\{\begin{array}{l}{r}_t^i=\sqrt{{\left(m_{x,i}-x\right)}^2+{\left(m_{y,i}-y\right)}^2}\\ {\varphi }_t^i=\mathrm{atan}2\left(m_{y,i}-y,m_{x,i}-x\right)-\theta \end{array}\text{\hspace{1em}(12)}$$

这里的相机测量有点类似于将其看做是二维激光的数据，方便计算吧，公式也简单。测量值最后是在robot坐标系下表示的，不是在相机坐标系，这个坐标系的变换过程其实就是$\left(m_{x,i}-x\right)$ 、$\left(m_{y,i}-y\right)$、 $-\theta$ 的过程。

cv::Rodrigues(rvec, R);
Eigen::Matrix4d T_c_m;
// 旋转 + 平移：  marker to camera ,  也是 marker 在 camera 坐标系下的位姿
T_c_m <<    R.at<double>(0, 0), R.at<double>(0, 1), R.at<double>(0, 2), tvec[0],
            R.at<double>(1, 0), R.at<double>(1, 1), R.at<double>(1, 2), tvec[1],
            R.at<double>(2, 0), R.at<double>(2, 1), R.at<double>(2, 2), tvec[2],
            0.,0.,0.,1.;
// marker to robot ，也是 marker 在 robot 坐标系下的位姿
Eigen::Matrix4d T_r_m = T_r_c_ * T_c_m; // T_r_c_ 外参

// 转为观测模型的测量值
double& x = T_r_m(0, 3);
double& y = T_r_m(1, 3);

double r = sqrt(x*x + y*y);
double phi = atan2(y, x);
int aruco_id = IDs[i];

---

程序逻辑上，接下来这部分计算应该是在地图点添加之后进行，因为初始化是状态量只有机器人位姿。

根据扩展卡尔曼滤波，需要求解观测 ${\mathbf{z}}_t^i$ 相对于 ${\tilde{X}}_t$ （感觉原文表述不是很正确？）的雅克比 ${\mathbf{H}}_t^i$，实际上一个路标点观测只涉及到机器人的位姿和这个路标点的坐标，组合在一起就是五个量：$v=[xy\theta m_{x,i}{m}_{y,i}]$。于是，观测 ${\mathbf{z}}_t^i$ 相对于 $v$ 的雅克比是：

$${\mathbf{H}}_{\nu }=\frac{\partial h}{\partial \nu }=\frac{1}{q_i}\left[\begin{array}{ccccc}-\sqrt{q_i}{\delta }_{x_i}& -\sqrt{q_i}{\delta }_{y_i}& 0& \sqrt{q_i}{\delta }_{x_i}& \sqrt{q_i}{\delta }_{y_i}\\ {\delta }_{y_i}& -{\delta }_{x_i}& -q_i& -{\delta }_{y_i}& {\delta }_{x_i}\end{array}\right](\left\{\begin{array}{l}{\delta }_{x_i}=m_{x,i}-x\\ {\delta }_{y_i}=m_{y,i}-y\end{array}{q}_i={\delta }_{x_i}^2+{\delta }_{y_i}^2\right)\text{\hspace{1em}(13)}$$

最后提一个$1/q$ 出去就是。

原文公式14，写的2*(3+2N)维，应该是一次测量，最后扩展起来应该是2N*(3+2N)，这一点留着在看代码的时候求证吧。

// 公式14 F_i
int N = mu_.rows();
Eigen::MatrixXd F(5, N);
F.setZero();
F.block(0,0,3,3) = Eigen::Matrix3d::Identity();
F(3, 3 + 2*i) = 1;
F(4, 4 + 2*i) = 1;
// 公式13 中一些中间变量的计算
double& mx = mu_(3 + 2*i, 0);
double& my = mu_(4 + 2*i, 0);
double& x = mu_(0,0);
double& y = mu_(1,0);
double& theta = mu_(2,0);
double delta_x = mx - x;
double delta_y = my -y;
double q = delta_x * delta_x + delta_y * delta_y;
double sqrt_q = sqrt(q);
// 公式14 
Eigen::MatrixXd Hv(2, 5);
Hv << -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;

Hv = (1/q) * Hv;

Eigen::MatrixXd Ht = Hv * F;

${\mathbf{H}}_{\nu }$ 维数 25，${\mathbf{F}}_i$ 维数 5 (2N+3) ; ${\mathbf{H}}_t^i$维数则为 2*(2N+3)，对于N个数据观测则是2N*(2N+3)维，不知道这样理解是否正确。看代码求证。

$$\begin{array}{rl}& K_t^i={\bar{\Sigma }}_t{H}_t^{iT}{\left(H_t^i{\bar{\Sigma }}_t{H}_t^{iT}+Q_t\right)}^{-1}\\ & {\mu }_t={\bar{\mu }}_t+K_t^i\left(z_t-h\left({\bar{\mu }}_t\right)\right)\\ & {\Sigma }_t=\left(I-K_t^i{H}_t^i\right){\bar{\Sigma }}_t\end{array}\text{\hspace{1em}(15)}$$

// 公式15 EKF的后三个公式
// kalman 增益
Eigen::MatrixXd K = sigma_ * Ht.transpose()*( Ht * sigma_ * Ht.transpose() + Q ).inverse();

double phi_hat = atan2(delta_y, delta_x)- theta;
normAngle(phi_hat);
Eigen::Vector2d z_hat(
   sqrt_q, phi_hat
);
Eigen::Vector2d z(ob.r_, ob.phi_);
mu_ = mu_ + K * (z - z_hat);
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(N, N);
sigma_ = ( I - K * Ht) * sigma_;

$$h\left({\bar{\mu }}_t\right)={\hat{\mathbf{z}}}_t^i=\left[\begin{array}{l}\sqrt{{\left({\bar{m}}_{x,i}-\bar{x}\right)}^2+{\left({\bar{m}}_{y,i}-\bar{y}\right)}^2}\\ \mathrm{atan}2\left({\bar{m}}_{y,i}-\bar{y},{\bar{m}}_{x,i}-\bar{x}\right)-\bar{\theta }\end{array}\right]\text{\hspace{1em}(16)}$$

此图来自ref02.EKF_SLAM实践。至此，就完成了状态量的驱动与更新。

3.地图更新

上文所说的操作都是假设路标点的数量是已知的，这个值也可以认为是不知道的，可以边运行边加入路标点：当看到一个新的地图点时就扩展状态空间和协方差。当观测到一个新的路标点，其观测为$z={\left[\begin{array}{ll}r& \varphi \end{array}\right]}^T$，此时的机器人位姿为 $x_t=(x,y,\theta {)}^T$ 根据机器人的位姿可以计算地图点的坐标为：

$$map=\left[\begin{array}{l}{m}_x\\ m_y\end{array}\right]=\left[\begin{array}{cc}\cos (\theta )& -\sin (\theta )\\ \sin (\theta )& \cos (\theta )\end{array}\right]\left[\begin{array}{c}r\cos (\varphi )\\ r\sin (\varphi )\end{array}\right]+\left[\begin{array}{l}x\\ y\end{array}\right]=r\left[\begin{array}{c}\cos (\theta +\varphi )\\ \sin (\theta +\varphi )\end{array}\right]+\left[\begin{array}{l}x\\ y\end{array}\right]\text{\hspace{1em}(17)}$$

// 添加新路标
/* 均值 */
double angle = mu_(2,0) + ob.phi_; 
normAngle(angle);
double mx = ob.r_ * cos(angle) + mu_(0,0);
double my = ob.r_ * sin(angle) + mu_(1,0);

3.1.新地图点的协方差

这部分可以参考ref02.EKF_SLAM实践4.5小节-状态向量增广。

地图点的协方差为：

$${\mathbf{\Sigma }}_{mm}={\mathbf{G}}_p{\mathbf{\Sigma }}_{\xi }{\mathbf{G}}_p^T+{\mathbf{G}}_z\mathbf{Q}{\mathbf{G}}_z^T\text{\hspace{1em}(18)}$$
问：这里为什么要乘${\mathbf{\Sigma }}_{\xi }$？
$$G_p=\left[\begin{array}{ccc}1& 0& -r\sin (\theta +\varphi )\\ 0& 1& r\cos (\theta +\varphi )\end{array}\right]\text{\hspace{1em}(19)}$$$$G_z=\left[\begin{array}{cc}\cos (\theta +\varphi )& -r\sin (\theta +\varphi )\\ \sin (\theta +\varphi )& r\cos (\theta +\varphi )\end{array}\right]\text{\hspace{1em}(20)}$$

Eigen::Matrix3d sigma_xi = sigma_.block(0,0, 3, 3);

Eigen::Matrix<double, 2, 3> Gp;
Gp <<   1, 0, -ob.r_ * sin(angle),
        0, 1,  ob.r_ * cos(angle);

Eigen::Matrix2d Gz;
Gz <<   cos(angle), -ob.r_ * sin(angle),
        sin(angle),  ob.r_ * cos(angle);

// 新地图点的协方差
Eigen::Matrix2d sigma_m = Gp * sigma_xi * Gp.transpose() + Gz * Q * Gz.transpose();

3.2 新地图点与原状态之间的协方差

下面，还需要计算新加入的状态（地图点）与原状态（1个机器人位姿+N个地图点）之间的协方差。

$${\mathbf{\Sigma }}_{mx}={\mathbf{G}}_{fx}{\mathbf{\Sigma }}_t\text{\hspace{1em}(21)}$$

问：为什么要乘以${\mathbf{\Sigma }}_t$ ?

${\mathbf{\Sigma }}_t$ 为原状态的协方差矩阵，${\mathbf{\Sigma }}_{fx}$ 为(17)式相对于原状态的雅克比矩阵：

$${\mathbf{G}}_{fx}=\left[\begin{array}{cccccc}1& 0& -r\sin (\theta +\varphi )& 0& \cdots & 0\\ 0& 1& r\cos (\theta +\varphi )& 0& \cdots & 0\end{array}\right]\text{\hspace{1em}(22)}$$

// 新地图点相对于已有状态的协方差， Gfx的维数为当前状态量的维数
Eigen::MatrixXd Gfx;
Gfx.resize ( 2, mu_.rows() );
Gfx.setZero();
Gfx.block ( 0,0, 2, 3 ) = Gp;

此时就可以得出公式21：

Eigen::MatrixXd sigma_mx;
sigma_mx.resize ( 2, mu_.rows() );
sigma_mx.setZero();
sigma_mx = Gfx * sigma_;

最后按照如图矩阵所示进行增广：

/**** 加入到地图中:  矩阵增广 ****/
/* 扩展均值 = 状态量 */
int N = mu_.rows();
// 一次观测只有 一个地图点，mx, my 所以是 N(原来的)+2(新地图点)
Eigen::MatrixXd tmp_mu ( N + 2, 1 );
tmp_mu.setZero();
tmp_mu << mu_ , mx, my;
mu_.resize ( N+2, 1 );
mu_ = tmp_mu;

/* 扩展协方差 ： 按照图示矩阵进行拼接，同样的，一次加一个观测进去*/
Eigen::MatrixXd tmp_sigma ( N+2, N+2 );
tmp_sigma.setZero();
tmp_sigma.block ( 0, 0, N, N ) = sigma_;
tmp_sigma.block ( N, N, 2, 2 ) = sigma_m;
tmp_sigma.block ( N, 0, 2, N ) = sigma_mx;
tmp_sigma.block ( 0, N, N, 2 ) = sigma_mx.transpose();

sigma_.resize ( N+2, N+2 );
sigma_ = tmp_sigma;

这部分代码，可以引入内存管理，不然每次都copy有点费时间。至此，EKF算法中所有的模型都已建立完毕。

4.数据关联

从参考材料ref01\ref02可知，完整的流程为：

• 运动预测
• 测量预测
• 测量
• 数据关联
• 更新

还有很重要的一步，数据关联。这个系统中marker有唯一的ID，所以省去了很多工作，可以节约时间，只关注EKF部分的学习，感谢原博主的开源。

显示的过程，知乎上也有评论指出，成员变量一边读一边写，可能容易出问题； 另外，矩阵好像只有增广，没有margin，如果marker一直增加的话，可能就爆炸了。不过作为学习材料十分牛逼了，感谢原博主的开源。

5.demo

• aruco竟然是在opencv_contrib里面。
• aruco参考。
• 最近没咋用opencv_contrib，再装一遍吧，Ubuntu18.04安装OpenCV3.4.15+opencv_contrib

还在下载数据集…，demo待测试。

---

更。

这个aruco按照原博主的参数，在我这儿无法检测，也尝试了好几个版本，应该是参数有问题，调了好久，不好调，得需要知道dictionary参数，原博主给的dictionary参数在我这儿又无法检测。心碎，最后把OpenCV-contribute模块中的aruco扣出来，挨着debug，也没办法得到很好的检测效果。心伤了。

有调出来的朋友，留言告知一下参数或者是其他什么错误，谢谢了。