流形上的迭代测量更新 - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (IV)

题目: 流形上的迭代测量更新 - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (IV)

---

前言

Dongjiao He, Wei Xu, Fu Zhang 的论文 “Kalman Filter on Differentiable Manifolds”^[1] 中构建了 IKFoM (“流形上的迭代 Kalman 滤波”). 该论文的知识含量很丰富, 对机器人姿态估计 Kalman Filter 进行了完整和深刻的论述, 需要认认真真读. 对初学者而言, 可能觉得流形上的公式太抽象太复杂了, 我自己也是这么觉得. 但抽丝剥茧慢慢理清后, 还是很有收获的.

书接上回, 我们本篇博客对其迭代测量更新部分进行详细推导和说明.

往期回顾

涉及到的之前内容可以查看

- 流形上的运动学 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (I)

- 流形上的复合函数偏导数 - Notes for “Kalman Filter on Differentiable Manifolds“ - IKFoM (II)

- 流形上的状态预测及误差传播 - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (III)

后期展望

- 流形上的投影雅可比矩阵 (Jacobian Matrix) - Notes for “Kalman Filter on Differentiable Manifolds” - IKFoM (V)
- 基于 Boost Preprocessor 的 Manifold Toolkit (MTK) 通用流形类构建 —— IKFoM 中实现的源码解读

本篇博客涉及的基础运算的设定、系统方程、误差方程、协方差计算等相关前设内容, 基本上都已在上面第三篇博客中陈述了, 我们不再重复.

由上一篇博客知道, 在预测过程中

- 系统状态根据状态预测方程进行预测;

- 系统状态的误差协方差是通过误差状态的误差协方差来求得的;

- 误差状态的预测值一直为零.

接下来要讨论的迭代测量更新过程, 我们将根据获得的测量值

- 更新误差状态的估计值

- 在不同的局部同胚"切平面"之间投影误差状态的协方差

- 更新系统状态

- 将误差协方差向更新后的系统状态对应的局部同胚"切平面"进行投影

- 迭代更新过程的可视化说明

---

I. 预备知识

1. 迭代扩展卡尔曼滤波的测量更新

Iterated Extended Kalman Filter Measurement Update^[2]

Step 1: (Initialization) Set the iteration $i=0$, and set the predictive estimate as the initial state
$${\hat{\mathbf{x}}}_{k+1}^0={\hat{\mathbf{x}}}_{k+1|k}\text{\hspace{1em}(I-1)}$$
with the predictive covariance as the initial covariance ${\mathbf{P}}_{k+1|k}$

Step 2: (Measurement Update Iterations) Compute the Jacobi matrix at the last state estmate available, the Kaman gain, and the next iteration of the state estimate as
$${\mathbf{H}}_{k+1}^i={\frac{\partial {\mathbf{h}}_{k+1}(\mathbf{s})}{\partial \mathbf{s}}|}_{{\hat{\mathbf{x}}}_{k+1}^i}\text{\hspace{1em}(I-2)}$$

$${\mathbf{K}}_{k+1}^i={\mathbf{P}}_{k+1|k}({\mathbf{H}}_{k+1}^i{)}^{\mathrm{T}}{\left({\mathbf{H}}_{k+1}^i{\mathbf{P}}_{k+1|k}({\mathbf{H}}_{k+1}^i{)}^{\mathrm{T}}+{\mathcal{R}}_{k+1}\right)}^{-1}\text{\hspace{1em}(I-3)}$$

$${\hat{\mathbf{x}}}_{k+1}^{i+1}={\hat{\mathbf{x}}}_{k+1|k}+{\mathbf{K}}_{k+1}^i\left[{\mathbf{z}}_{k+1}-{\mathbf{h}}_{k+1}({\hat{\mathbf{x}}}_{k+1}^i)-{\mathbf{H}}_{k+1}^i({\mathbf{x}}_{k+1|k}-{\hat{\mathbf{x}}}_{k+1}^i)\right]\text{\hspace{1em}(I-4)}$$

Repeat Step 2 with $i=i+1$ untial a stopping criterion is met, such as, (1) a maximum number of iterations , or (2) a negligible change between two consecutive iterations.

Step 3: (Updating the State and Covariance) Save the last iteration as the new filtering mean and compute the covariance matrix based on the last iteration
$${\hat{\mathbf{x}}}_{k+1|k+1}={\hat{\mathbf{x}}}_{k+1}^{i+1}\text{\hspace{1em}(I-5)}$$

$${\mathbf{P}}_{k+1|k+1}=(\mathbf{I}-{\mathbf{K}}_{k+1}^i{\mathbf{H}}_{k+1}^i){\mathbf{P}}_{k+1|k}\text{\hspace{1em}(I-6)}$$

流形上迭代卡尔曼滤波 IKFoM 会借助误差状态来实现计算, 因为误差状态处在与流形局部同胚的线性空间, 相比流形本身具有更好的可计算性.

此处之所以先把欧式空间中的迭代扩展卡尔曼滤波进行展示, 更多是为了帮助在流形上对应部分的理解.

---

2. 贝叶斯定理

贝叶斯定理 Bayes’ Theorem^[3]

参数 $\theta$ 的后验概率函数 (或后验概率分布)
$$P(\theta |\mathbf{x})=\frac{P(\theta )\cdot P(\mathbf{x}|\theta )}{P(\mathbf{x})}\text{\hspace{1em}(I-7)}$$

后验概率 Posterior Probability 是未知参数 $\theta$ 基于试验和测量后得到的概率. 即已经测得样本数据 $\mathbf{x}$ (数据 $\mathbf{x}$ 代表的事件发生) 的情况下, 事件发生的原因是由某个因素 (参数 $\theta$ 刻画) 引起的可能性的大小.

称 $P(\theta )$ 为关于参数 $\theta$ 的先验概率 Prior Beliefs, 是根据以往经验或者分析得到参数 $\theta$ 的发生概率.

称 $P(\mathbf{x}|\theta )$ 为似然 Likelihood, 描述了取得这指定样本数据的概率值, 而这个概率值由未知参数 $\theta$ 决定. 直白一点就是假定参数 $\theta$ 情况下, 某个样本事件 $\mathbf{x}$ 发生的概率.

式 (I-7) 分母中 $P(\mathbf{x})$ 是样本数据 $\mathbf{x}$ 被检测到 (数据 $\mathbf{x}$ 代表的事件发生) 的概率总和, 利用全概率公式计算
$$P(\mathbf{x})=\{\begin{array}{rlr}& {\int }_{\theta }P(\theta )P(\mathbf{x}|\theta )d\theta & \text{if }\theta \text{ is continuous}\\ & \sum _{\theta }P(\theta )P(\mathbf{x}|\theta )& \text{if }\theta \text{ is discrete}\end{array}$$
全概率 $P(\mathbf{x})$ 并不是参数 $\theta$ 的函数. 故式 (I-7) 可以重写为
$$\begin{gathered} P(\theta |\mathbf{x})\propto P(\theta )\cdot P(\mathbf{x}|\theta ) \\ \text{i.e. posterior }\propto \text{ prior}\times \text{likelihood}\text{\hspace{1em}(I-8)} \end{gathered}$$

---

3. 最大后验估计

最大后验估计 The Maximum a Posteriori Estimator

The Maximum a Posteriori (MAP) Estimator of $\theta$ is the value of $\theta$ that maximizes the posterior distribution of $\theta$.
$${\theta }_{\text{MAP}}=\underset{\theta }{\mathrm{argmax}}P(\theta |\mathbf{x})\text{\hspace{1em}(I-9)}$$
将贝叶斯定理中式 (I-8) 代入, 并利用 $log$ 函数的单调性, 得到
$${\theta }_{\text{MAP}}=\underset{\theta }{\mathrm{argmax}}P(\theta |\mathbf{x})=\underset{\theta }{\mathrm{argmaxlog}}\left(P(\theta )\cdot P(\mathbf{x}|\theta )\right)\text{\hspace{1em}(I-10)}$$

---

II. 误差融合估计

在 $k+1$ 时刻获得新的测量值 ${\mathbf{z}}_{k+1}$ 后的更新过程, 这是一个状态预测值根据测量值的修正过程, 最终获得一个最大后验估计值. 这也是预测信息和测量信息融合的过程. 如同预测过程一样, 为了实现流形上的计算, 借助误差状态进行迭代更新以及误差协方差计算.

下面的推导过程中, 在几何上可以局部同胚、投影、正交投影 (最小二乘法) 等解释; 在贝叶斯角度下, 可以视作先验概率、似然概率、最大后验估计等.

---

1. 初始误差

误差状态 $\delta {\mathbf{x}}_{k+1|k}$ 在状态预测值 ${\mathbf{x}}_{k+1|k}$ 附近与状态真实值 ${\mathbf{x}}_{k+1}$ 局部同胚. 将误差状态 $\delta {\mathbf{x}}_{k+1|k}$ 的均值和协方差描述的分布作为迭代更新的初始分布.
$$\delta {\mathbf{x}}_{k+1|k}\triangleq {\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k}\sim \mathcal{N}(0,{\mathbf{P}}_{k+1|k})\text{\hspace{1em}(II-1-1)}$$
记 ${\mathbf{x}}_{k+1|k+1}^j$ 为第 $j$ 次迭代修正的系统状态的估计值. 当 $j=0$ 时, ${\mathbf{x}}_{k+1|k+1}^j={\mathbf{x}}_{k+1|k}$.

---

2. 测量残差与似然概率

如同误差状态的定义一样, 构造测量残差为真实的测量值减去预测的测量值, 即
$${\mathbf{r}}_{k+1}^j\triangleq {\mathbf{z}}_{k+1}\boxminus \mathbf{h}({\mathbf{x}}_{k+1|k+1}^j,0)\text{\hspace{1em}(II-2-1)}$$
下面的分析中，假设测量向量 ${\mathbf{z}}_{k+1}\in {\mathbb{R}}^m$.

通过新定义迭代误差状态建立真实状态值与迭代修正值之间的关系
$$\delta {\mathbf{x}}_j\triangleq {\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^j\text{\hspace{1em}(II-2-2)}$$
$\delta {\mathbf{x}}_j\in {\mathbb{R}}^n$ 与 ${\mathbf{x}}_{k+1}\in {\mathcal{M}}_s$ 在 ${\mathbf{x}}_{k+1|k+1}^j$ 附近局部同胚. 以贝叶斯的观点, 将视 $\delta {\mathbf{x}}_j$ 为贝叶斯公式中的随机参数 ($\theta$) —— 待估计的值.

将上一篇博客中的测量方程 (Measurement equation) 代入式 (II-2-1) , 并在 “点” $(\delta {\mathbf{x}}_j=0,{\mathbf{v}}_{k+1}=0)$ 附近进行一阶泰勒展开获得线性化结果
$$\begin{array}{rl}{\mathbf{r}}_{k+1}^j& =\mathbf{h}({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j,{\mathbf{v}}_{k+1})\boxminus \mathbf{h}({\mathbf{x}}_{k+1|k+1}^j,0)\\ & \approx {\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}+{\mathbf{H}}_{k+1}^j\delta {\mathbf{x}}_j\end{array}\text{\hspace{1em}(II-2-3)}$$
当测量为 ${\mathbb{R}}^m$ 空间上的向量时 (保证偏微分的可行性), 计算
$${\mathbf{H}}_{k+1}^j={\frac{\partial \mathbf{h}({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j,0)}{\partial \delta {\mathbf{x}}_j}|}_{\delta {\mathbf{x}}_j=0}\text{\hspace{1em}(II-2-4)}$$

及
$${\mathbf{D}}_{k+1}^j={\frac{\partial \mathbf{h}({\mathbf{x}}_{k+1|k+1}^j,{\mathbf{v}}_{k+1})}{\partial {\mathbf{v}}_{k+1}}|}_{{\mathbf{v}}_{k+1}=0}\text{\hspace{1em}(II-2-5)}$$
${\mathbf{H}}_{k+1}^j\delta {\mathbf{x}}_j$ 作为 ${\mathbf{r}}_{k+1}^j$ 的预测.

由上一篇博客中测量噪声的已知分布,
$${\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}={\mathbf{r}}_{k+1}^j-{\mathbf{H}}_{k+1}^j\delta {\mathbf{x}}_j\sim \mathcal{N}(0,{\bar{\mathcal{R}}}_{k+1}^j)\text{\hspace{1em}(II-2-6)}$$
其中
$${\bar{\mathcal{R}}}_{k+1}^j={\mathbf{D}}_{k+1}^j{\mathcal{R}}_{k+1}({\mathbf{D}}_{k+1}^j{)}^{\mathrm{T}}\text{\hspace{1em}(II-2-7)}$$
${\bar{\mathcal{R}}}_{k+1}^j$ 也是基于"点" ${\mathbf{x}}_{k+1|k+1}^j$ 有关联, 故有上标 $j$.

结合式 (II-2-1) 和式 (II-2-3) 可知
$$\begin{array}{rl}{\mathbf{z}}_{k+1}& =\mathbf{h}({\mathbf{x}}_{k+1|k+1}^j,0)⊞{\mathbf{r}}_{k+1}^j\\ \Rightarrow \underset{测量值}{\underline{{\mathbf{z}}_{k+1}}}& =\underset{确定值}{\underline{\mathbf{h}({\mathbf{x}}_{k+1|k+1}^j,0)}}⊞(\underset{测量随机噪声}{\underline{{\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}}}+{\mathbf{H}}_{k+1}^j\underset{随机参数}{\underline{\delta {\mathbf{x}}_j}})\end{array}\text{\hspace{1em}(II-2-8)}$$
实际上, 测量随机噪声${\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}$ 和作为随机参数 $\delta {\mathbf{x}}_j$ 之间是相互独立的. 故由式 (II-2-6) 我们可以知道, 在某个随机参数 $\delta {\mathbf{x}}_j$ 下, 测量数据 ${\mathbf{z}}_{k+1}$ 的条件概率, 受随机测量噪声的影响.
$$P({\mathbf{z}}_{k+1}|\delta {\mathbf{x}}_j)=P({\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1})={\mathcal{N}({\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1})|}_{\mathcal{N}(0,{\bar{\mathcal{R}}}_{k+1})}\text{\hspace{1em}(II-2-9)}$$
其中 ${\mathcal{N}({\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1})|}_{\mathcal{N}(0,{\bar{\mathcal{R}}}_{k+1})}$ 表示如果随机变量满足正态分布 $\mathcal{N}(0,{\bar{\mathcal{R}}}_{k+1}^j)$, 当随机变量的值为 ${\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}$ 时的概率密度值. 在清楚知道对应的正态概率分布情况下, 可简写为 $\mathcal{N}({\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1})$.

我们将 $P({\mathbf{z}}_{k+1}|\delta {\mathbf{x}}_j)$ 作为似然概率.

---

3. 误差投影与先验概率

为了运用贝叶斯公式, 下面我们需要计算先验概率 $\delta {\mathbf{x}}_j$.

几何上, 要实现式 (II-1-1) 中初始误差和式 (II-2-6) 中测量残差之间的融合, 我们需要将误差投影到相同的局部切空间.

由式 (I-1-1) 及式 (II-2-2) 可知
$$\delta {\mathbf{x}}_{k+1|k}={\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k}=({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\text{\hspace{1em}(II-3-1)}$$
对 $\delta {\mathbf{x}}_{k+1|k}$ 在 $\delta {\mathbf{x}}_j=0$ 处进行一阶泰勒展开, 得到
$$\delta {\mathbf{x}}_{k+1|k}=({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})+{\frac{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)}{\partial \delta {\mathbf{x}}_j}|}_{\delta {\mathbf{x}}_j=0}\cdot \delta {\mathbf{x}}_j\text{\hspace{1em}(II-3-2)}$$
我们这里要求的是由 $\delta {\mathbf{x}}_{k+1|k}$ 推演得到的 $\delta {\mathbf{x}}_j$ 及其误差分布. 故我们式 (II-3-2) 要变为
$$\delta {\mathbf{x}}_j={\frac{\partial \delta {\mathbf{x}}_j}{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)}|}_{\delta {\mathbf{x}}_j=0}\left[\delta {\mathbf{x}}_{k+1|k}-({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})\right]\text{\hspace{1em}(II-3-3)}$$
式 (II-3-3) 描述了 $\delta {\mathbf{x}}_{k+1|k}$ 向 $\delta {\mathbf{x}}_j$ 的几何投影.

我们先定义投影的 Jacobian, 并按照上一篇博客中 $⊞$ / $\boxminus$ 的定义及运算规则凑成标准形式
$$\begin{array}{rl}{\mathbf{J}}_{k+1}^j& \triangleq {\frac{\partial \delta {\mathbf{x}}_j}{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)}|}_{\delta {\mathbf{x}}_j=0}\\ \left[(\mathbf{x}⊞\mathbf{u})\boxminus \mathbf{x}=\mathbf{u}\right]& ={\frac{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k+1}^j\right)}{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)}|}_{\delta {\mathbf{x}}_j=0}\\ \left[\mathbf{x}⊞(\mathbf{y}\boxminus \mathbf{x})=\mathbf{y}\right]& ={\frac{\partial \left(\left[{\mathbf{x}}_{k+1|k}⊞\left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)\right]\boxminus {\mathbf{x}}_{k+1|k+1}^j\right)}{\partial \left(({\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j)\boxminus {\mathbf{x}}_{k+1|k}\right)}|}_{\delta {\mathbf{x}}_j=0}\\ \left[\delta {\mathbf{x}}_j=0\right]& =\frac{\partial \left(\left[{\mathbf{x}}_{k+1|k}⊞\left({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k}\right)\right]\boxminus {\mathbf{x}}_{k+1|k+1}^j\right)}{\partial \left({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k}\right)}\\ & ={\frac{\partial \left(\left(\left(\mathbf{x}⊞\mathbf{u}\right)\oplus \mathbf{v}\right)\boxminus \mathbf{y}\right)}{\partial \mathbf{u}}|}_{\mathbf{x}={\mathbf{x}}_{k+1|k};\mathbf{u}={\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k};\mathbf{v}=0;\mathbf{y}={\mathbf{x}}_{k+1|k+1}^j}\end{array}\text{\hspace{1em}(II-3-4)}$$
由式 (II-3-1) 可知, 这个 Jacobian 更简洁的等价形式为
$${\mathbf{J}}_{k+1}^j\triangleq {\frac{\partial \delta {\mathbf{x}}_j}{\partial \delta {\mathbf{x}}_{k+1|k}}|}_{\delta {\mathbf{x}}_j=0}\text{\hspace{1em}(II-3-5)}$$

那么式 (II-3-3) 可以写成
$$\begin{array}{rl}\delta {\mathbf{x}}_j& ={\mathbf{J}}_{k+1}^j\left[\delta {\mathbf{x}}_{k+1|k}-({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})\right]\\ & \sim \mathcal{N}\left(-{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k}),{\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}\right)\\ & \triangleq \mathcal{N}({\mathbf{\mu }}_1,{\mathbf{\Sigma }}_1)\end{array}\text{\hspace{1em}(II-3-6)}$$

其中 $-{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})$ 是 $\delta {\mathbf{x}}_j$ 分布的期望/均值, 其实就是 $\delta {\mathbf{x}}_j$ 的预测值.

故先验概率
$$P(\delta {\mathbf{x}}_j)={\mathcal{N}(\delta {\mathbf{x}}_j)|}_{\mathcal{N}\left(-{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k}),{\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}\right)}\text{\hspace{1em}(II-3-7)}$$
简写为
$$P(\delta {\mathbf{x}}_j)=\mathcal{N}(\delta {\mathbf{x}}_j)\text{\hspace{1em}(II-3-8)}$$

此处 $\delta {\mathbf{x}}_j$ 的分布是通过误差状态 $\delta {\mathbf{x}}_{k+1|k}$ 的预测分布得来的, 是先验概率.

---

4. 最大后验估计与最小二乘法

迭代误差状态 $\delta {\mathbf{x}}_j$ 相对于 $k+1$ 时刻测量值 ${\mathbf{z}}_{k+1}$ 的后验概率 (Posteriori) 定义为 $P(\delta {\mathbf{x}}_j|{\mathbf{z}}_{k+1})$.

根据最大后验估计式 (I-10), 并代入已求得的先验概率式 (II-3-8) 和似然概率式 (II-2-9), 得到 $\delta {\mathbf{x}}_j$ 最大后验概率/分布.
$$\begin{array}{rl}\underset{\delta {\mathbf{x}}_j}{\mathrm{argmax}}P(\delta {\mathbf{x}}_j|{\mathbf{z}}_{k+1})& =\underset{\delta {\mathbf{x}}_j}{\mathrm{argmaxlog}}\left(P(\delta {\mathbf{x}}_j)\cdot P({\mathbf{z}}_{k+1}|\delta {\mathbf{x}}_j)\right)\\ & =\underset{\delta {\mathbf{x}}_j}{\mathrm{argmaxlog}}\left(\mathcal{N}(\delta {\mathbf{x}}_j)\cdot \mathcal{N}({\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1})\right)\\ & =\underset{\delta {\mathbf{x}}_j}{\mathrm{argmin}}\left(\Vert \underset{\mathrm{state}}{\underline{\delta {\mathbf{x}}_j}}+\underset{-\text{prediction of state}}{\underline{{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k}}}){\Vert }_{{\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}}^2+\Vert \underset{\mathrm{measurement}}{\underline{{\mathbf{r}}_{k+1}^j}}-\underset{\text{pred. of meas.}}{\underline{{\mathbf{H}}_{k+1}^j\delta {\mathbf{x}}_j}}{\Vert }_{{\bar{\mathcal{R}}}_{k+1}^j}^2\right)\end{array}\text{\hspace{1em}(II-4-1)}$$

其中 $\Vert \mathbf{x}{\Vert }_{\mathbf{A}}^2={\mathbf{x}}^{\mathrm{T}}{\mathbf{A}}^{-1}\mathbf{x}$.

这样最大后验估计就转换成了一个最小二乘问题. 从最小二乘的代价函数的两部分的意义也可以看出确实是关于预测和测量的融合.

进一步, 论文 “The iterated Kalman filter update as a Gauss-Newton method”^[4] 中揭示了迭代卡尔曼滤波的迭代更新过程本质就是最小二乘问题的高斯-牛顿求解过程^[4].

下文将要得到的流形上的卡尔曼滤波的迭代测量更新的每一步, 是一个关于迭代误差状态的最大后验估计 (等价于增强观测变量的最大似然估计); 而整个迭代测量更新过程, 等价于高斯-牛顿法的计算收敛过程.

---

III. 迭代更新计算

1. 状态迭代更新

根据论文 [4] 中的推导, 能够得到 “I.预备知识”-“1. 迭代扩展卡尔曼滤波的测量更新” 中的内容, 具体证明计算我们不展开. 我们从该论文的结论出发, 对比和计算流形上的迭代测量更新.

Table 1. 迭代更新对比计算表

	标准迭代卡尔曼滤波的测量更新[4]	流形上的迭代卡尔曼滤波的测量更新[1]
Augmented Observation	$Z=\left[\begin{array}{c}z\\ \hat{x}\end{array}\right]$	$Z=\left[\begin{array}{c}{\mathbf{r}}_{k+1}^i\\ -{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})\end{array}\right]$
Augmented Measurement	$g(x)=\left[\begin{array}{c}h(x)\\ x\end{array}\right]$	$\mathbf{g}(\delta {\mathbf{x}}_j)=\left[\begin{array}{c}{\mathbf{H}}_{k+1}^j\delta {\mathbf{x}}_j\\ \delta {\mathbf{x}}_j\end{array}\right]$
Covariance of $Z$	$\mathcal{Q}=\left[\begin{array}{cc}R& 0\\ 0& P\end{array}\right]$	$\mathcal{Q}=\left[\begin{array}{cc}{\bar{\mathcal{R}}}_{k+1}^j& 0\\ 0& {\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}\end{array}\right]$
Linearization of $g(x)$	$G=g^{\prime }({\hat{x}}^{+})=\left[\begin{array}{c}H\\ I\end{array}\right]$, $H=h^{\prime }({\hat{x}}^{+})$	$\mathbf{G}=\left[\begin{array}{c}{\mathbf{H}}_{k+1}^j\\ \mathbf{I}\end{array}\right]$
	$H^{\ast }{R}^{-1}H+P^{-1}$	${\mathbf{Q}}_{k+1}^j={({\mathbf{H}}_{k+1}^j)}^{\mathrm{T}}{({\bar{\mathcal{R}}}_{k+1}^j)}^{-1}{\mathbf{H}}_{k+1}^j+({\mathbf{J}}_{k+1}^j{)}^{-\mathrm{T}}{\mathbf{P}}_{k+1|k}^{-1}({\mathbf{J}}_{k+1}^j{)}^{-1}$
	$HP{H}^{\ast }+R$	${\mathbf{S}}_{k+1}^j={\mathbf{H}}_{k+1}^j{\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}({\mathbf{H}}_{k+1}^j{)}^{\mathrm{T}}+{\bar{\mathcal{R}}}_{k+1}^j$
Kalman Gain	$K=(H^{\ast }{R}^{-1}H+P^{-1}{)}^{-1}{H}^{\ast }{R}^{-1}$	${\mathbf{K}}_{k+1}^j=({\mathbf{Q}}_{k+1}^j{)}^{-1}({\mathbf{H}}_{k+1}^j{)}^{\mathrm{T}}{({\bar{\mathcal{R}}}_{k+1}^j)}^{-1}$
Kalman Gain	$K=P{H}^{\ast }(HP{H}^{\ast }+R{)}^{-1}$	${\mathbf{K}}_{k+1}^j={\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}({\mathbf{H}}_{k+1}^j{)}^{\mathrm{T}}({\mathbf{S}}_{k+1}^j{)}^{-1}$
Iterated State	$x_{i+1}=\hat{x}+K_i(z-h(x_i)-H_i(\hat{x}-x_i))$	$\begin{array}{rl}\delta {\mathbf{x}}_j^o& =-{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})\\ & +{\mathbf{K}}_{k+1}^j\left({\mathbf{r}}_{k+1}^i+{\mathbf{H}}_{k+1}^j{\mathbf{J}}_{k+1}^j({\mathbf{x}}_{k+1|k+1}^j\boxminus {\mathbf{x}}_{k+1|k})\right)\end{array}$
Difinition of Covariance of Iterated State	$P^{+}=\mathrm{E}[(x-x_{i+1})(x-x_{i+1}{)}^{\mathrm{T}}]$	${\mathbf{P}}_{k+1}^j=\mathrm{E}\left[(\delta {\mathbf{x}}_j-\delta {\mathbf{x}}_j^o)(\delta {\mathbf{x}}_j-\delta {\mathbf{x}}_j^o{)}^{\mathrm{T}}\right]$
Covariance of Iterated State	$P^{+}=(H^{\ast }{R}^{-1}H+P^{-1}{)}^{-1}$	${\mathbf{P}}_{k+1}^j=({\mathbf{Q}}_{k+1}^j{)}^{-1}$
Covariance of Iterated State	$P^{+}=(I-KH)P$	${\mathbf{P}}_{k+1}^j=\left(\mathbf{I}-{\mathbf{K}}_{k+1}^j{\mathbf{H}}_{k+1}^j\right){\mathbf{J}}_{k+1}^j{\mathbf{P}}_{k+1|k}({\mathbf{J}}_{k+1}^j{)}^{\mathrm{T}}$

根据标准的迭代卡尔曼滤波的测量更新, 每一步迭代都得到关于迭代误差状态 $\delta {\mathbf{x}}_j$ 的优化的解 $\delta {\mathbf{x}}_j^o$, $\delta {\mathbf{x}}_j^o$ 是对 $\delta {\mathbf{x}}_j$ 的最优估计. 我们可以发现流形上的迭代卡尔曼滤波的测量更新计算中, 都是基于当前获得的系统状态的迭代估计 ${\mathbf{x}}_{k+1|k+1}^j$ 展开. 即使是隐含的 ${\mathbf{J}}_{k+1}^j$ (式 (II-3-4)) 和 ${\mathbf{H}}_{k+1}^j$ (式 (II-2-4)) 都是如此. 也就是说, 每获得一个迭代误差状态的最优估计值 $\delta {\mathbf{x}}_j^o$, 需要获得一个新的迭代系统状态的估计值 ${\mathbf{x}}_{k+1|k+1}^{j+1}$. 然后再基于这个新的迭代系统状态估计值 ${\mathbf{x}}_{k+1|k+1}^{j+1}$, 继续计算获得下一个最优的迭代误差状态 $\delta {\mathbf{x}}_{j+1}^o$, 周而复始知道满足停止条件 (迭代收敛或者最大迭代步数).

根据迭代状态误差的定义式 (II-2-2), 可知
$${\mathbf{x}}_{k+1}={\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j\text{\hspace{1em}(III-1-1)}$$
由 Table 1 已获得了迭代误差状态 $\delta {\mathbf{x}}_j$ 的在 ${\mathbf{x}}_{k+1|k+1}^j$ 条件下的最优估计 $\delta {\mathbf{x}}_j^o$, 通过上式获得新的迭代系统状态估计值
$${\mathbf{x}}_{k+1|k+1}^{j+1}={\mathbf{x}}_{k+1|k+1}^j⊞\delta {\mathbf{x}}_j^o\text{\hspace{1em}(III-1-2)}$$
那么 $\delta {\mathbf{x}}_j^o$ 也可视作连续两个迭代系统状态之间的差,
$$\delta {\mathbf{x}}_j^o={\mathbf{x}}_{k+1|k+1}^{j+1}\boxminus {\mathbf{x}}_{k+1|k+1}^j\text{\hspace{1em}(III-1-3)}$$
当 $j=\gamma \ge 0$, 达到停止条件时, 此时得到 $\delta {\mathbf{x}}_{\gamma }^o$ 和相应的协方差 ${\mathbf{P}}_{k+1}^{\gamma }$, 以及系统状态的迭代估计 ${\mathbf{x}}_{k+1|k+1}^{\gamma +1}$.

针对 $k+1$ 时刻测量采样而进行的迭代测量更新过程中, 停止迭代更新时的迭代步数为 $\gamma$.

后面我们在可视化整个迭代测量更新的过程中, 我们更愿意视 $\delta {\mathbf{x}}_j^o$ 为第 $j$ 个迭代状态 ${\mathbf{x}}_{k+1|k+1}^j$ 向第 $j+1$ 个迭代状态 ${\mathbf{x}}_{k+1|k+1}^{j+1}$ 移动的"驱动力". 在 $\delta {\mathbf{x}}_j^o$ 驱动下, 迭代状态越来越接近于真实状态值.

---

2. 协方差重新投影

将迭代过程最终得到的系统状态的迭代估计值 ${\mathbf{x}}_{k+1|k+1}^{\gamma +1}$ 作为 $k+1$ 时刻的最终估计值, 即
$${\mathbf{x}}_{k+1|k+1}={\mathbf{x}}_{k+1|k+1}^{\gamma +1}\text{\hspace{1em}(III-2-1)}$$
有了状态估计值以后自然想到要计算对应的误差协方差, 即
$${\mathbf{P}}_{k+1|k+1}\triangleq \mathrm{E}\left[({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1})({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}{)}^{\mathrm{T}}\right]\text{\hspace{1em}(III-2-2)}$$
由上式可知 ${\mathbf{P}}_{k+1|k+1}$ 是 ${\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}$ 的协方差. ${\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}$ 处于系统状态流形 ${\mathcal{M}}_s$ 上点 ${\mathbf{x}}_{k+1|k+1}$ 附近的局部同胚空间 (“切空间”).

作为对比, 迭代过程中得到 ${\mathbf{P}}_{k+1}^{\gamma }$ 是 $\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o$ 的协方差. 结合式 (II-2-2) 和式 (III-1-3) 可知
$$\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o=({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma })-({\mathbf{x}}_{k+1|k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma })\text{\hspace{1em}(III-2-3)}$$
由上式 (III-2-3) 可知, $\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o$ 处于系统状态流形 ${\mathcal{M}}_s$ 上点 ${\mathbf{x}}_{k+1|k+1}^{\gamma }$ 附近的局部同胚空间 (“切空间”). ${\mathbf{P}}_{k+1|k+1}$ 和 ${\mathbf{P}}_{k+1}^{\gamma }$ 处于同一流形上但不同点的"切空间"上.

因为流形并不是 “直的”, 不能像欧几里得空间那样具有平移不变性, 故
$$({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma })-({\mathbf{x}}_{k+1|k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma })\ne {\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}\text{\hspace{1em}(III-2-4)}$$
所以要作重新投影, 使得两个误差协方差处于同一空间, 进而使得能够将未知的 ${\mathbf{P}}_{k+1|k+1}$ 用已知的 ${\mathbf{P}}_{k+1}^{\gamma }$ 来进行表示.

我们从误差状态的定义 (式(I-1-1) 和式 (II-2-2)) 出发
$$\begin{array}{rlrl}\gamma +1& :& \delta {\mathbf{x}}_{k+1|k+1}& ={\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}\\ \gamma & :& \delta {\mathbf{x}}_{\gamma }& ={\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma }\end{array}\text{\hspace{1em}(III-2-5)}$$
推导得到
$$\delta {\mathbf{x}}_{k+1|k+1}=({\mathbf{x}}_{k+1|k+1}^{\gamma }⊞\delta {\mathbf{x}}_{\gamma })\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma +1}\text{\hspace{1em}(III-2-6)}$$
将 $\delta {\mathbf{x}}_{k+1|k+1}$ 看作是以 $\delta {\mathbf{x}}_{\gamma }$ 为自变量的函数, 并在 $\delta {\mathbf{x}}_{\gamma }^o$ 处进行一阶泰勒展开, 得到
$$\begin{array}{rl}\delta {\mathbf{x}}_{k+1|k+1}& =\underset{=0\text{eq. (III-1-1)}}{\underline{({\mathbf{x}}_{k+1|k+1}^{\gamma }⊞\delta {\mathbf{x}}_{\gamma }^o)\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma +1}}}+{\frac{\partial \left(({\mathbf{x}}_{k+1|k+1}^{\gamma }⊞\delta {\mathbf{x}}_{\gamma })\boxminus {\mathbf{x}}_{k+1|k+1}^{\gamma +1}\right)}{\partial \delta {\mathbf{x}}_{\gamma }}|}_{\delta {\mathbf{x}}_{\gamma }=\delta {\mathbf{x}}_{\gamma }^o}\cdot (\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o)\\ & ={\mathbf{L}}_{k+1}\cdot (\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o)\end{array}\text{\hspace{1em}(III-2-7)}$$
其中 $\delta {\mathbf{x}}_{k+1|k+1}$ 相对于 $\delta {\mathbf{x}}_{\gamma }$ 在"点" $\delta {\mathbf{x}}_{\gamma }^o$ 上的 Jacobian 记为 ${\mathbf{L}}_{k+1}$.
$${\mathbf{L}}_{k+1}={\frac{\partial \left(((\mathbf{x}⊞\mathbf{u})\oplus \mathbf{v})\boxminus \mathbf{y}\right)}{\partial \mathbf{u}}|}_{\mathbf{x}={\mathbf{x}}_{k+1|k+1}^{\gamma };\mathbf{u}=\delta {\mathbf{x}}_{\gamma }^o;\mathbf{v}=0;\mathbf{y}={\mathbf{x}}_{k+1|k+1}^{\gamma +1}}\text{\hspace{1em}(III-2-8)}$$
式 (III-2-7) 就实现了 $(\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o)$ 向 $\delta {\mathbf{x}}_{k+1|k+1}$ 的投影转换. 也就可以从误差协方差 ${\mathbf{P}}_{k+1|k+1}$ 定义式 (III-2-2) 开始推导了
$$\begin{array}{rl}{\mathbf{P}}_{k+1|k+1}& =\mathrm{E}\left[({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1})({\mathbf{x}}_{k+1}\boxminus {\mathbf{x}}_{k+1|k+1}{)}^{\mathrm{T}}\right]\\ \text{(III-2-5)}& =\mathrm{E}\left[(\delta {\mathbf{x}}_{k+1|k+1})(\delta {\mathbf{x}}_{k+1|k+1}{)}^{\mathrm{T}}\right]\\ & =\mathrm{E}\left[{\mathbf{L}}_{k+1}(\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o)(\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o{)}^{\mathrm{T}}{\mathbf{L}}_{k+1}^{\mathrm{T}}\right]\\ & ={\mathbf{L}}_{k+1}\mathrm{E}\left[(\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o)(\delta {\mathbf{x}}_{\gamma }-\delta {\mathbf{x}}_{\gamma }^o{)}^{\mathrm{T}}\right]{\mathbf{L}}_{k+1}^{\mathrm{T}}\\ \text{(Table 1)}& ={\mathbf{L}}_{k+1}{\mathbf{P}}_{k+1}^{\gamma }{\mathbf{L}}_{k+1}^{\mathrm{T}}\end{array}\text{\hspace{1em}(III-2-9)}$$

---

IV. 融合与更新的几何示意

不管是KF、EKF、IKF 还是这里讨论的 IKFoM, 测量更新部分都是值得去仔细分析和理解的.

前面已经从公式上严格推导了整个迭代更新的过程, 包括状态值、误差值和误差协方差的迭代更新. 我们再从几何直观的角度, 可视化描述一下这个过程.

先补充部分内容:

通过对式 (II-2-6) 进行变换, 得到由测量值推导出的迭代误差状态 $\delta {\mathbf{x}}_j$ 的分布
$$\begin{array}{rl}\delta {\mathbf{x}}_j& =({\mathbf{H}}_{k+1}^j{)}^{-1}{\mathbf{r}}_{k+1}^j-({\mathbf{H}}_{k+1}^j{)}^{-1}{\mathbf{D}}_{k+1}^j{\mathbf{v}}_{k+1}\\ & \sim \mathcal{N}\left(({\mathbf{H}}_{k+1}^j{)}^{-1}{\mathbf{r}}_{k+1}^j,({\mathbf{H}}_{k+1}^j{)}^{-1}{\bar{\mathcal{R}}}_{k+1}({\mathbf{H}}_{k+1}^j{)}^{-\mathrm{T}}\right)\\ & \triangleq \mathcal{N}({\mathbf{\mu }}_2,{\mathbf{\Sigma }}_2)\end{array}\text{\hspace{1em}(IV-1)}$$
由式 (II-2-2) 可知, $\delta {\mathbf{x}}_j$ 所在"切平面"与流形 ${\mathcal{M}}_s$ 上点 ${\mathbf{x}}_{k+1|k+1}^j$ 附近区域局部同胚. 式 (II-2-6) 以及上式描述的分布是与点 ${\mathbf{x}}_{k+1|k+1}^j$ 关联的.

需要说明: 上式中 $({\mathbf{H}}_{k+1}^j{)}^{-1}$ 并非必然存在, 而由具体的测量函数决定. 上式只是为了方便说明几何关系而构建, 并非严格的计算式, 实际计算中也不会用到 $({\mathbf{H}}_{k+1}^j{)}^{-1}$ 的值.

Fig. 1. 测量迭代更新过程的可视化描述

比照 Fig. 1., 流形上的迭代卡尔曼滤波 (IKFoM) 的迭代测量更新过程描述如下.

迭代卡尔曼滤波 (IKFoM) 的迭代测量更新的流程步骤

Step 1:

​ 初始化

​ 初始迭代步 $j=0$

​ 初始系统状态 ${\mathbf{x}}_{k+1|k+1}^0={\mathbf{x}}_{k+1|k}$

​ 初始误差状态分布 $\delta {\mathbf{x}}_{k+1|k}\sim \mathcal{N}(0,{\mathbf{P}}_{k+1|k})$

​ 预设收敛小量 $\epsilon$ 和最大迭代步 $j_{\max }$

Step 2:

​ 当 $j=0$ 时, 以 ${\mathbf{x}}_{k+1|k+1}^0$ 作为 “迭代点”/“评估点”

​ 从式 (II-2-1) 计算测量残差 ${\mathbf{r}}_{k+1}^0$

​ 从式 (II-2-4) 计算 ${\mathbf{H}}_{k+1}^0$

​ 从式 (II-2-5) 计算 ${\mathbf{D}}_{k+1}^0$

​ 从式 (II-2-7) 计算 ${\mathcal{R}}_{k+1}^0$

​ 从式 (II-3-4) 计算 ${\mathbf{J}}_{k+1}^0$

​ 利用 Table 1 中各算式, 计算 ${\mathbf{Q}}_{k+1}^0$、${\mathbf{S}}_{k+1}^0$、${\mathbf{K}}_{k+1}^0$

​ 利用 Table 1 中 $\delta {\mathbf{x}}_j^o$ 的计算式, 计算出 $\delta {\mathbf{x}}_0^o={\mathbf{K}}_{k+1}^0{\mathbf{r}}_{k+1}^0$ 的具体值

​ 从式 (III-1-2) 计算新的 “迭代点”/“评估点” ${\mathbf{x}}_{k+1|k+1}^1$

​ 评估停止条件 $\Vert \delta {\mathbf{x}}_0^o\Vert <\epsilon$ 或者 $0\ge j_{\max }$

​ \quad 如上述停止条件不满足, 进入 Step 3

​ \quad 如上述停止条件满足, 进入 Step 4

Step 3:

​ 迭代步数加一为 $j=j+1$ ,

​ 计算 ${\mathbf{r}}_{k+1}^j$、${\mathbf{H}}_{k+1}^j$、${\mathbf{D}}_{k+1}^j$、${\bar{\mathcal{R}}}_{k+1}^j$、${\mathbf{J}}_{k+1}^j$、${\mathbf{Q}}_{k+1}^j$、${\mathbf{S}}_{k+1}^j$、${\mathbf{K}}_{k+1}^j$

​ 利用 Table 1 中 $\delta {\mathbf{x}}_j^o$ 的计算式, 计算出 $\delta {\mathbf{x}}_j^o$ 的具体值

​ 从式 (III-1-2) 计算下一个 “迭代点”/“评估点” ${\mathbf{x}}_{k+1|k+1}^{j+1}$

​ 评估停止条件 $\Vert \delta {\mathbf{x}}_j^o\Vert <\epsilon$ 或者 $j\ge j_{\max }$

​ \quad 如上述停止条件不满足, 重新进入 Step 3

​ \quad 如上述停止条件满足, 进入 Step 4

Step 4:

​ 停止步数 $\gamma =j$

​ 最优估计的系统状态 ${\mathbf{x}}_{k+1|k+1}={\mathbf{x}}_{k+1|k+1}^{j+1}$

​ 基于 Step 3 中已得到 ${\mathbf{H}}_{k+1}^j$、${\mathbf{J}}_{k+1}^j$、${\mathbf{K}}_{k+1}^j$, 并利用 Table 1 中 ${\mathbf{P}}_{k+1}^j$ 的计算式, 计算该值

​ 从式 (III-2-8) 计算 ${\mathbf{L}}_{k+1}$

​ 从式 (III-2-9) 计算 ${\mathbf{P}}_{k+1|k+1}$

Step 5:

​ 输出 ${\mathbf{x}}_{k+1|k+1}$、${\mathbf{P}}_{k+1|k+1}$

从 Fig. 1 中可以看出

- 每一步迭代更新都得到一个最大后验估计 $\delta {\mathbf{x}}_j^o$ ;

- 最大后验估计值 $\delta {\mathbf{x}}_j^o$ 又去驱动系统状态 ${\mathbf{x}}_{k+1|k+1}^j$ 的一步更新;

- 迭代状态 ${\mathbf{x}}_{k+1|k+1}^j$ ($j=0,1,2,...,\gamma ,\gamma +1$) 形成一条状态流形上的迭代更新轨迹;

- 每一个迭代状态都有相关联的同胚"切平面", 误差和协方差的计算都在关联的同胚"切平面"上进行;

- 不同"切平面"上的误差状态及协方差可以通过投影操作 (Projection & Reprojection) 实现相互转换.

---

总结

以上完成了流形上的迭代卡尔曼滤波算法中迭代测量更新部分的讨论, 重点关注

- 迭代更新的"驱动力"

- 迭代更新的几何描述

- 迭代更新的最优化特性

- 误差状态与系统状态之间的局部同胚

- 不同"切平面"上状态与协方差的投影

- 迭代更新的算法流程

如有问题, 请指出, 谢谢!

---

参考文献

[1] Dongjiao He, Wei Xu, Fu Zhang, “Kalman Filters on Differentiable Manifolds”, https://arxiv.org/abs/2102.03804, 2021, Version V3

[2] Jindřich Havlík and Ondřej Straka, Performance evaluation of iterated extended Kalman filter with variable step-length, J. Phys.: Conf. Ser. 659, 2015

[3] Fawcett Lee, http://www.mas.ncl.ac.uk/~nlf8/teaching/mas2317/notes/chapter2.pdf, in the course Introduction to Bayesian Statistics, at Newcastle University, 2014-2015

[4] B. M. Bell, F. W. Cathey, The iterated Kalman filter update as a Gauss-Newton method. IEEE Transactions on Automatic Control, 38(2), 294–297, 1993

---