ESKF中关于状态reset的一些理解

ESKF最后估计出来的后验误差状态为：$\delta X\sim N(\widehat{\delta X},\hat{P})$
将误差状态injection到nominal 状态后，误差状态需要被重置，用于下一次的卡尔曼迭代，也就是所谓的reset，数学表达为:$\delta X^{+}=\delta X\ominus \widehat{\delta X}=g(\delta X)$
也就是重置后的状态为$\delta X^{+}$，也同样是一个高斯分布的随机变量。
但是我们实际关心是这个高斯随机变量的均值和协方差阵，然而，$g(\delta X)$并不能保证是一个线性函数，因此首先想到的便是进行泰勒展开线性化，由于$\delta X$是高斯随机变量，因此我们因该对$\delta X$进行泰勒展开，线性化点位于$\widehat{\delta X}$处,展开后如下：
$$\begin{gathered} \delta X^{+}=g(\delta X)=g(\widehat{\delta X})+\frac{\partial g(\delta X)}{\partial \delta X}{|}_{\widehat{\delta X}}(\delta X-\widehat{\delta X}) \\ =g(\widehat{\delta X})+G(\delta X-\widehat{\delta X}) \end{gathered}$$
则均值为：$E(\delta X^{+})=g(\widehat{\delta X})$
方差：$P=E((\delta X^{+}-E(\delta X^{+}))(\delta X^{+}-E(\delta X^{+}{)}^T)=G\hat{P}{G}^T$
显然，对于各个状态，$g(\widehat{\delta X})=\widehat{\delta X}\ominus \widehat{\delta X}=0$,即reset后误差状态的均值为0。
$G=\frac{\partial g(\delta X)}{\partial \delta X}{|}_{\widehat{\delta X}}$,显然，对于$P,V,bias$等具有线性性质的变量来说，这个偏导数就是单位阵，但是对于$\delta \theta$来说，$\delta \theta \ominus \widehat{\delta \theta }\ne \delta \theta -\widehat{\delta \theta }$,因此，$G$可以写做：
$$G=\left[\begin{array}{ccc}{I}_6& 0& 0\\ 0& \frac{\partial \delta \theta \ominus \widehat{\delta \theta }}{\partial \delta \theta }{|}_{\widehat{\delta \theta }}& 0\\ 0& 0& I_9\end{array}\right]$$
那么接下来的关键就在于求解$\frac{\partial \delta \theta \ominus \widehat{\delta \theta }}{\partial \delta \theta }{|}_{\widehat{\delta \theta }}$这一项了。
其中：
$$\delta \theta \ominus \widehat{\delta \theta }=\delta {\theta }^{+}$$
而状态的真实值是不变的，因此有：$$q^{+}\otimes q(\delta {\theta }^{+})=q\otimes q(\delta \theta )$$知 $q^{+}=q\otimes q(\widehat{\delta \theta })$,因此有：$$q(\delta {\theta }^{+})=q(\widehat{\delta \theta }{)}^{\ast }\otimes q(\delta \theta )=[q(\widehat{\delta \theta }{)}^{\ast }{]}_{L}q(\delta \theta )$$即：$$\left[\begin{array}{c}1\\ \frac{\delta {\theta }^{+}}{2}\end{array}\right]=\left[\begin{array}{cc}1& \frac{{\widehat{\delta \theta }}^T}{2}\\ -\frac{\widehat{\delta \theta }}{2}& I-[\frac{\widehat{\delta \theta }}{2}{]}_{\times }\end{array}\right]\left[\begin{array}{c}1\\ \frac{\delta \theta }{2}\end{array}\right]$$
展开后整理可得：$\delta {\theta }^{+}=-\widehat{\delta \theta }+(I-[\frac{\widehat{\delta \theta }}{2}{]}_{\times })\delta \theta$
与是$$\frac{\partial \delta \theta \ominus \widehat{\delta \theta }}{\partial \delta \theta }{|}_{\widehat{\delta \theta }}=\frac{\partial \delta {\theta }^{+}}{\partial \delta \theta }{|}_{\widehat{\delta \theta }}=I-[\frac{\widehat{\delta \theta }}{2}{]}_{\times }$$
将上式带入到G的表达式中，可得：
$$G=\left[\begin{array}{ccc}{I}_6& 0& 0\\ 0& I-[\frac{\widehat{\delta \theta }}{2}{]}_{\times }& 0\\ 0& 0& I_9\end{array}\right]$$
最后，误差状态reset的总结如下：
1、reset后均值为0。
2、reset后协方差为$G\hat{P}{G}^T$。
3、由于$\widehat{\delta \theta }$很小，因此，大部分情况下也可以将$G$近似为单位阵。