First Estimate Jacobian (FEJ)经典论文中的EKF SLAM的error-state equation的推导过程

First Estimate Jacobian (FEJ)经典论文中的EKF SLAM的error-state equation的推导过程

论文A First Estimate Jacobian EKF for Improving SLAM Consistency中首次提出了First Estimate Jacobian (FEJ)，这篇论文是FEJ的经典论文了，值得深入研读。本文先贴出这篇论文中EKF SLAM的Error-State Equation的推导过程。感兴趣的同学，如果有需要讨论的地方，可以email联系754971421@qq.com

在标准化的SLAM公式中，机器人的状态向量包括在全局参考坐标系下的机器人位姿和路标的位置。因此在时刻k$k$的状态向量可以表示为（在这篇论文中，为了表示的简洁，状态向量中只包含了一个路标）：

xTk=[pTRkϕRkpTL]=[xTRkpTL](1)$$\begin{array}{}\text{(1)}& {\mathbf{x}}_k^T=\left[\begin{array}{ccc}{\mathbf{p}}_{R_k}^T& {\varphi }_{R_k}& {\mathbf{p}}_L^T\end{array}\right]=\left[\begin{array}{cc}{\mathbf{x}}_{R_k}^T& {\mathbf{p}}_L^T\end{array}\right]\end{array}$$

其中xTRk=[pTRkϕRk]${\mathbf{x}}_{R_k}^T=\left[\begin{array}{cc}{\mathbf{p}}_{R_k}^T& {\varphi }_{R_k}\end{array}\right]$表示机器人的位姿，pL${\mathbf{p}}_L$表示路标的位置。

EKF-SLAM包含两个步骤：propagation和update。

EKF Propagation

EKF propagation公式如下：

p̂ Rk+1|kϕ̂ Rk+1|kp̂ Lk+1|k===p̂ Rk|k+C(ϕ̂ Rk|k)Rkp̂ Rk+1ϕ̂ Rk|k+Rkϕ̂ Rk+1p̂ Lk|k(2)(3)(4)$$\begin{array}{}\text{(2)}& {\hat{\mathbf{p}}}_{R_{k+1|k}}& =& {\hat{\mathbf{p}}}_{R_{k|k}}+\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}\text{(3)}& {\hat{\varphi }}_{R_{k+1|k}}& =& {\hat{\varphi }}_{R_{k|k}}+{}^{R_k}{\hat{\varphi }}_{R_{k+1}}\text{(4)}& {\hat{\mathbf{p}}}_{L_{k+1|k}}& =& {\hat{\mathbf{p}}}_{L_{k|k}}\end{array}$$

其中C(⋅)$\mathbf{C}(\cdot )$表示2×2$2\times 2$的旋转矩阵，而Rkx̂ Rk+1=[Rkp̂ TRk+1Rkϕ̂ Rk+1]T${}^{R_k}{\hat{\mathbf{x}}}_{R_{k+1}}={\left[\begin{array}{cc}{}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}^T& {}^{R_k}{\hat{\varphi }}_{R_{k+1}}\end{array}\right]}^T$是从基于里程计估计的从时刻k$k$到时刻k+1$k+1$机器人的运动。毫无疑问这个估计是含有均值为0的高斯白噪声wk=RkxRk+1−Rkx̂ Rk+1${\mathbf{w}}_k={}^{R_k}{\mathbf{x}}_{R_{k+1}}-{}^{R_k}{\hat{\mathbf{x}}}_{R_{k+1}}$，其协方差矩阵为Qk${\mathbf{Q}}_k$。

除了上述的状态递推公式外，EKF还需要线性化的error-state递推公式，该公式由公式(5)给出

x˜k+1|k=[ΦRk02×303×2I2][x˜Rk|kx˜Lk|k]+[GRk02×2]wk=Φkx˜k|k+Gkwk(5)$$\begin{array}{}\text{(5)}& {\tilde{\mathbf{x}}}_{k+1|k}=\left[\begin{array}{cc}{\mathbf{\Phi }}_{R_k}& {\mathbf{0}}_{3\times 2}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{I}}_2\end{array}\right]\left[\begin{array}{c}{\tilde{\mathbf{x}}}_{R_{k|k}}\\ {\tilde{\mathbf{x}}}_{L_{k|k}}\end{array}\right]+\left[\begin{array}{c}{\mathbf{G}}_{R_k}\\ {\mathbf{0}}_{2\times 2}\end{array}\right]{\mathbf{w}}_k={\mathbf{\Phi }}_k{\tilde{\mathbf{x}}}_{k|k}+{\mathbf{G}}_k{\mathbf{w}}_k\end{array}$$

其中ΦRk${\mathbf{\Phi }}_{R_k}$和GRk${\mathbf{G}}_{R_k}$可以从状态递推公式得到

ΦRkGRk==[I201×2JC(ϕ̂ Rk|k)Rkp̂ Rk+11]=[I201×2J(p̂ Rk+1|K−p̂ Rk|k)1][C(ϕ̂ Rk|k)01×202×11](6)(7)$$\begin{array}{}\text{(6)}& {\mathbf{\Phi }}_{R_k}& =& \left[\begin{array}{cc}{\mathbf{I}}_2& \mathbf{J}\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}\\ {\mathbf{0}}_{1\times 2}& 1\end{array}\right]=\left[\begin{array}{cc}{\mathbf{I}}_2& \mathbf{J}\left({\hat{\mathbf{p}}}_{R_{k+1|K}}-{\hat{\mathbf{p}}}_{R_{k|k}}\right)\\ {\mathbf{0}}_{1\times 2}& 1\end{array}\right]\text{(7)}& {\mathbf{G}}_{R_k}& =& \left[\begin{array}{cc}\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)& {\mathbf{0}}_{2\times 1}\\ {\mathbf{0}}_{1\times 2}& 1\end{array}\right]\end{array}$$

其中J=[01−10]$\mathbf{J}=\left[\begin{array}{cc}0& -1\\ 1& 0\end{array}\right]$。

state和error state
- x̂ $\hat{\mathbf{x}}$是状态向量x$\mathbf{x}$的估计;
- x˜=x−x̂ $\tilde{\mathbf{x}}=\mathbf{x}-\hat{\mathbf{x}}$是error state；

对于不包含噪声的机器人状态向量也满足

pRk+1|kϕRk+1|k==pRk|k+C(ϕRk|k)RkpRk+1ϕRk|k+RkϕRk+1(8)(9)$$\begin{array}{}\text{(8)}& {\mathbf{p}}_{R_{k+1|k}}& =& {\mathbf{p}}_{R_{k|k}}+\mathbf{C}\left({\varphi }_{R_{k|k}}\right){}^{R_k}{\mathbf{p}}_{R_{k+1}}\text{(9)}& {\varphi }_{R_{k+1|k}}& =& {\varphi }_{R_{k|k}}+{}^{R_k}{\varphi }_{R_{k+1}}\end{array}$$

公式(8)和(2)的两边相减，可以得到

p˜Rk+1|k=p˜k|k+C(ϕRk|k)RkpRk+1−C(ϕ̂ Rk|k)Rkp̂ Rk+1$${\tilde{\mathbf{p}}}_{R_{k+1|k}}={\tilde{\mathbf{p}}}_{k|k}+\mathbf{C}\left({\varphi }_{R_{k|k}}\right){}^{R_k}{\mathbf{p}}_{R_{k+1}}-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}$$

p˜Rk+1|k=p˜k|k+(C(ϕRk|k)−C(ϕ̂ Rk|k))RkpRk+1+C(ϕ̂ Rk|k)Rkp˜Rk+1$${\tilde{\mathbf{p}}}_{R_{k+1|k}}={\tilde{\mathbf{p}}}_{k|k}+\left(\mathbf{C}\left({\varphi }_{R_{k|k}}\right)-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)\right){}^{R_k}{\mathbf{p}}_{R_{k+1}}+\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\tilde{\mathbf{p}}}_{R_{k+1}}$$

p˜Rk+1|k≈p˜k|k+(C(ϕRk|k)−C(ϕ̂ Rk|k))Rkp̂ Rk+1+C(ϕ̂ Rk|k)Rkp˜Rk+1(10)$$\begin{array}{}\text{(10)}& {\tilde{\mathbf{p}}}_{R_{k+1|k}}\approx {\tilde{\mathbf{p}}}_{k|k}+\left(\mathbf{C}\left({\varphi }_{R_{k|k}}\right)-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}+\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\tilde{\mathbf{p}}}_{R_{k+1}}\end{array}$$

显然ϕRk|k=ϕ̂ Rk|k+ϕ˜Rk|k${\varphi }_{R_{k|k}}={\hat{\varphi }}_{R_{k|k}}+{\tilde{\varphi }}_{R_{k|k}}$，所以

C(ϕRk|k)−C(ϕ̂ Rk|k)=C(ϕ̂ Rk|k+ϕ˜Rk|k)−C(ϕ̂ Rk|k)(11)$$\begin{array}{}\text{(11)}& \mathbf{C}\left({\varphi }_{R_{k|k}}\right)-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)=\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}+{\tilde{\varphi }}_{R_{k|k}}\right)-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)\end{array}$$

对于这种2D旋转，我们可以将旋转矩阵C$\mathbf{C}$写成：

C(ϕ)=[cos(ϕ)sin(ϕ)−sin(ϕ)cos(ϕ)](12)$$\begin{array}{}\text{(12)}& \mathbf{C}\left(\varphi \right)=\left[\begin{array}{cc}\cos \left(\varphi \right)& -\sin \left(\varphi \right)\\ \sin \left(\varphi \right)& \cos \left(\varphi \right)\end{array}\right]\end{array}$$

易得，其导数为

DϕC=J⋅C(13)$$\begin{array}{}\text{(13)}& D_{\varphi }\mathbf{C}=\mathbf{J}\cdot \mathbf{C}\end{array}$$

可以得到如下的一阶近似：

C(ϕ̂ Rk|k+ϕ˜Rk|k)−C(ϕ̂ Rk|k)=JC(ϕ̂ Rk|k)ϕ˜Rk|k(14)$$\begin{array}{}\text{(14)}& \mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}+{\tilde{\varphi }}_{R_{k|k}}\right)-\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right)=\mathbf{J}\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){\tilde{\varphi }}_{R_{k|k}}\end{array}$$

所以，可以得到公式(10)为：

p˜Rk+1|k=p˜Rk|k+JC(ϕ̂ Rk|k)Rkp̂ Rk+1ϕ˜Rk|k+C(ϕ̂ Rk|k)Rkp˜Rk+1(15)$$\begin{array}{}\text{(15)}& {\tilde{\mathbf{p}}}_{R_{k+1|k}}={\tilde{\mathbf{p}}}_{R_{k|k}}+\mathbf{J}\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}{\tilde{\varphi }}_{R_{k|k}}+\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\tilde{p}}_{R_{k+1}}\end{array}$$

由公式(2)知C(ϕ̂ Rk|k)Rkp̂ Rk+1=p̂ Rk+1|k−p̂ Rk|k$\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\hat{\mathbf{p}}}_{R_{k+1}}={\hat{\mathbf{p}}}_{R_{k+1|k}}-{\hat{\mathbf{p}}}_{R_{k|k}}$，所以

p˜Rk+1|k=p˜Rk|k+J(p̂ Rk+1|k−p̂ Rk|k)ϕ˜Rk|k+C(ϕ̂ Rk|k)Rkp˜Rk+1(16)$$\begin{array}{}\text{(16)}& {\tilde{\mathbf{p}}}_{R_{k+1|k}}={\tilde{\mathbf{p}}}_{R_{k|k}}+\mathbf{J}\left({\hat{\mathbf{p}}}_{R_{k+1|k}}-{\hat{\mathbf{p}}}_{R_{k|k}}\right){\tilde{\varphi }}_{R_{k|k}}+\mathbf{C}\left({\hat{\varphi }}_{R_{k|k}}\right){}^{R_k}{\tilde{p}}_{R_{k+1}}\end{array}$$

EKF Update

EKF SLAM的测量模型是关于路标相对机器人的位置的函数：

zk=h(RkpL)+vk(17)$$\begin{array}{}\text{(17)}& {\mathbf{z}}_k=\mathbf{h}\left({}^{R_k}{\mathbf{p}}_L\right)+{\mathbf{v}}_k\end{array}$$

其中RkpL=CT(ϕRk)(pL−pRk)${}^{R_k}{\mathbf{p}}_L={\mathbf{C}}^T\left({\varphi }_{R_k}\right)\left({\mathbf{p}}_L-{\mathbf{p}}_{R_k}\right)$是时刻k$k$路标相对于机器人的位置，vk${\mathbf{v}}_k$是均值为0，协方差为Rk${\mathbf{R}}_k$的高斯测量噪声。

为了不失一般性，我们假定h$\mathbf{h}$是非线性函数。线性化的measurement-error equation为：

z˜k≈[HRkHLk][x˜Rk|k−1x˜Lk|k−1]+vk=Hkx˜k|k−1+vk(18)$$\begin{array}{}\text{(18)}& {\tilde{\mathbf{z}}}_k\approx \left[\begin{array}{cc}{\mathbf{H}}_{R_k}& {\mathbf{H}}_{L_k}\end{array}\right]\left[\begin{array}{c}{\tilde{\mathbf{x}}}_{R_{k|k-1}}\\ {\tilde{\mathbf{x}}}_{L_{k|k-1}}\end{array}\right]+{\mathbf{v}}_k={\mathbf{H}}_k{\tilde{\mathbf{x}}}_{k|k-1}+{\mathbf{v}}_k\end{array}$$

其中HRk${\mathbf{H}}_{R_k}$和HLk${\mathbf{H}}_{L_k}$分别是h$\mathbf{h}$对机器人位姿和路标的Jacobian矩阵。

注意：和EKF Propagation不同, 是因为h$\mathbf{h}$是非线性的，我们要在x̂ k|k−1${\hat{\mathbf{x}}}_{k|k-1}$处线性化，并且采用链式法则。

首先计算HLk${\mathbf{H}}_{L_k}$，HLk=Dh⋅DpLRkpL=(∇h)⋅DpLRkpL${\mathbf{H}}_{L_k}=D\mathbf{h}\cdot D_{{\mathbf{p}}_L}{}^{R_k}{\mathbf{p}}_L=\left(\mathrm{\nabla }\mathbf{h}\right)\cdot D_{{\mathbf{p}}_L}{}^{R_k}{\mathbf{p}}_L$， 而

DpLRkpL(x̂ k|k−1)=(∇h)CT(ϕ̂ k|k−1)(19)$$\begin{array}{}\text{(19)}& D_{{\mathbf{p}}_L}{}^{R_k}{\mathbf{p}}_L\left({\hat{\mathbf{x}}}_{k|k-1}\right)=\left(\mathrm{\nabla }\mathbf{h}\right){\mathbf{C}}^T\left({\hat{\varphi }}_{k|k-1}\right)\end{array}$$

接下来计算HRk${\mathbf{H}}_{R_k}$，可以把HRk${\mathbf{H}}_{R_k}$写成分块矩阵的形式HRk=Dh⋅DxRkRkpL=(∇h)[G1G2]${\mathbf{H}}_{R_k}=D\mathbf{h}\cdot D_{{\mathbf{x}}_{R_k}}{}^{R_k}{\mathbf{p}}_L=\left(\mathrm{\nabla }\mathbf{h}\right)\left[\begin{array}{cc}{\mathbf{G}}_1& {\mathbf{G}}_2\end{array}\right]$，其中

G1G2==DpRkRkpL=−CT(ϕ̂ Rk|k−1)DϕRkRkpL=(DϕRkCT(ϕ̂ Rk|k−1))⋅(p̂ Lk|k−1−p̂ Rk|k−1)(20)(21)$$\begin{array}{}\text{(20)}& {\mathbf{G}}_1& =& D_{{\mathbf{p}}_{R_k}}{}^{R_k}{\mathbf{p}}_L=-{\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)\text{(21)}& {\mathbf{G}}_2& =& D_{{\varphi }_{R_k}}{}^{R_k}{\mathbf{p}}_L=\left(D_{{\varphi }_{R_k}}{\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)\right)\cdot \left({\hat{\mathbf{p}}}_{L_{k|k-1}}-{\hat{\mathbf{p}}}_{R_{k|k-1}}\right)\end{array}$$

又因为

DϕRkCT(ϕ̂ Rk|k−1)=(JCT(ϕ̂ Rk|k−1))T=CT(ϕ̂ Rk|k−1)JT=CT(ϕ̂ Rk|k−1)(−J)(22)$$\begin{array}{}\text{(22)}& D_{{\varphi }_{R_k}}{\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)={\left(\mathbf{J}{\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)\right)}^T={\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right){\mathbf{J}}^T={\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)\left(-\mathbf{J}\right)\end{array}$$

所以，

G2=DϕRkRkpL=CT(ϕ̂ Rk|k−1)(−J)(p̂ Lk|k−1−p̂ Rk|k−1)(23)$$\begin{array}{}\text{(23)}& {\mathbf{G}}_2=D_{{\varphi }_{R_k}}{}^{R_k}{\mathbf{p}}_L={\mathbf{C}}^T\left({\hat{\varphi }}_{R_{k|k-1}}\right)\left(-\mathbf{J}\right)\left({\hat{\mathbf{p}}}_{L_{k|k-1}}-{\hat{\mathbf{p}}}_{R_{k|k-1}}\right)\end{array}$$