状态估计（KF-EKF-ESKF）

模型描述

离散时间状态估计问题
$$\begin{array}{rl}运动方程：x_k& =f(x_{k-1},u_k)+w_k\\ 观测方程：z_k& =h(x_k)+v_k\end{array}$$
变量含义如下：

• 系统状态：$x_k\in {\mathbb{R}}^n$
• 初始状态：$x_0\in {\mathbb{R}}^n\sim \mathcal{N}({\check{x}}_0,{\check{P}}_0)$
• 输入：$u_k\in {\mathbb{R}}^p$
• 过程噪声：$w_k\in {\mathbb{R}}^n\sim \mathcal{N}(0,R_k)$
• 测量 ：$z_k\in {\mathbb{R}}^m$
• 测量噪声：$v_k\in {\mathbb{R}}^m\sim \mathcal{N}(0,Q_k)$

已知数据：

• 先验初始状态及其协方差矩阵${\check{x}}_0,{\check{P}}_0$
• 输入量$u_{1:k}$
• 观测量$z_{0:k}$
• 系统的运动模型$f()$和观测模型$h()$

待估计数据：

• 系统真实状态的后验估计值${\hat{x}}_k$

最大后验估计(Maximum A Posteriori，MAP)

定义宏观变量：

• $x=x_{0:K}=(x_0,\cdots x_K)$
• $u=({\check{x}}_0,u_{1:K})$
• $z=z_{0:K}=(z_0,\cdots z_K)$

利用贝叶斯公式，得到MAP估计为：
$$\begin{array}{rl}\hat{x}& =\underset{x}{\mathrm{argmax}}p(x|u,z)\\ & =\underset{x}{\mathrm{argmax}}\frac{p(z|x,u)p(x|u)}{p(z|u)}\\ & =\underset{x}{\mathrm{argmax}}p(z|x)p(x|u)\end{array}$$
其中：
$$\begin{array}{rl}p(z|x)& =\prod _{k=0}^{K}p(z_k|x_k)\\ p(x|u)& =p(x_0|{\check{x}}_0)\prod _{k=1}^{K}p(x_k|x_{k-1},u_k)\end{array}$$
所以：
$$\begin{array}{rl}\hat{x}& =\underset{x}{\mathrm{argmax}}p(x_0|{\check{x}}_0)\left(\prod _{k=1}^{K}p(x_k|x_{k-1},u_k)\right)\left(\prod _{k=0}^{K}p(z_k|x_k)\right)\\ & =\underset{x}{\mathrm{argmax}}\left(\ln p(x_0|{\check{x}}_0)+\sum _{k=1}^K\ln (p(x_k|x_{k-1},u_k))+\sum _{k=0}^K(p(z_k|x_k))\right)\end{array}$$

贝叶斯滤波

​ 马尔可夫性：当一个随机过程在给定现在状态及所有过去状态情况下，其未来状态的条件概率分布仅依赖于当前状态；换句话说，在给定现在状态时，未来状态与过去（该过程的历史路径）是条件独立的，那么此随机过程称为马尔可夫过程。

​ 根据马尔可夫性，当已知$x_{k-1}$时，可根据运动方程推出$x_k$
$$\begin{array}{rl}\underset{\text{后}验}{\underbrace{p(x_k|{\check{x}}_0,u_{1:k},z_{0:k})}}& =\eta \underset{\text{似}然}{\underbrace{p(z_k|x_k)}}\underset{\text{先}验}{\underbrace{p(x_k|{\check{x}}_0,u_{1:k},z_{0:k-1})}}\\ & =\eta p(z_k|x_k)\int p(x_k,x_{k-1}|{\check{x}}_0,u_{1:k},z_{0:k-1})d{x}_{k-1}\\ & =\eta p(z_k|x_k)\int p(x_k|x_{k-1},{\check{x}}_0,u_{1:k},z_{1:k-1})p(x_{k-1}|{\check{x}}_0,u_{1:k},z_{0:k-1})d{x}_{k-1}\\ & =\eta \underset{\text{h}(\cdot )进行更新}{\underbrace{p(z_k|x_k)}}\int \underset{\text{f}(\cdot )进行预测}{\underbrace{p(x_k|x_{k-1},u_k)}}\underset{先验}{\underbrace{p(x_{k-1}|{\check{x}}_0,u_{1:k-1},z_{0:k-1})}}d{x}_{k-1}\end{array}$$

广义高斯滤波

假设估计的状态为高斯分布时

从$k-1$时刻高斯先验开始
$$\begin{array}{r}p(x_{k-1}|{\check{x}}_0,u_{1:k-1},z_{0:k-1})\sim \mathcal{N}({\hat{x}}_{k-1},{\hat{P}}_{k-1})\end{array}$$
预测步骤：利用最新的输入$u_k$，通过运动方程，得到$k$时刻的高斯先验
$$\begin{array}{r}p(x_k|{\check{x}}_0,u_{1:k},z_{0:k-1})\sim \mathcal{N}({\check{x}}_k,{\check{P}}_k)\end{array}$$
校正步骤：写出$k$时刻的状态和最新测量的联合高斯分布
$$\begin{array}{r}p(x_k,z_k|{\check{x}}_0,u_{1:k},z_{0:k-1})\sim \mathcal{N}\left(\left[\begin{array}{c}{\mu }_{x,k}\\ {\mu }_{z,k}\end{array}\right],\left[\begin{array}{cc}{\Sigma }_{xx,k}& {\Sigma }_{xz,k}\\ {\Sigma }_{zx,k}& {\Sigma }_{zz,k}\end{array}\right]\right)\end{array}$$
根据多元高斯概率的分解，得到$x_k$的条件概率密度（后验概率）
$$\begin{array}{r}p(x_k|{\check{x}}_0,u_{1:k},z_{0:k})\sim \mathcal{N}(\underset{{\hat{x}}_k}{\underbrace{{\mu }_{x,k}+{\Sigma }_{xz,k}{\Sigma }_{zz,k}^{-1}(z_k-{\mu }_{z,k})}},\underset{\widehat{P_k}}{\underbrace{{\Sigma }_{xx,k}-{\Sigma }_{xz,k}{\Sigma }_{zz,k}^{-1}{\Sigma }_{zx,k}}})\end{array}$$
得出校正方程：
$$\begin{array}{rl}{K}_k& ={\Sigma }_{xz,k}{\Sigma }_{zz,k}^{-1}\\ \widehat{P_k}& =\check{P_k}-K_k{\Sigma }_{xz,k}^T\\ {\hat{x}}_k& ={\check{x}}_k+K_k(z_k-{\mu }_{z,k})\end{array}$$
其中，${\mu }_{x,k}={\check{x}}_k$，${\Sigma }_{xx,k}={\check{P}}_k$

卡尔曼滤波（KF）

$$\begin{array}{rl}运动方程：x_k& =A_{k-1}{x}_{k-1}+u_k+w_k\\ 观测方程：z_k& =C_k{x}_k+v_k\end{array}$$

• 过程噪声：$w_k\in {\mathbb{R}}^n\sim \mathcal{N}(0,R_k)$
• 测量噪声：$v_k\in {\mathbb{R}}^m\sim \mathcal{N}(0,Q_k)$

预测步骤：
$$\begin{array}{rl}{\check{x}}_k& =A_{k-1}{\hat{x}}_{k-1}+u_k\\ {\check{P}}_k& =A_{k-1}{\hat{P}}_{k-1}{A}_{k-1}^T+R_k\end{array}$$
校正步骤：

联合高斯分布参数推导：
$$\begin{array}{rl}{\mu }_{z,k}& =E[z_k]=E[C_k{x}_k+v_k]=C_k{\check{x}}_k\\ {\Sigma }_{zz,k}& =E[(z_k-{\mu }_{z,k})(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(C_k{x}_k+v_k-C_k{\check{x}}_k)(C_k{x}_k+v_k-C_k{\check{x}}_k{)}^T]\\ & =E[C_k(x_k-{\check{x}}_k)+v_k)(C_k(x_k-{\check{x}}_k)+v_k{)}^T]\\ & =C_k\cdot E[(x_k-{\check{x}}_k)(x_k-{\check{x}}_k{)}^T]\cdot C_k^T+E[v_k{v}_k^T]\\ & =C_k{\check{P}}_k{C}_k^T+Q_k\\ {\Sigma }_{xz,k}& =E[(x_k-{\check{x}}_k)(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(x_k-{\check{x}}_k)(C_k(x_k-{\check{x}}_k)+v_k{)}^T]\\ & =E[(x_k-{\check{x}}_k)(x_k-{\check{x}}_k{)}^T{C}_k^T]\\ & ={\check{P}}_k{C}_k^T\\ {\Sigma }_{zx,k}& ={\Sigma }_{xz,k}^T\\ & =C_k{\check{P}}_k^T\\ & =C_k{\check{P}}_k\end{array}$$
带入广义高斯滤波得:
$$\begin{array}{rl}{K}_k& ={\Sigma }_{xz,k}{\Sigma }_{zz,k}^{-1}\\ & ={\check{P}}_k{C}_k^T(C_k{\check{P}}_k{C}_k^T+Q_k{)}^{-1}\\ \widehat{P_k}& =\check{P_k}-K_k{\Sigma }_{xz,k}^T\\ & =\check{P_k}-K_k{C}_k{\check{P}}_k\\ & =(I-K_k{C}_k)\check{P_k}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(z_k-{\mu }_{z,k})\end{array}$$

扩展卡尔曼滤波（EKF）

对运动方程和观测方程进行线性化，即进行一阶泰勒展开
$$\begin{array}{rl}运动方程：x_k& =f(x_{k-1},u_k)+w_k\\ & =f({\hat{x}}_{k-1},u_k)+\frac{\partial f}{\partial x_{k-1}}{|}_{{\hat{x}}_{k-1}}(x_{k-1}-{\hat{x}}_{k-1})+w_k\\ & =f({\hat{x}}_{k-1},u_k)+F_{k-1}(x_{k-1}-{\hat{x}}_{k-1})+w_k\\ 观测方程：z_k& =h(x_k)+v_k\\ & =h({\check{x}}_{k-1})+\frac{\partial h}{x_k}{|}_{{\check{x}}_{k-1}}(x_k-{\check{x}}_{k-1})+v_k\\ & =h({\check{x}}_{k-1})+H_k(x_k-{\check{x}}_{k-1})+v_k\end{array}$$

• 过程噪声：$w_k\in {\mathbb{R}}^n\sim \mathcal{N}(0,R_k)$
• 测量噪声：$v_k\in {\mathbb{R}}^m\sim \mathcal{N}(0,Q_k)$

预测步骤：
$$\begin{array}{rl}{\check{x}}_k& =f({\hat{x}}_{k-1},u_k)\\ {\check{P}}_k& =F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+R_k\end{array}$$
校正步骤：

联合高斯分布推导：
$$\begin{array}{rl}{\mu }_{z,k}& =E[z_k]\\ & =E[h({\check{x}}_{k-1})+H_k(x_k-{\check{x}}_{k-1})+v_k]\\ & =h({\check{x}}_{k-1})\\ {\Sigma }_{zz,k}& =E[(z_k-{\mu }_{z,k})(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(h({\check{x}}_{k-1})+H_k(x_k-{\check{x}}_{k-1})+v_k-h({\check{x}}_{k-1}))\\ & (h({\check{x}}_{k-1})+H_k(x_k-{\check{x}}_{k-1})+v_k-h({\check{x}}_{k-1}){)}^T]\\ & =E[(H_k(x_k-{\check{x}}_{k-1})+v_k)(H_k(x_k-{\check{x}}_{k-1})+v_k{)}^T]\\ & =H_k{\check{P}}_k{H}_k^T+Q_k\\ {\Sigma }_{xz,k}& =E[(x_k-{\check{x}}_k)(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(x_k-{\check{x}}_k)(h({\check{x}}_{k-1})+H_k(x_k-{\check{x}}_{k-1})+v_k-h({\check{x}}_{k-1}){)}^T]\\ & =E[(x_k-{\check{x}}_k)(H_k(x_k-{\check{x}}_{k-1})+v_k{)}^T]\\ & ={\check{P}}_k{H}_k^T\\ {\Sigma }_{zx,k}& ={\Sigma }_{xz,k}^T\\ & =H_k{\check{P}}_k^T\\ & =H_k{\check{P}}_k\end{array}$$
带入广义高斯滤波器得：
$$\begin{array}{rl}{K}_k& ={\Sigma }_{xz,k}{\Sigma }_{zz,k}^{-1}\\ & ={\check{P}}_k{H}_k^T(H_k{\check{P}}_k{H}_k^T+Q_k{)}^{-1}\\ \widehat{P_k}& =\check{P_k}-K_k{\Sigma }_{xz,k}^T\\ & =\check{P_k}-K_k{H}_k{\check{P}}_k\\ & =(I-K_k{H}_k)\check{P_k}\\ {\hat{x}}_k& ={\check{x}}_k+K_k(z_k-{\mu }_{z,k})\\ & ={\check{x}}_k+K_k(z_k-h({\check{x}}_{k-1}))\end{array}$$

误差卡尔曼滤波（ESKF）

将真值（true）状态变量分解为名义（nominal）状态变量和误差（error）状态变量的组合：$x_t=x\oplus \delta x$。书写简单起见，无特殊描述时，下文统一使用一般的$+/-$符号。

核心思想是将名义状态视为大信号（可非线性积分），将误差状态视为小信号（因此可线性积分，适用于线性高斯滤波）。

重写运动方程，将输入噪声项叠加到输入$u_k$上
$$\begin{array}{rl}{x}_{t,k}& =f(x_{t,k-1},u_{t,k})\\ x_k+\delta x_k& =f(x_{k-1}+\delta x_{k-1},u_k+\delta u_k)\\ & =f(x_{k-1},u_k)+\frac{\partial f}{\partial x}{|}_{x_{k-1}}\cdot \delta x_{k-1}+\frac{\partial f}{\partial u}{|}_{u_k}\cdot \delta u_k\\ \delta x_k& =F_{k-1}\cdot \delta x_{k-1}+G_k\cdot \delta u_k\end{array}$$

• 过程噪声：$G_k\cdot \delta u_k=w_k\sim \mathcal{N}(0,R_k)$

$$\delta x_k=F_{k-1}\cdot \delta x_{k-1}+w_k$$

• 初始状态：$\delta {\hat{x}}_0\sim \mathcal{N}(0,{\hat{P}}_0)$

重写观测方程
$$\begin{array}{rl}{z}_k& =h(x_{t,k})+v_k\\ & =h(x_k+\delta x_k)+v_k\\ & =h(x_k)+\frac{\partial h}{\partial \delta x_k}{|}_{x_k}\cdot \delta x_k+v_k\\ & =h(x_k)+H_k\cdot \delta x_k+v_k\end{array}$$

• 测量噪声：$v_k\in {\mathbb{R}}^m\sim \mathcal{N}(0,Q_k)$

预测步骤：
$$\begin{array}{rl}{x}_k& =f(x_{k-1},u_k)\\ \delta {\check{x}}_k& =F_{k-1}\cdot \delta {\hat{x}}_{k-1}\\ {\check{P}}_k& =F_{k-1}{\hat{P}}_{k-1}{F}_{k-1}^T+R_k\end{array}$$
说明：$\delta {\check{x}}_k=F_{k-1}\cdot \delta {\hat{x}}_{k-1}$在此更新意义不大，因每次更新后，会重置$\delta x_k=0$

校正步骤：

联合高斯分布推导
$$\begin{array}{rl}{\mu }_{x,k}& =E[\delta x_k]=0\\ {\Sigma }_{xx,k}& =E[\delta x_k\cdot \delta x_k^T]={\check{P}}_k\\ {\mu }_{z,k}& =E[h(x_k)+H_k\cdot \delta x_k+v_k]\\ & =h(x_k)\\ {\Sigma }_{zz,k}& =E[(z_k-{\mu }_{z,k})(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(h(x_k)+H_k\cdot \delta x_k+v_k-h(x_k)(h(x_k)+H_k\cdot \delta x_k+v_k-h(x_k{)}^T]\\ & =E[(H_k\cdot \delta x_k+v_k)(H_k\cdot \delta x_k+v_k{)}^T]\\ & =H_k{\check{P}}_k{H}_k^T+Q_k\\ {\Sigma }_{xz,k}& =E[(\delta x_k-\delta {\check{x}}_k)(z_k-{\mu }_{z,k}{)}^T]\\ & =E[(\delta x_k)(H_k\cdot \delta x_k+v_k{)}^T]\\ & ={\check{P}}_k{H}_k^T\\ {\Sigma }_{zx,k}& ={\Sigma }_{xz,k}^T\\ & =H_k{\check{P}}_k^T\\ & =H_k{\check{P}}_k\end{array}$$
带入广义高斯滤波得：
$$\begin{array}{rl}{K}_k& ={\check{P}}_k{H}_k^T(H_k{\check{P}}_k{H}_k^T+Q_k{)}^{-1}\\ \widehat{P_k}& =(I-K_k{H}_k)\check{P_k}\\ \delta {\hat{x}}_k& =K_k(z_k-{\mu }_{z,k})\\ & =K_k(z_k-h(x_k))\end{array}$$
误差重置：

更新完成后，将观测误差$\delta x_k$更新到名义状态变量中，保持误差状态变量均值为零。

名义状态变量更新：
$$\begin{array}{r}{\hat{x}}_k=x_k+\delta {\hat{x}}_k\end{array}$$
状态误差重置：
$$\begin{array}{rl}\delta x_k& \leftarrow g(\delta x_k)=\delta x_k\ominus \delta {\hat{x}}_k\\ P& \leftarrow JP{J}^T\\ J& =\frac{\partial g}{\partial \delta x}{|}_{\delta {\hat{x}}_k}\end{array}$$
重置前：$\delta x_k\sim \mathcal{N}(\delta {\hat{x}}_k,{\hat{P}}_k)$描述的是关于$x_k$切空间的分布

重置后：$\delta x_k\sim \mathcal{N}(0,P_k)$描述的是关于$x_k+\delta x_k$切空间的分布

对于欧拉空间$J$为单位阵，但是对于带旋转的状态变量，切空间零点发生改变。Jacobian推导如下：

• 旧的角度误差$\delta \theta$及其对应的四元数$\delta q$，观测误差$\delta \hat{\theta }$

• 新的角度误差$\delta {\theta }^{+}$及其对应的四元数$\delta q^{+}$

• 新的名义旋转变量$q^{+}$

• 旋转的真值不变：
  $$\begin{array}{r}{q}_t=q^{+}\otimes \delta q^{+}=q\otimes \delta q\end{array}$$

• 观察误差注入到名义状态变量
  $$\begin{array}{r}{q}^{+}=q\otimes \delta \hat{q}\end{array}$$

• 联合上述两式得
  $$\begin{array}{rl}\delta q^{+}& =\delta {\hat{q}}^{\ast }\otimes \delta q\\ & =[\delta {\hat{q}}^{\ast }{]}_L\cdot \delta q\\ & \approx {\left[\begin{array}{c}1\\ -\frac{1}{2}\delta \hat{\theta }\end{array}\right]}_L\cdot \delta q\\ \left[\begin{array}{c}1\\ \frac{1}{2}\delta {\theta }^{+}\end{array}\right]& \approx \left[\begin{array}{cc}1& \frac{1}{2}\delta {\hat{\theta }}^T\\ -\frac{1}{2}\delta \hat{\theta }& I-{\left[\frac{1}{2}\delta \hat{\theta }\right]}_{\times }\end{array}\right]\cdot \left[\begin{array}{c}1\\ \frac{1}{2}\delta \theta \end{array}\right]\\ \delta {\theta }^{+}& \approx -\delta \hat{\theta }+\left(I-{\left[\frac{1}{2}\delta \hat{\theta }\right]}_{\times }\right)\delta \theta \end{array}$$
  得到：
  $$\begin{array}{r}\frac{\partial \delta {\theta }^{+}}{\partial \delta \theta }=I-{\left[\frac{1}{2}\delta \hat{\theta }\right]}_{\times }\end{array}$$