FA：formulas and algorithm, FF：fusion and filtering，ESKF：（Error State Kalman Filter）
经典文献：Joan Solà 《Quaternion kinematics for the error-state Kalman filter》，VIO / LIO / IMU+GNSS 组合导航的标准框架，也就是基于误差模型 / 误差状态的卡尔曼滤波。
一、核心思想：名义状态 + 误差状态分离
ESKF 不直接估计真值，把真值拆成两部分：
xtrue=xnominal⊕δx\boldsymbol{x}{true} = \boldsymbol{x}{nominal} \oplus \delta\boldsymbol{x}xtrue=xnominal⊕δx
- xnominal\boldsymbol{x}_{nominal}xnominal：名义状态，非线性积分传播（IMU 机械编排，四元数积分），不带噪声；
- δx\delta\boldsymbol{x}δx：误差状态 （小量，切空间向量），是卡尔曼滤波真正估计的变量，线性；
- ⊕\oplus⊕：状态叠加，旋转是四元数乘法，位置 / 速度 / 偏置是普通加法。
对比传统 EKF：EKF 直接对真值状态线性化；ESKF 在误差空间线性化 ，误差始终是小量，线性化误差更小，SO (3) 旋转用 3 维微小旋转向量δθ\delta\mathbf{\theta}δθ，无奇异性，数值稳定性远优于直接 EKF。
IMU 组合导航标准 15 维误差状态（最常用）
δx=δp3×1δv3×1δθ3×1δba, 3×1δbg, 3×1 \delta\boldsymbol{x}= \begin{bmatrix} \delta\boldsymbol{p}{3\times1} \\ \delta\boldsymbol{v}{3\times1} \\ \delta\boldsymbol{\theta}{3\times1} \\ \delta\boldsymbol{b}{a,\;3\times1} \\ \delta\boldsymbol{b}_{g,\;3\times1} \end{bmatrix} δx= δp3×1δv3×1δθ3×1δba,3×1δbg,3×1
- δp\delta\mathbf{p}δp：位置误差
- δv\delta\mathbf{v}δv：速度误差
- δθ\delta\mathbf{\theta}δθ：姿态微小旋转（SO(3) 切空间，3 维）
- δba\delta\mathbf{b}_aδba：加速度计偏置误差
- δbg\delta\mathbf{b}_gδbg：陀螺仪偏置误差
名义状态 ：xn=p,v,q,ba,bg\mathbf{x}_n = \\mathbf{p},\\mathbf{v},\\mathbf{q},\\mathbf{b}_a,\\mathbf{b}_gxn=p,v,q,ba,bg，其中q\mathbf{q}q是四元数。
二、ESKF 完整流程（两大阶段：Predict 预测 + Update 更新 + Reset 重置）
1. Predict：IMU 积分传播名义状态 + 传播误差协方差 P
误差状态δx\delta\mathbf{x}δx 预测阶段理论上是 0（只传播协方差）
1.1 名义状态离散传播（IMU 机械编排，dt 为 IMU 采样间隔）
pn,k+1=pn,k+vn,k⋅dt+12(R(qk)(am−ba,k)+g)dt2vn,k+1=vn,k+(R(qk)(am−ba,k)+g)dtqk+1=qk⊗exp(12(ωm−bg,k)dt)ba,k+1=ba,kbg,k+1=bg,k \begin{align*} \mathbf{p}{n,k+1} &= \mathbf{p}{n,k} + \mathbf{v}{n,k}\cdot dt + \frac{1}{2}\big(R(\mathbf{q}k)(\mathbf{a}m-\mathbf{b}{a,k})+\mathbf{g}\big)dt^2\\ \mathbf{v}{n,k+1} &= \mathbf{v}{n,k} + \big(R(\mathbf{q}k)(\mathbf{a}m-\mathbf{b}{a,k})+\mathbf{g}\big)dt\\ \mathbf{q}{k+1} &= \mathbf{q}k \otimes \exp\left(\frac{1}{2} (\mathbf{\omega}m-\mathbf{b}{g,k})dt\right)\\ \mathbf{b}{a,k+1} &= \mathbf{b}{a,k}\\ \mathbf{b}{g,k+1} &= \mathbf{b}_{g,k} \end{align*} pn,k+1vn,k+1qk+1ba,k+1bg,k+1=pn,k+vn,k⋅dt+21(R(qk)(am−ba,k)+g)dt2=vn,k+(R(qk)(am−ba,k)+g)dt=qk⊗exp(21(ωm−bg,k)dt)=ba,k=bg,k
am,ωm\mathbf{a}_m,\mathbf{\omega}_mam,ωm：IMU 测量的加速度、角速度；R(q)R(\mathbf{q})R(q)：四元数转旋转矩阵；g\mathbf{g}g：重力。
1.2 误差状态连续动力学，离散化得到状态转移矩阵(\boldsymbol{F})
δx˙=Fcδx+Gcw\delta\dot{\mathbf{x}} = \mathbf{F}_c \delta\mathbf{x}+\mathbf{G}_c \mathbf{w}δx˙=Fcδx+Gcw
离散：
δxk+1=Fδxk+Gwk\delta\mathbf{x}_{k+1} = \mathbf{F}\delta\mathbf{x}_k+\mathbf{G}\mathbf{w}_kδxk+1=Fδxk+Gwk
协方差传播：
Pk+1∣k=FPk∣kFT+GQGT\mathbf{P}{k+1|k} = \mathbf{F}\mathbf{P}{k|k}\mathbf{F}^T + \mathbf{G}\mathbf{Q}\mathbf{G}^TPk+1∣k=FPk∣kFT+GQGT
Q\mathbf{Q}Q：IMU 噪声对角阵（加速度噪声、陀螺噪声、偏置随机游走）
2. Update：观测到来（GNSS 位置 / 视觉 / 激光里程计），卡尔曼更新误差
2.1、计算观测残差：r=z−h(xnominal)\mathbf{r}= \mathbf{z}-h(\mathbf{x}_{nominal})r=z−h(xnominal)
2.2、观测雅可比矩阵H\mathbf{H}H：观测对误差状态δx\delta xδx求导（不是对名义状态）
2.3、卡尔曼增益：
K=Pk∣k−1HT(HPk∣k−1HT+R)−1 \mathbf{K}= \mathbf{P}{k|k-1}\mathbf{H}^T\left(\mathbf{H}\mathbf{P}{k|k-1}\mathbf{H}^T+\mathbf{R}\right)^{-1} K=Pk∣k−1HT(HPk∣k−1HT+R)−1
R\mathbf{R}R：观测噪声协方差
2.4、 更新误差状态：
δx^=Kr \delta\hat{\mathbf{x}} = \mathbf{K}\mathbf{r} δx^=Kr
2.5、更新误差协方差（Joseph 形式保证正定）：
Pk∣k=(I−KH)Pk∣k−1(I−KH)T+KRKT \mathbf{P}{k|k}=(\mathbf{I}-\mathbf{K}\mathbf{H})\mathbf{P}{k|k-1}(\mathbf{I}-\mathbf{K}\mathbf{H})^T+\mathbf{K}\mathbf{R}\mathbf{K}^T Pk∣k=(I−KH)Pk∣k−1(I−KH)T+KRKT
3. Reset（ESKF 特有！）：把误差δx^\delta\hat{x}δx^回馈到名义状态，重置误差状态为 0
pn←pn+δpvn←vn+δvqn←qn⊗exp(δθ/2)ba←ba+δbabg←bg+δbg \begin{align*} \mathbf{p}_n &\leftarrow \mathbf{p}_n+\delta\mathbf{p}\\ \mathbf{v}_n &\leftarrow \mathbf{v}_n+\delta\mathbf{v}\\ \mathbf{q}_n &\leftarrow \mathbf{q}_n \otimes \exp(\delta\mathbf{\theta}/2)\\ \mathbf{b}_a &\leftarrow \mathbf{b}_a+\delta\mathbf{b}_a\\ \mathbf{b}_g &\leftarrow \mathbf{b}_g+\delta\mathbf{b}_g \end{align*} pnvnqnbabg←pn+δp←vn+δv←qn⊗exp(δθ/2)←ba+δba←bg+δbg
误差状态清零：δx←0\delta\mathbf{x}\leftarrow \mathbf{0}δx←0
协方差做重置映射：P←GresetPGresetT\mathbf{P}\leftarrow \mathbf{G}{reset}\mathbf{P}\mathbf{G}{reset}^TP←GresetPGresetT；小角度下Greset≈I\mathbf{G}_{reset}\approx IGreset≈I，工程常近似单位阵。
✅ ESKF 关键区别：每次更新完，误差被打进名义状态，误差状态归零，下一轮继续用小量假设。
三、ESKF 优点总结
- 旋转用 3 维微小旋转向量，无欧拉角奇异性；四元数仅用于名义状态；
- 误差始终是小量，雅可比简单，线性化误差小，一致性更好；
- 分离 IMU 积分（非线性）与滤波更新（线性误差空间），工程模块化；
- 广泛用于 VIO、LIO、组合导航、机器人定位。
四、ESKF 与 EKF、IEKF 对比
表格
| 算法 | 估计对象 | 旋转表达 | 线性化位置 | 特点 |
|---|---|---|---|---|
| EKF | 直接估计真值状态 | 四元数 / 欧拉角 | 真值空间 | 容易线性化误差大，奇异 |
| ESKF | 估计误差状态 δx | 名义四元数，误差 3 维微小旋转 | 误差切空间 | 小量、稳定、VIO/LIO 标配 |
| InEKF | 不变误差 | 李群 | 不变流形 | 一致性更强，更复杂 |