基于GPS、IMU、轮速的ESKF

1 背景
在使用传感器采集到一些数据后，需要计算车辆位姿。在融合GPS位置、IMU角速度、轮速后，可以通过ESKF进行求解。ESKF本质还是一个EKF过程，只是这里面更新量换成了值较小的误差状态量。由于是EKF过程，只需要导出下面公式(10)里的\(F、F_i、H\)矩阵，就可以使用EKF过程进行递推。

2 原理
设真值状态变量 \(\mathbf{x}=[\mathbf{p},\theta,\mathbf{b}]\),其分别表示位置、朝向、车辆坐标系下角速度偏置，误差状态量为 \(\delta\mathbf{x}=[\delta\mathbf{p},\delta\theta,\delta\mathbf{b}]\)，\(t\)时刻含有噪声状态量为 \(\mathbf{x}_t=[\mathbf{p}_t,\theta_t,\mathbf{b}_t]\)。状态变量之间使用下式进行关联  
(1)
$$
\begin{cases}
\mathbf{p}_t &=\mathbf{p}+\delta\mathbf{p}
\\
\theta_t&=\theta+\delta\theta&,R_t=\exp(\theta_t^\wedge) \approx \exp(\theta^\wedge)\exp(\delta\theta^\wedge)=R\exp(\delta\theta^\wedge)
\\
\mathbf{b}_t&=\mathbf{b}+\delta\mathbf{b}
\end{cases}
$$
以常见的四轮汽车模型为例，现在已知车辆坐标系下的后左右轮速\(v_l,v_r\)以及角速度\(\bar{\mathbf{w}}\),这里以后左右轮中心作为车辆坐标系原点，设\(\bar{\mathbf{v}}=[\dfrac{v_x+v_y}{2},0,0]\)，速度与角速度噪声为\(\eta_v,\eta_w\)，接下来需要找到满足如下方程的\(F,F_i\)  
(2)
$$
\delta\mathbf{x}=F\delta\mathbf{x}+F_i\begin{bmatrix}
\eta_v
\\
\eta_w
\end{bmatrix}
$$
根据物理模型有  
(3)
$$
\begin{aligned}
&\begin{cases}
\dot{\mathbf{p}}_t &=R_t(\bar{\mathbf{v}}+\eta_v)
\\
\dot{R}_t&=R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge
\\
\dot{\mathbf{b}}_t&=\eta_w
\end{cases}
\\
&\begin{cases}
\dot{\mathbf{p}} &=R\bar{\mathbf{v}}
\\
\dot{R}&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
\dot{\mathbf{b}}&=\mathbf{0}
\end{cases}
\end{aligned}
$$
对于位置更新，结合(1)(3)可得  
(4)
$$
\begin{aligned}
\delta \dot{\mathbf{p}} &=\dot{\mathbf{p}}_t-\dot{\mathbf{p}}
\\
&=(R_t-R)\bar{\mathbf{v}}+R_t\eta_v
\\
&\approx (R\exp(\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R\exp(\delta \theta^\wedge)\eta_v
\\
&\approx (R(I+\delta \theta^\wedge)-R)\bar{\mathbf{v}}+R(I+\delta \theta^\wedge)\eta_v
\\
&=R\delta\theta^\wedge\bar{\mathbf{v}}+R\delta\theta^\wedge\eta_v+R\eta_v
\\
&=-R(\bar{\mathbf{v}}+\eta_v)^\wedge\delta\theta+R\eta_v
\\
&\approx -R\bar{\mathbf{v}}^\wedge\delta\theta+R\eta_v
\end{aligned}
$$
对于旋转部分，结合(1)(3)可得  
(5)
$$
\begin{aligned}
\dot{R}_t&=\dot{R}\exp(\delta\theta^\wedge)+R\dot{\exp(\delta \theta^\wedge)}
\\&=\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)(\mathbf{J}_r(\delta\theta)\delta\dot{\theta})^\wedge
\\&\Downarrow
\\
\dot{R}_t&\approx\dot{R}\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R_t(\bar{\mathbf{w}}-\mathbf{b}_t-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
R\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=R(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+R\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\exp(\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge&=(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)+\exp(\delta\theta^\wedge)\delta\dot{\theta}^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}^\wedge&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-\exp(-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge\exp(\delta\theta^\wedge)
\\
&\approx(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(I-\delta\theta^\wedge)(\bar{\mathbf{w}}-\mathbf{b})^\wedge(I+\delta\theta^\wedge)
\\
&=(\bar{\mathbf{w}}-\mathbf{b}-\delta\mathbf{b}-\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})
\\
&\approx -(\delta\mathbf{b}+\eta_w)^\wedge-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta^\wedge+\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b})^\wedge
\\
&= -(\delta\mathbf{b}+\eta_w)^\wedge+(\delta\theta^\wedge(\bar{\mathbf{w}}-\mathbf{b}))^\wedge
\\
&=-(\delta\mathbf{b}+\eta_w)^\wedge-((\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta)^\wedge
\\&\Downarrow
\\
\delta\dot{\theta}&=-(\delta\mathbf{b}+\eta_w)-(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta
\end{aligned}
$$
对于角速度偏置部分，结合(1)(3)可以得到  
(6)
$$
\delta\dot{\mathbf{b}}=\dot{\mathbf{b}}_t-\dot{\mathbf{b}} =\eta_w
$$
因此在一个小时间\(\triangle t\)内，综合(3)(4)(5)可以得到  
(7)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}_{t+\triangle t}&=\delta \mathbf{p}_t+\triangle t\delta \dot{\mathbf{p}}=\delta \mathbf{p}_t-\triangle tR\bar{\mathbf{v}}^\wedge \delta \theta_t+\triangle tR\eta_v
\\
\delta \theta_{t+\triangle t}&=\delta \theta_t+\triangle t\delta\dot{\theta}=\delta \theta_t-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge\delta\theta_t-\triangle t(\delta\mathbf{b}_t+\eta_w)
\\
\delta\mathbf{b}_{t+\triangle t}&=\delta\mathbf{b}_t+\triangle t\delta\dot{\mathbf{b}}=\delta\mathbf{b}_t+\triangle t\eta_w
\end{cases}
\\
&\Downarrow
\\
&F=\begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & I-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\approx \begin{bmatrix}
I &-\triangle tR\bar{\mathbf{v}}^\wedge &\mathbf{0}
\\
\mathbf{0} & \exp(-\triangle t(\bar{\mathbf{w}}-\mathbf{b})^\wedge) &-\triangle tI
\\
\mathbf{0}&\mathbf{0} & I
\end{bmatrix}
\\
&F_i=\triangle t\begin{bmatrix}
R &\mathbf{0}
\\
\mathbf{0} &-I
\\
\mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$
接下来，以GPS位置 \(\bar{\mathbf{p}}\) 作为观测，因此建立观测函数  
(8)
$$
\begin{aligned}
\bar{\mathbf{p}} &=z(\mathbf{x},\delta \mathbf{x})
\\
&=\mathbf{p}_t+\triangle tR_t\bar{\mathbf{v}}
\\
&=\mathbf{p}+\delta\mathbf{p}+\triangle tR\exp(\delta\theta^\wedge)\bar{\mathbf{v}}
\\
&\Downarrow
\\
&\begin{cases}
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{p}}&=I
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \theta}&=\triangle tR\dfrac{\partial(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})}{\partial\delta \theta}\approx-\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge
\\
\\
\dfrac{\partial\bar{\mathbf{p}}}{\partial\delta \mathbf{b}}&=\mathbf{0}
\end{cases}
\\
&\Downarrow
\\
H&=\begin{bmatrix}
I & -\triangle tR(\exp(\delta\theta^\wedge)\bar{\mathbf{v}})^\wedge &\mathbf{0}
\end{bmatrix}\approx
\begin{bmatrix}
I & -\triangle tR\bar{v}^\wedge&\mathbf{0}
\end{bmatrix}
\end{aligned}
$$
注意，\(H\)计算中，忽略了二阶小量，这里\(\triangle t、\delta\theta\)均为小量。

EKF更新完成后，需要对协方差矩阵\(P\)进行重置。设 \(\delta \mathbf{x}'=[\delta \mathbf{p}’,\delta \theta’,\delta\mathbf{b}’]\)为\(\delta \mathbf{x}\)受到均值 \(\delta \bar{\mathbf{x}}=[\delta \bar{\mathbf{p}},\delta \bar{\theta},\delta\bar{\mathbf{b}}]\)的影响,因此按照逆重构形式(减法)找到一个满足\(\delta \mathbf{x}'=G\delta\mathbf{x}+…\)形式的重置矩阵\(G\):  
(9)
$$
\begin{aligned}
&\begin{cases}
\delta \mathbf{p}'&=\delta\mathbf{p}-\delta\bar{\mathbf{p}}
\\
\\
\delta \theta'&=\log(\exp(\delta\theta'^\wedge))^\vee
\\
&\approx \log(\exp(\delta\theta^\wedge)\exp(-\delta\bar{\theta})^\wedge)^\vee
\\
&\approx (I-\dfrac{1}{2}\delta\bar{\theta}^\wedge)\delta\theta-\delta\bar{\theta}
\\
\\
\delta\mathbf{b}'&=\delta\mathbf{b}-\delta\bar{\mathbf{b}}
\end{cases}
\\
&\Downarrow
\\
&G=\begin{bmatrix}
I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &I-\dfrac{1}{2}\delta\bar{\theta}^\wedge&\mathbf{0}
\\
\mathbf{0} & \mathbf{0} & I
\end{bmatrix}
\end{aligned}
$$

3 算法过程
因此，最终的ESKF过程为  
(10)
$$
\begin{aligned}
\text{预测}:&\begin{cases}
\delta \mathbf{x}&=F\delta\mathbf{x}
\\
P&=FPF^T+F_iQF_i^T
\end{cases}
\\
\text{增益}:&\begin{cases}
K&=PH^T(HPH^T+V)^{-1}
\end{cases}
\\
\text{更新}:&\begin{cases}
\delta \mathbf{x}&=K(\bar{\mathbf{p}}-z(\mathbf{x},\delta\mathbf{x}))
\\
\mathbf{x}_{t+\triangle t}&=\mathbf{x}_t+\delta\mathbf{x}
\\
P&=(I-KH)P(I-KH)^T+KVK^T
\end{cases}
\\
\text{重置}:&\begin{cases}
P&=GPG^T
\end{cases}
\end{aligned}
$$
公式里\(V\)为\(3\times 3\)的GPS观察的协方差矩阵，而\(Q\)是噪声协方差矩阵，\(Q=\mathbf{Diag}(Cov(\eta_v),Cov(\eta_w))\)，特别的，对于如下形式的\(Q\)可进行简化  
(11)
$$
\begin{aligned}
Q&=\begin{bmatrix}
\alpha I&\mathbf{0}
\\
\mathbf{0} &\beta I
\end{bmatrix}
\\
&\Downarrow
\\
F_iQF_i^T&=\triangle t^2\begin{bmatrix}
\alpha I &\mathbf{0} & \mathbf{0}
\\
\mathbf{0} &\beta I & \mathbf{0}
\\
\mathbf{0} & \mathbf{0} &\beta I
\end{bmatrix}
\end{aligned}
$$
注意:  
① 公式(10)当中\(P\)的表达式与参考[2]里的不一致，公式(10)里的表达式不局限于\(P\)是否为方阵，且其有更好的数值稳定性。  
② 由于 \(\delta \bar{\theta}\)为0附近一个小量，因此在大部分场景里，可以忽略不计，这时 \(G\)就等价于单位阵，因此不使用重置步骤。

③ \(Q、V\)由仪器提供，或者按经验设置。

④ 计算过程中，注意上面公式里的位置\(\mathbf{p}\)是全局坐标系，\(\bar{\mathbf{v}}、\mathbf{w}、\mathbf{b}\)为车辆局部坐标系，旋转矩阵\(R\)为局部坐标系到世界坐标系的表示方式。

参考
[1] Sola J. Quaternion kinematics for the error-state Kalman filter[J]. arXiv preprint arXiv:1711.02508, 2017.  
[2] 高翔. 简明ESKF推导. 2022