IKFoM学习笔记(2)状态更新部分

IKFoM状态更新

$\delta {\hat{x}}_k=x_k-{\hat{x}}_k$ ，表示k时刻的状态真值减去k时刻的名义状态变量；
$\delta {\hat{x}}_k^j=x_k-{\hat{x}}_k^j$ ，表示k时刻的状态真值减去k时刻第j次迭代的先验状态变量，$j=0$时，${\hat{x}}_k^j={\hat{x}}_k$；
$x_k-{\hat{x}}_k$ ，表示k时刻的状态真值减去k时刻的名义状态变量；
$\delta x_{k+1}^j$，表示第j次更新的误差状态。

1、观测方程

首先引出观测方程：
$$\begin{gathered} r_{k+1}^j=z_{k+1}\boxminus h(x_{k+1}^j,0) \\ =h(x_{k+1},v_{k+1})\boxminus h({\hat{x}}_{k+1}^j,0) \\ \approx D_{k+1}^j{v}_{k+1}+H_{k+1}^j\delta x_j \end{gathered}$$
其中
$$\begin{gathered} H_{k+1}^j=\frac{\partial (h({\hat{x}}_{k+1}^j⊞\delta x,0)\boxminus h({\hat{x}}_{k+1}^j,0))}{\partial \delta x}{|}_{\delta x=0} \\ =\frac{\partial h({\hat{x}}_{k+1}^j⊞\delta x,0)}{\partial \delta x}{|}_{\delta x=0} \\ {D}_{k+1}^j=\frac{\partial (h({\hat{x}}_{k+1}^j⊞\delta x,v)\boxminus h({\hat{x}}_{k+1}^j,0))}{\partial \delta v}{|}_{\delta v=0} \\ =\frac{\partial h({\hat{x}}_{k+1}^j⊞\delta x,v)}{\partial \delta x}{|}_{\delta v=0} \end{gathered}$$
进一步得出：
$$\begin{gathered} D_{k+1}^j{v}_{k+1}=r_{k+1}^j-H_{k+1}^j\delta x_j，服从高斯分布N(0,{\overline{R}}_{k+1}) \\ {\overline{\mathcal{R}}}_{k+1}=D_{k+1}^j{\mathcal{R}}_{k+1}(D_{k+1}^j{)}^T \end{gathered}$$

2、切空间投影变换

$$\begin{gathered} \delta {\hat{x}}_{k+1}=x_{k+1}\boxminus {\hat{x}}_{x+1} \\ =(x_{k+1}^j⊞\delta {\hat{x}}_{k+1}^j)\boxminus {\hat{x}}_{k+1} \\ =(x_{k+1}^j\boxminus {\hat{x}}_{k+1})+(J_{k+1}^j{)}^{-1}\delta {\hat{x}}_{k+1}^j \end{gathered}$$
其中
$$\begin{gathered} J_{k+1}^j=\frac{\partial (((x⊞u)\oplus v)\boxminus y)}{\partial u}{|}_{x={\hat{x}}_{k+1},u={\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1},v=0,y={\hat{x}}_{k+1}^j} \\ \delta {\hat{x}}_{k+1}\sim \mathcal{N}(0,{\hat{P}}_{k+1}) \end{gathered}$$
是$\partial {\hat{x}}_{k+1}$关于$\partial {\hat{x}}_{k+1}^j$的逆雅可比矩阵

继而可以得出：
$$\delta {\hat{x}}_{k+1}^j\sim \mathcal{N}(-J_{k+1}^j({\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1}),J_{k+1}^j{\hat{P}}_{k+1}(J_{k+1}^j{)}^T)$$
其中$-J_{k+1}^j({\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1})$由如下得出：
$$\begin{gathered} \delta {\hat{x}}_{k+1}=0=(x_{k+1}^j\boxminus {\hat{x}}_{k+1})+(J_{k+1}^j{)}^{-1}\delta {\hat{x}}_{k+1}^j \\ -(x_{k+1}^j\boxminus {\hat{x}}_{k+1})=(J_{k+1}^j{)}^{-1}\delta {\hat{x}}_{k+1}^j \\ \delta {\hat{x}}_{k+1}^j=-J_{k+1}^j(x_{k+1}^j\boxminus {\hat{x}}_{k+1}) \end{gathered}$$

3、迭代更新

综上所述迭代误差状态卡尔曼滤波可以等价为$\delta {\hat{x}}_{k+1}^j$的最大后验估计，其中：
先验分布为 $\delta {\hat{x}}_{k+1}^j\sim \mathcal{N}(-J_{k+1}^j({\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1}),J_{k+1}^j{\hat{P}}_{k+1}(J_{k+1}^j{)}^T)$
似然分布为$D_{k+1}^j{v}_{k+1}\sim \mathcal{N}(0,{\overline{R}}_{k+1})$

最大后验估计表示为：
$$\begin{gathered} \arg \underset{\delta {\hat{\mathbf{x}}}_{k+1}^j}{\max }\log \left(\mathcal{N}(\delta {\hat{x}}_{k+1}^j)\mathcal{N}\left(({\mathbf{D}}_{k+1}^j{v}_{k+1})|\delta {\hat{x}}_{k+1}^j\right)\right)=\arg \underset{\delta {\mathbf{x}}_{k+1}^j}{\min }g(\delta {\hat{\mathbf{x}}}_{k+1}^j); \\ g(\delta {\hat{\mathbf{x}}}_{k+1}^j)=\Vert {\mathbf{r}}_{k+1}^j-{\mathbf{H}}_{k+1}^j\delta {\hat{\mathbf{x}}}_{k+1}^j{\Vert }_{{\overline{\mathcal{R}}}_{k+1}}^2+\Vert ({\hat{\mathbf{x}}}_{k+1}^j\boxminus {\hat{\mathbf{x}}}_{k+1})+({\mathbf{J}}_{k+1}^j{)}^{-1}\delta {\hat{\mathbf{x}}}_{k+1}^j{\Vert }_{{\hat{\mathbf{P}}}_{k+1}}^2; \end{gathered}$$
下面是卡尔曼滤波的更新过程：
$$\begin{gathered} \delta x_{k+1}^j=\delta {\hat{x}}_{k+1}^j+{\mathbf{K}}_{k+1}^j({\mathbf{r}}_{k+1}^j-{\mathbf{H}}_{k+1}^j\delta {\hat{x}}_{k+1}^j) \\ =-{\mathbf{J}}_{k+1}^j({\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1})+{\mathbf{K}}_{k+1}^j({\mathbf{r}}_{k+1}^j+{\mathbf{H}}_{k+1}^j{\mathbf{J}}_{k+1}^j({\hat{x}}_{k+1}^j\boxminus {\hat{x}}_{k+1})) \end{gathered}$$
卡尔曼增益：
$${\mathbf{K}}_{k+1}^j={\mathbf{J}}_{k+1}^j{\hat{\mathbf{P}}}_{k+1}({\mathbf{J}}_{k+1}^j{)}^T({\mathbf{H}}_{k+1}^j{)}^T({\mathbf{H}}_{k+1}^j{\mathbf{J}}_{k+1}^j{\hat{\mathbf{P}}}_{k+1}({\mathbf{J}}_{k+1}^j{)}^T({\mathbf{H}}_{k+1}^j{)}^T+{\overline{\mathcal{R}}}_{k+1}{)}^{-1}$$
更新状态变量：
$${\hat{x}}_{k+1}^{j+1}={\hat{x}}_{k+1}^j⊞\delta x_{k+1}^j$$

4、状态更新

迭代收敛后首先更新协方差矩阵：
$${\mathbf{P}}_{k+1}^j=(\mathbf{I}-{\mathbf{K}}_{k+1}^j{\mathbf{H}}_{k+1}^j){\mathbf{J}}_{k+1}^j{\hat{\mathbf{P}}}_{k+1}({\mathbf{J}}_{k+1}^j{)}^{\top }$$
最后对协方差矩阵进行切空间投影

更新后的协方差矩阵${\hat{P}}_{k+1}^j$位于上图中$x_{k+1|k+1}^k$的切空间中，而在下一次滤波中要把${\hat{P}}_{k+1}^j$作为先验协方差矩阵，投影的上图$x_{k+1|k+1}^{k+1}$的切空间中：
$$P_{k+1}={\mathbf{L}}_{k+1}{\hat{P}}_{k+1}^j{\mathbf{L}}_{k+1}中$$

在迭代卡尔曼滤波中，迭代收敛后通常${\mathbf{L}}_{k+1}=\mathbf{I}$