误差状态卡尔曼滤波

1. True-state 真值状态 $x$
2. Nominal-state 名义状态 $\bar{x}$
3. Error-state 误差状态 $\delta x$
   $$x=\bar{x}+\delta x$$

• 真值状态被表达成名义状态和误差状态的组合(线性相加、四元数乘法或矩阵乘法)
• 名义状态不考虑噪声项和其他可能的建模误差

连续时间模型

假设系统模型方程:
$$\dot{x}\left(t\right)=f\left(x\left(t\right),v\left(t\right),w\left(t\right)\right)$$
则按照名义状态的定义，有：
$$\dot{\bar{x}}\left(t\right)=f\left(\bar{x}\left(t\right),v\left(t\right),0\right)$$
$$\begin{array}{rl}\dot{x}(t)& =\dot{(\bar{x}\left(t\right)+\delta x)}=\dot{\bar{x}}\left(t\right)+\dot{\delta x}\\ & =f\left(\bar{x}\left(t\right)+\delta x\left(t\right),u\left(t\right),w\left(t\right)\right)\end{array}$$

$$\begin{array}{rl}\dot{\delta x}(t)& =\dot{x}\left(t\right)-\dot{\bar{x}}\left(t\right)=f\left(\bar{x}\left(t\right)+\delta x\left(t\right),u\left(t\right),w\left(t\right)\right)-f\left(\bar{x}\left(t\right),u\left(t\right),0\right)\\ & =\frac{\partial f(\bar{x}(t),u(t),0)}{\partial x}\delta x\left(t\right)+\frac{1}{2!}\frac{{\partial }^{2}f(\bar{x}(t),u(t),0)}{\partial x^2}\delta x{\left(t\right)}^2+\cdots \\ & +\frac{\partial f(\bar{x}(t),u(t),0)}{\partial w}w\left(t\right)+\frac{1}{2!}\frac{{\partial }^{2}f(\bar{x}(t),u(t),0)}{\partial w^2}w{\left(t\right)}^2+\cdots \\ & =\frac{\partial f(\bar{x}(t),u(t),0)}{\partial x}\delta x\left(t\right)+O\left(\delta x\left(t\right)\right)+\frac{\partial f(\bar{x}(t),u(t),0)}{\partial w}w\left(t\right)+O\left(w\left(t\right)\right)\end{array}$$
记$F(t)=\frac{\partial f(\bar{x}(t),u(t),0)}{\partial x},L(t)=\frac{\partial f(\bar{x}(t),u(t),0)}{\partial w}$，忽略噪声$w$的高阶项，则
$$\dot{\delta x}(t)=F(t)\delta x(t)+L(t)w(t)+O\left(\delta x\left(t\right)\right)$$
误差状态方程的离散化：
$$\begin{array}{rl}\delta x\left(t+\Delta t\right)& =\delta x\left(t\right)+\dot{\delta x}\left(t\right)\Delta t\\ & =\delta x\left(t\right)+F\left(t\right)\delta x\left(t\right)\Delta t+O\left(\delta x\left(t\right)\right)\Delta t+L\left(t\right)w\left(t\right)\Delta t\\ & =\left(I+F\left(t\right)\Delta t\right)\delta x\left(t\right)+O\left(\delta x\left(t\right)\right)\Delta t+L\left(t\right)w\left(t\right)\Delta t\\ & =\Phi \left(t\right)\delta x\left(t\right)+O\left(\delta x\left(t\right)\right)\Delta t+L\left(t\right)w\left(t\right)\Delta t\end{array}$$
在ESKF中，$\Phi (t)$被称作State Transition Matrix。ESKF的Predict步骤作近似$\delta x\left(t+\Delta t\right)\simeq \Phi (t)\delta x\left(t\right)$ ，剩下的由于非线性导致的误差项$O\left(\delta x\left(t\right)\right)\Delta t+L\left(t\right)w\left(t\right)\Delta t$在Update步骤使用观测方程进行估计。
从 $F(t)$ 的定义中可以看到，其与 error$\delta x(t)$ 是无关的，使用上一步的估计值 $\hat{x}(t)$ 与这一步的控制值 $u(t)$ 即可计算得到。于是，在 Predict 步骤无需设计 error 部分 $\delta x(t)$ ，对 $\Phi (t)$ 进行近似。

离散时间模型

真实状态的运动模型方程
$$x_k=f\left(x_{k-1},u_k,w_k\right)\text{\hspace{1em}(1)}$$
由名义状态的定义，不考虑噪声项，得到名义状态的运动模型方程
$${\bar{x}}_k=f\left({\bar{x}}_{k-1},u_k,0\right)\text{\hspace{1em}(2)}$$

由定义：$x_k={\bar{x}}_k+\delta x_k$，得
$$\begin{array}{rl}\delta x_k& =f\left(x_{k-1},u_k,w_k\right)-f\left({\bar{x}}_{k-1},u_k,0\right)\\ & =f\left({\bar{x}}_{k-1}+\delta x_{k-1},u_k,w_k\right)-f\left({\bar{x}}_{k-1},u_k,0\right)\\ & =\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial x}\delta x_{k-1}+\frac{1}{2!}\frac{{\partial }^{2}f({\bar{x}}_{k-1},u_k,0)}{\partial x^2}\delta x_{k-1}^2+\cdots \\ & +\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial w}{w}_k+\frac{1}{2!}\frac{{\partial }^{2}f({\bar{x}}_{k-1},u_k,0)}{\partial w^2}{w}_k^2+\cdots \\ & =\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial x}\delta x_{k-1}+O\left(\delta x_{k-1}\right)+\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial w}{w}_k+O\left(w_k\right)\end{array}$$
记$F_{k-1}=\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial x},L_k=\frac{\partial f({\bar{x}}_{k-1},u_k,0)}{\partial w}$，忽略非线性小量部分：$\delta x_k,w$的高阶项，则
$$\delta x_k=F_{k-1}\delta x_{k-1}+L_k{w}_k\text{\hspace{1em}(3)}$$
观测方程：
$$\begin{array}{rl}{y}_k& =h\left(x_k,n_n\right)=h\left({\overline{x}}_k+\delta x_k,n_k\right)\\ & =h\left({\bar{x}}_k+\delta x_k,n_k\right)-h\left({\bar{x}}_k,0\right)+h\left({\overline{x}}_k,0\right)\\ & \approx h\left({\bar{x}}_k,0\right)+\frac{\partial h\left({\bar{x}}_k,0\right)}{\partial x}\delta x_k+\frac{\partial h\left({\bar{x}}_k,0\right)}{\partial n}{n}_k+O(\delta x_k)+O(n_k)\end{array}$$
记$H_{k-1}=\frac{\partial h({\bar{x}}_k,0)}{\partial x},L_k=\frac{\partial h({\bar{x}}_k,0)}{\partial n}$，忽略非线性小量部分：$\delta x_k,n_k$的高阶项，则
$$y_k=h({\bar{x}}_k,0)+H_k\delta x_k+M_k{n}_k\text{\hspace{1em}(4)}$$

ES-EKF过程

ES-EKF直接估计Error State,然后用它矫正Nominal State。在整个滤波过程中，我们实际上修正的变量是$\delta x$。

1. 使用运动模型更新Nominal state
   $${\bar{x}}_k=f\left(x_{k-1},u_k,0\right)$$
   式中的$x_{k-1}$是当前能获得的k-1时刻最优状态估计值。可能是前一次Prediction产生的State值(连续多次使用Motion Model)，也可能是Measurement Update后State值。
2. 前向传播
   我们认为前一时刻的误差状态为零均值的高斯变量
   $$\delta x_{k-1}\sim \mathcal{N}\left(0,{\hat{P}}_{k-1}\right)$$
   由误差状态模型方程
   $$\delta x_k=F_{k-1}\delta x_{k-1}+L_k{w}_k$$
   概率密度向前传播得
   $$\begin{gathered} \delta {\check{x}}_k=0 \\ {\check{P}}_k=F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+L_k{Q}_k{L}_k^T \end{gathered}$$
3. 使用测量更新后验估计
   • 计算卡尔曼增益
     $$K_k={\check{P}}_k{H}_k^T{\left(H_k{\check{P}}_k{H}_k^T+M_k{R}_k{M}_k^T\right)}^{-1}$$
   • 计算误差状态后验估计
     $$\begin{array}{rl}\delta \hat{x}& =\delta \check{x}+K_k\left(y_k-h({\bar{x}}_k,0)\right)\\ {\hat{P}}_k& =(1-K_k{H}_k){\check{P}}_k\end{array}$$
4. 校正名义状态
   $${\hat{x}}_k={\bar{x}}_k+\delta {\hat{x}}_k$$

ESKF的优势0

若状态量里面有旋转，则旋转通常被表示为欧拉角或四元数的形式。

在EKF框架中，若一个旋转状态量：

1. 表示为欧拉角,三个参数三个自由度,则在优化的过程中有可能出现万向节死锁的情况。
2. 表示为四元数,四个参数三个自由度,需要满足模为1的约束,优化的时候可能 会出现协方差奇异(?)的情况。

但这些问题在ESKF中被弥补。在ESKF中，一个误差旋转变量：

1. 表示为欧拉角,在优化的过程中三个角度都在0附近,不用担心万向节死锁的情况出现。
2. 表示为四元数,当一个旋转很小的时候,四元数的w=1,同样只要优化三个参数。
   $$\delta q\simeq {\left[\begin{array}{cc}\frac{1}{2}\delta {\theta }^T& 1\end{array}\right]}^T$$

参考链接1
参考链接2