ESKF的更新过程

前面介绍的是ESKF的运动过程，现在考虑更新过程。假设一个抽象的传感器能够对状态变量产生观测，其观测方程为抽象的h,那么可以写为

其中z为观测数据，v为观测噪声，V为该噪声的协方差矩阵。

在传统的EKF中，可以直观地对观测方程线性化，求出观测方程相对与状态变量的雅克比矩阵，进而更新卡尔曼滤波器。而在ESKF中，当前拥有名义状态变量x的估计和误差状态变量delta x的估计，且希望更新的是误差状态，因此要计算观测方程相对于误差状态的雅克比矩阵：

然后计算卡尔曼增益，进而计算误差状态的更新过程：

其中，K为卡尔曼增益，Ppred为预测的协方差矩阵，最后的P为修正后的协方差矩阵。

下面就是如何计算H，大部分的观测数据是对名义状态的观测。此时H可以通过链式法则来生成：

其中第一项只需对观测方程进行线性化；第二项，根据之前对状态变量的定义，可以得到

其它几项都是平凡的，只有旋转部分，因为 delat theat 定义为R的右乘，用右乘的BCH即可