一文看尽4种SLAM中零空间的维护方法

 

---

写在前面

笔者最近在总结整个MSCKF的相关知识的时候，算是比较仔细的推导了与零空间相关的部分，放在之前笔者会认为这部分知识确实是比较难理解的部分，但是花了很多时间总结发现其实都还是有很强的相关性的，所以这里笔者算是总结一下这部分的内容，算是对近来知识的一个总结。

本文主要会涉及4种零空间的维护方法，分别是：

1. First-Estimate-Jacobian，该方法也是用途最广的维护能观性的方法，可以适用于EKF-base和Graph-base的SLAM系统中；
2. Observability-Constrainted 方法，该方法用途可能不多，但其有自身的优势在，且是开源的S-MSCKF中使用的方法；
3. DSO中关于零空间维护的方法，其中不仅使用了FEJ技术，同时对增量方程和求解的增量都做了去除零空间变量的影响；
4. VINS中关于零空间维护的方法，该方法主要是数值上的维护；

 

---

Reference

1. https://zhuanlan.zhihu.com/p/304889273 笔者关于FEJ 的相关总结；
2. https://zhuanlan.zhihu.com/p/328940891 笔者关于OC-KF的相关总结；
3. https://blog.csdn.net/wubaobao1993/article/details/105106301 笔者关于DSO对于零空间维护的相关总结，该总结现下看来是不太对的；
4. https://blog.csdn.net/wubaobao1993/article/details/108354488 笔者关于VINS对于零空间维护的相关总结；
5. Consistency of EKF-Based Visual-Inertial Odometry. MSCKF能观性的分析论文；

 

---

EKF-Base方法对零空间的维护

Notation:

1. 在以下推导中，所有的旋转均使用JPL表示法；
2. 在以下推导中，变量均选择为旋转，位置和速度，即$\mathrm{x}={\left[\begin{array}{ccc}{}_G^{\ell }q& {}^G{p}_{\ell }& {}^G{v}_{\ell }\end{array}\right]}^T$；

从前有两位年轻人都致力于EKF-Base（本文更多的是针对MSCKF方法）方法中能观性的分析，但是都遇到了很大的问题。于是两人约定一起到山上去请教一位德高望重的老师。

老师：你二人既然都是研究能观性的分析，那么一定知道能观性分析的评价标准是能观性矩阵吧？
$$\mathcal{O}=\left[\begin{array}{c}{\mathrm{H}}_0\\ {\mathrm{H}}_1{\Phi }_0\\ ⋮\\ {\mathrm{H}}_t{\Phi }_{t-1}\dots {\Phi }_0\end{array}\right]\text{\hspace{1em}(1)}$$
两位年轻人：自然是知道的，对于线性系统而言，系统的能观性主要由能观性矩阵反应。

老师：嗯，很好，既然如此，我就给二位一人三个锦囊，希望你二人能在各自的研究方向上都能悟道。

两位年轻人接过锦囊，开心的下山了。

 

First-Estimate-Jacobian方法

第一位年轻人下山之后便回到了家，继续开始对于能观性的分析，他迫不及待的打开第一个锦囊，上面写着一行字和如下的公式：

锦囊1. 不依赖IMU的运动方程，从头推导理想情况下的状态传递方程

$$\begin{array}{rl}& \{\begin{array}{ll}{}_G^{I_{t+1}}R& ={}_{I_t}^{I_{t+1}}R{}_G^{I_t}R\\ {}_G^{I_t}R& =(\mathbf{I}-{\left[{}^{I_t}\tilde{\theta }\right]}_{\times }){}_G^{I_t}\hat{R}\end{array}\\ & \{\begin{array}{ll}{}^G{v}_{I_{t+1}}& ={}^G{v}_{I_t}+({}_G^{I_t}R{)}^T{\int }_{t_l}^{t_{l+1}}({}_{I_l}^{I_{\tau }}R{)}^T{\mathbf{a}}_{\mathbf{m}}d\tau +g\Delta t\\ {}^G{v}_{I_t}& ={}^G{\hat{v}}_{I_t}+{}^G{\tilde{v}}_{I_t}\\ {}_G^{I_t}R& =(\mathbf{I}-{\left[{}^{I_t}\tilde{\theta }\right]}_{\times }){}_G^{I_t}\hat{R}\end{array}\\ & \{\begin{array}{ll}{}^G{p}_{I_{t+1}}& ={}^G{p}_{I_t}+{}^G{v}_{I_t}\Delta t+({}_G^{I_t}R{)}^T{\int }_{t_l}^{t_{l+1}}{\int }_{t_l}^s({}_{I_t}^{I_{\tau }}R{)}^T{\mathbf{a}}_{\mathbf{m}}d\tau ds+\frac{1}{2}g\Delta t^2\\ {}^G{p}_{I_t}& ={}^G{\hat{p}}_{I_t}+{}^G{\tilde{p}}_{I_t}\\ {}^G{v}_{I_t}& ={}^G{\hat{v}}_{I_t}+{}^G{\tilde{v}}_{I_t}\\ {}_G^{I_t}R& =(\mathbf{I}-{\left[{}^{I_t}\tilde{\theta }\right]}_{\times }){}_G^{I_t}\hat{R}\end{array}\end{array}\text{\hspace{1em}(2)}$$

年轻人反复看了多遍老师的建议，于是经过一段时日，年轻人终于按照提示推出了新的状态传递方程：
$$\left[\begin{array}{c}{}_G^{I_{t+1}}\tilde{\theta }\\ {}^G{\tilde{p}}_{I_{t+1}}\\ {}^G{\tilde{v}}_{I_{t+1}}\end{array}\right]=\left[\begin{array}{ccc}{}_{I_t}^{I_{t+1}}R& 0& 0\\ -\lfloor {\mathrm{y}}_t{\rfloor }_{\times }({}_G^{I_t}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\mathrm{s}}_t{\rfloor }_{\times }({}_G^{I_t}R{)}^T& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{}_G^{I_t}\tilde{\theta }\\ {}^G{\tilde{p}}_{I_t}\\ {}^G{\tilde{v}}_{I_t}\end{array}\right]+\left[\begin{array}{c}{}_{I_t}^{I_{t+1}}\tilde{\theta }\\ ({}_G^{I_t}R{)}^T\left[{\tilde{\mathrm{y}}}_l\right]\\ ({}_G^{I_t}R{)}^T\left[{\tilde{\mathrm{s}}}_l\right]\end{array}\right]\text{\hspace{1em}(3)}$$
其中

1. 所有的变量都是理想情况下的值，也就是并不涉及到时刻的概念（公式中的 t 和 t+1 更多的表示的递进关系，可以理解为当前的位姿经过IMU激励得到下一个位姿），更进一步的讲，某个变量在预测阶段被预测出来，那么由于预测阶段都是理想的，所以该预测值也是相当理想的，即真值；
2. ${\mathrm{y}}_t={}^G{p}_{t+1}-{}^G{p}_t-{}^G{v}_t\Delta t-\frac{1}{2}g\Delta t^2$；
3. ${\mathrm{s}}_t={}^G{v}_{t+1}-{}^G{v}_t-g\Delta t$；

年轻人觉得很开心，觉得新的状态传递方程一定可以帮助他解开心中的迷惑。但是研究良久，年轻人还是一无所获，于是他又拿出第二个锦囊，想看看老师又给了他怎样的启示，只见上面写道：

 

锦囊2. 使用理想的观测矩阵，从前到后推导能观性矩阵中的某一行，总结出零空间应该是怎么样的

年轻人很聪明，他马上把注意力集中在理想情况下的过程，首先他写出了理想情况下的观测矩阵：
$$\begin{array}{rl}{H}_{(I_l|l)}& =J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}R\left[\begin{array}{ccc}\underset{\partial e/\partial \theta }{{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_i}\right]}_{\times }({}_G^{I_l}R{)}^T}& \underset{\partial e/\partial \mathrm{p}}{-{\mathbf{I}}_{3\times 3}}& \underset{\partial e/\partial \mathrm{v}}{0_{3\times 3}}\end{array}\right]\\ H_{(f_j|l)}& =J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}R\end{array}\text{\hspace{1em}(4)}$$
将公式（4）中的两部分和在一起可得：
$${\mathbf{H}}_{f_j|t}=J_{(f_j|t)}{}_I^{C}R{}_G^{I_t}R\left[\begin{array}{c}\begin{array}{cccc}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_i}{\rfloor }_{\times }({}_G^{I_l}R{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\end{array}\right]\text{\hspace{1em}(5)}$$

于是年轻人按照老师的建议，取出其中一行进行推导，如下：
$${\mathcal{O}}_t={\mathbf{H}}_t\Phi (t,t-1)\dots \Phi (t_1,t_0)\text{\hspace{1em}(6)}$$
将公式（3）和公式（5）带入到公式（6）中可得：
$$\begin{gathered} {\mathcal{O}}_{t|3\times 3}=\left[\begin{array}{ccc}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_t}{\rfloor }_{\times }({}_G^{I_l}R{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{ccc}{}_G^{I_t}R({}_G^{I_{t-1}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-1}{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\mathrm{s}}_{t-1}{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T& 0& \mathbf{I}\end{array}\right] \\ \left[\begin{array}{ccc}{}_G^{I_{t-1}}R({}_G^{I_{t-2}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\mathrm{s}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& 0& \mathbf{I}\end{array}\right]\dots \left[\begin{array}{ccc}{}_G^{I_{t_1}}R({}_{I_G}^{I_{t_0}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t_0}{\rfloor }_{\times }({}_G^{I_{t_0}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\mathrm{s}}_{t_0}{\rfloor }_{\times }({}_G^{I_{t_0}}R{)}^T& 0& \mathbf{I}\end{array}\right]\text{\hspace{1em}(7)} \end{gathered}$$

公式（7）中因为该部分仅涉及到IMU状态部分的分析，因此省去了观测的部分，同时省去了观测矩阵前面的公共部分。

在推导过程中，年轻人发现按照这样乘法下去，在每一次乘法之后都能得到一个很相似的形式，以前面两次乘积为例可以得到：
$$\begin{array}{rl}& \left[\begin{array}{ccc}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_t}{\rfloor }_{\times }({}_G^{I_l}R{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{ccc}{}_G^{I_t}R({}_G^{I_{t-1}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-1}{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t_{t-1}\\ -\lfloor {\mathrm{s}}_{t-1}{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{ccc}{}_G^{I_{t-1}}R({}_G^{I_{t-2}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t_{t-1}\\ -\lfloor {\mathrm{s}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& 0& \mathbf{I}\end{array}\right]\\ & ={\left[\begin{array}{c}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_t}-{\mathrm{y}}_{t-1}-{\mathrm{s}}_{t-1}\Delta t{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}\Delta t_{t-1}\end{array}\right]}^T\left[\begin{array}{ccc}{}_G^{I_{t-1}}R({}_G^{I_{t-2}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t_{t-2}\\ -\lfloor {\mathrm{s}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& 0& \mathbf{I}\end{array}\right]\\ & ={\left[\begin{array}{c}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{t-1}}-{}^G{\mathrm{v}}_{t-1}\Delta t_{t-1}-\frac{1}{2}g\Delta t_{t-1}^2{\rfloor }_{\times }({}_G^{I_{t-1}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}\Delta t_{t-1}\end{array}\right]}^T\left[\begin{array}{ccc}{}_G^{I_{t-1}}R({}_G^{I_{t-2}}R{)}^T& 0& 0\\ -\lfloor {\mathrm{y}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& \mathbf{I}& \mathbf{I}\Delta t_{t-2}\\ -\lfloor {\mathrm{s}}_{t-2}{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T& 0& \mathbf{I}\end{array}\right]\\ & ={\left[\begin{array}{c}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{t-2}}-{}^G{\mathrm{v}}_{t-2}\Delta t_{t-2}^{t-1}-\frac{1}{2}g(\Delta t_{t-2}^{t-1}{)}^2{\rfloor }_{\times }({}_G^{I_{t-2}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}\Delta t_{t-2}^{t-1}\end{array}\right]}^T\end{array}\text{\hspace{1em}(8)}$$
年轻人很快便总结出了规律，于是在 t0 时刻，整个乘积变作（这里把特征部分的元素给添加回来了）：
$${\mathcal{O}}_t={\left[\begin{array}{c}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{t_0}}-{}^G{\mathrm{v}}_{t_0}\Delta t_{t_0}^{t-1}-\frac{1}{2}g(\Delta t_{t_0}^{t-1}{)}^2{\rfloor }_{\times }({}_G^{I_{t_0}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}\Delta t_{t_0}^{t-1}\\ \mathbf{I}\end{array}\right]}^T\text{\hspace{1em}(9)}$$
所以对应的零空间其实就是：
$${\mathbf{N}}_t=\left[\begin{array}{cc}{0}_3& {}_G^{I_{t_0}}\mathbf{R}\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{t_0}\right]}_{\times }\mathbf{g}\\ 0_3& -{\left[{}^G{\mathbf{v}}_{t_0}\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{f_j}\right]}_{\times }\mathbf{g}\end{array}\right]\text{\hspace{1em}(10)}$$
除此之外，年轻人也在推导过程中总结出了一个很重要的规律：之所以乘积最后能满足公式（10）所示的零空间，其比较重要的原因在于在乘积过程中，所有的位姿和速度都用的是一个值（在理想情况下，都使用的是理想值），这保证了相同项之间可以互相抵消。

例如在$\Phi (t,t-1{)}_{11}={}_G^{I_t}R({}_G^{I_{t-1}}R{)}^T$和$\Phi (t-1,t-2{)}_{11}={}_G^{I_{t-1}}R({}_G^{I_{t-2}}R{)}^T$，其中当${}_G^{I_{t-1}}R$使用同样的值时才可以抵消，不至于引入其他项。

于是年轻人回想自己的推导过程，因为在自己的系统中，整个系统维护了一个滑动窗口，该窗口内记录了一段时间内的位姿数据，该数据会因为新观测而被更新，导致后续在构建观测矩阵时一直使用的是最新的值，也就是同样的位姿和速度，使用了两个时刻的值，这样的方法使得推导过程中增加好很多因为使用不同时刻值的扰动项！例如在时刻${\alpha }_{i+1}$ ，则对于第 $\ell$ 个节点（假设该节点的预测时刻也为 $\ell$ 时刻）来说形式如下：

$${\mathcal{O}}_{\ell }^{({\alpha }_{i+1})}={\mathbf{M}}_{\ell }^{(t)}\left[\begin{array}{c}\begin{array}{cccc}{\mathbf{\Gamma }}_{\ell }^{({\alpha }_{i+1})}+\Delta {\mathbf{\Gamma }}_{\ell }^{({\alpha }_{i+1})}& -{\mathbf{I}}_3& -\Delta t_{\ell }{\mathbf{I}}_3& {\mathbf{I}}_3\end{array}\end{array}\right]\text{\hspace{1em}(11)}$$

其中：

1. $\Gamma$ 的形式应与公式（9）中的理想情况相似：
   $${\mathbf{\Gamma }}_{\ell }^{({\alpha }_{i+1})}=\lfloor {}^G{\hat{\mathbf{p}}}_{f_i}-{}^G{\hat{\mathbf{p}}}_{t_0}^{(t_0)}-{}^G{\hat{\mathbf{v}}}_{t_0}^{(t_0)}\Delta t_{\ell }-\frac{1}{2}{}^G\mathbf{g}\Delta t_{\ell }^2\times \rfloor ({}_G^{t_0}{\hat{\mathbf{R}}}^{(t_0)}{)}^T$$

2. $\Delta \Gamma$ 是由于不同时刻线性化造成的扰动项，形式如下：
   $$\begin{array}{rl}\Delta {\mathbf{\Gamma }}_{\ell }^{({\alpha }_{i+1})}=& ({\lfloor {}^G{\hat{\mathbf{p}}}_{f_j}-{}^G{\hat{\mathbf{p}}}_{\ell }^{({\alpha }_i)}\rfloor }_{\times }{\overline{\mathbf{E}}}_{\mathbf{q}}+{\overline{\mathbf{E}}}_{\mathbf{p}}+\sum _{j=t_1}^{\ell -1}(\sum _{s=t_1}^j{\mathbf{E}}_{\mathbf{v}}^s\Delta t+{\mathbf{E}}_{\mathbf{p}}^j+\\ & \sum _{s=t_1}{\mathbf{\Phi }}_{\mathbf{v}\mathbf{q}}\left({\hat{\mathbf{x}}}_{I_{s+1}}^{(s)},{\hat{\mathbf{x}}}_{I_s}^{(s)}\right){}_G^s{\hat{\mathbf{R}}}^{(s)}{\mathbf{E}}_{\mathbf{q}}^s\Delta t+{\Phi }_{\mathbf{p}\mathbf{q}}\left({\hat{\mathbf{x}}}_{I_{j+1}}^{(j)},{\hat{\mathbf{x}}}_{I_j}^{(j)}\right){}_G^j{\hat{\mathbf{R}}}^{(j)}{\mathbf{E}}_{\mathbf{q}}^j))({}_G^k{\hat{\mathbf{R}}}^{(k)}{)}^T\end{array}$$

   其中形如${\hat{\mathrm{X}}}_s^{(s)}$就是在 s 时刻第 s 个节点的值，可以看到其中有较多的同一节点在不同时刻的值，这些值导致了扰动项的产生，具体形式可以参考参考【5】中的公式。

3. $\mathbf{M}$矩阵是一些公共项，这里不多做分析；

 

锦囊3. 对同一变量使用第一次预测出的值作为观测方程的线性化点——FEJ

年轻人从推导理想情况的步骤中得到启发，于是他尝试将同一变量使用同一时刻的值进行实际情况的推导，选择怎样的时刻呢？年轻人有三个选择，以 t 时刻产生的节点为例：

• 该变量被 t-1 时刻的更新值预测出来的值——${\hat{\mathrm{x}}}_t^{(t-1)}$；
• 该变量在 t 时刻更新之后的值——${\hat{\mathrm{x}}}_t^{(t)}$；
• 该变量在之后时刻被更新之后的值——${\hat{\mathrm{x}}}_t^{({\alpha }_i)}$；

最后年轻人选择了第一个，因为在能观性矩阵中涉及到状态转移矩阵$\Phi ({\hat{\mathrm{x}}}_t,{\hat{\mathrm{x}}}_{t-1})$，此时对于 $t$ 节点，仅有刚刚预测出来的值。

于是年轻人对公式做了如下的改动：

1. 将所有的状态转移矩阵做如下变换：
   $$\begin{array}{rl}\hat{\Phi }(t,t-1)& =\left[\begin{array}{ccc}{}_G^{I_t}{\hat{R}}^{(t-1)}({}_G^{I_{t-1}}{\hat{R}}^{(t-1)}{)}^T& 0& 0\\ -\lfloor {\hat{\mathrm{y}}}_t^{(t-1)}{\rfloor }_{\times }({}_G^{I_{t-1}}{\hat{R}}^{(t-1)}{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\hat{\mathrm{s}}}_t^{(t-1)}{\rfloor }_{\times }({}_G^{I_{t-1}}{\hat{R}}^{(t-1)}{)}^T& 0& \mathbf{I}\end{array}\right]\\ \Rightarrow \hat{\Phi }(t,t-1)& =\left[\begin{array}{ccc}{}_G^{I_t}{\hat{R}}^{(t-1)}({}_G^{I_{t-1}}{\hat{R}}^{(t-2)}{)}^T& 0& 0\\ -\lfloor {\hat{\mathrm{y}}}_t^{(t-1)}{\rfloor }_{\times }({}_G^{I_{t-1}}{\hat{R}}^{(t-2)}{)}^T& \mathbf{I}& \mathbf{I}\Delta t\\ -\lfloor {\hat{\mathrm{s}}}_t^{(t-1)}{\rfloor }_{\times }({}_G^{I_{t-1}}{\hat{R}}^{(t-2)}{)}^T& 0& \mathbf{I}\end{array}\right]\end{array}\text{\hspace{1em}(12)}$$
   里面的 $\hat{\mathrm{y}}$ 和 $\hat{\mathrm{s}}$ 中的相应项也要变为FEJ的形式；

2. 将所有的观测矩阵做如下变换：
   $$\begin{array}{rl}{\mathbf{H}}_{f_j|t}& =J_{(f_j|t)}{}_I^{C}R{}_G^{I_t}{\hat{R}}^{({\alpha }_i)}\left[\begin{array}{c}\begin{array}{cccc}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_t}^{({\alpha }_i)}{\rfloor }_{\times }({}_G^{I_l}{\hat{R}}^{({\alpha }_i)}{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\end{array}\right]\\ \Rightarrow {\mathbf{H}}_{f_j|t}& =J_{(f_j|t)}{}_I^{C}R{}_G^{I_t}{\hat{R}}^{(t-1)}\left[\begin{array}{c}\begin{array}{cccc}\lfloor {}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_t}^{(t-1)}{\rfloor }_{\times }({}_G^{I_l}{\hat{R}}^{(t-1)}{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\end{array}\right]\end{array}\text{\hspace{1em}(13)}$$

将上述的改动带入到能观性矩阵中之后，果然消掉了所有的扰动项，得到了形如公式（10）的零空间：
$${\mathbf{N}}_t^{({\alpha }_i)}=\left[\begin{array}{cc}{0}_3& {}_G^{I_{t_0}}{\hat{\mathbf{R}}}^{(t_0)}\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{t_0}^{(t_0)}\right]}_{\times }\mathbf{g}\\ 0_3& -{\left[{}^G{\mathbf{v}}_{t_0}^{(t_0)}\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{f_j}\right]}_{\times }\mathbf{g}\end{array}\right]\text{\hspace{1em}(14)}$$
年轻人很开心，他连忙打开老师给予的第三个锦囊，果然老师的提示与自己的想法是一致的。

以上的详细推导可以看参考【1】

 

---

Observability-Constraint 方法

时间回到第二个年轻人那里，他下了山只有，并没有直接回家，而是去参加了一个朋友主办的研讨会，会议上他了解到如何求解带约束的最优化问题，这个研讨会在后面帮助了他很多。第二个年轻人也从老师的建议中获取了很多；

锦囊1. 根据IMU运动方程，推导误差状态转移方程的闭式解，并用该理想情况下的转移矩阵传递初始的零空间

根据年轻人之前的研究，根据IMU的运动方程推导得到的误差状态微分方程为：
$${\dot{\tilde{\mathbf{X}}}}_{\mathrm{I}\mathrm{M}\mathrm{U}}=\mathbf{F}{\tilde{\mathbf{X}}}_{\mathrm{I}\mathrm{M}\mathrm{U}}+\mathbf{G}{\mathbf{n}}_{\mathrm{I}\mathrm{M}\mathrm{U}}\text{\hspace{1em}(15)}$$
其中：
$$\mathbf{F}=\left[\begin{array}{ccc}-\lfloor \hat{\mathbf{\omega }}\times \rfloor & 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_3& {\mathbf{I}}_{3\times 3}\\ -({}_G^{I_l}R{)}^T\lfloor \widehat{{\mathbf{a}}_{\mathbf{m}}}\times \rfloor & 0_{3\times 3}& 0_{3\times 3}\end{array}\right]$$
这里误差驱动矩阵$\mathbf{G}$不对零空间的分析有贡献，所以暂时就不分析了；

那么根据微分方程，可以得到状态转移方程为：
$$\tilde{\mathrm{X}}\left(t_{l+1}\right)=\mathbf{\Phi }\left(t_{l+1},t_l\right)\tilde{\mathrm{X}}\left(t_l\right)+{\int }_{t_l}^{t_{l+1}}\mathbf{\Phi }\left(t_{l+1},\tau \right)\mathbf{G}(\tau )\mathbf{n}(\tau )\mathrm{d}\tau \text{\hspace{1em}(16)}$$
其中：
$$\{\begin{array}{l}\dot{\mathbf{\Phi }}\left(t_{l+1},t_l\right)=\mathbf{F}(t)\mathbf{\Phi }\left(t_{l+1},t_l\right)\\ \mathbf{\Phi }\left(t_{l+1},t_l\right)=\exp ({\int }_{t_l}^{t_{l+1}}\mathbf{F}(t)\mathrm{d}t)\end{array}\text{\hspace{1em}(17)}$$
根据公式（17）可以列出如下公式：
$$\begin{array}{rl}\left[\begin{array}{ccc}{\dot{\Phi }}_{11}(t)& {\dot{\Phi }}_{12}(t)& {\dot{\Phi }}_{13}(t)\\ {\dot{\Phi }}_{21}(t)& {\dot{\Phi }}_{22}(t)& {\dot{\Phi }}_{23}(t)\\ {\dot{\Phi }}_{31}(t)& {\dot{\Phi }}_{32}(t)& {\dot{\Phi }}_{33}(t)\end{array}\right]& =\left[\begin{array}{ccc}-\lfloor \mathbf{\omega }(t){\rfloor }_{\times }& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_3& {\mathbf{I}}_{3\times 3}\\ -({}_t^{G}R{)}^T\lfloor {\mathbf{a}}_{\mathbf{m}}(t){\rfloor }_{\times }& 0_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{ccc}{\Phi }_{11}(t)& {\Phi }_{12}(t)& {\Phi }_{13}(t)\\ {\Phi }_{21}(t)& {\Phi }_{22}(t)& {\Phi }_{23}(t)\\ {\Phi }_{31}(t)& {\Phi }_{32}(t)& {\Phi }_{33}(t)\end{array}\right]\\ \left[\begin{array}{ccc}{\Phi }_{11}(t_0)& {\Phi }_{12}(t_0)& {\Phi }_{13}(t_0)\\ {\Phi }_{21}(t_0)& {\Phi }_{22}(t_0)& {\Phi }_{23}(t_0)\\ {\Phi }_{31}(t_0)& {\Phi }_{32}(t_0)& {\Phi }_{33}(t_0)\end{array}\right]& =\left[\begin{array}{ccc}\mathbf{I}& 0& 0\\ 0& \mathbf{I}& 0\\ 0& 0& \mathbf{I}\end{array}\right]\end{array}\text{\hspace{1em}(18)}$$
求解上述的微分方程，易得：
Φ ( t