MSCKF（四）——能观性分析

---

 

---

写在前面

讲实话，笔者之前就听说过很多关于SLAM系统的能观性、或者说一致性的分析，最开始的切入点当然是大名鼎鼎的First-Estimate-Jacobian（FEJ）技术，且当时更多接触的是Graph-Base的方法，当时就直观感受而言，仅仅觉得FEJ固定了求解的方向，导致整个优化问题的信息矩阵在求解的时候是固定的，由此来解决由于信息矩阵的变化导致过度估计的问题。

但是上述仅仅是直观的一些想法，并没有理论做依据，之前也零星的看过黄老师关于FEJ的文章，但是公式太多，个人水平也很有限，所以不能很好的理解其中的精髓。

这次借着阅读整理MSCKF的机会，更深一步的理解一下如何分析系统的能观性以及FEJ到底在解决什么，参考更多的其实是李明扬大佬的一些文章和MSCKF2.0的工作。

 

---

Reference

1. Consistency of EKF-Based Visual-Inertial Odometry. 关于MSCKF一致性的分析，也是本文主要参考的论文；
2. Analysis and Improvement of the Consistency of Extended Kalman Filter based SLAM. 黄老师关于EKF一致性的分析；
3. Generalized Analysis and Improvement of the Consistency of EKF-based SLAM. 黄老师同年发表的一篇更长的关于一致性的文章，可以认为是参考2的详细版；

 

---

Notation

本文中公式比较多，所以Notation也比较多，先对Notation进行一些前置说明：

1. $l$ 节点的理想值表示为${\mathrm{x}}_l$；在 k 时刻的估计值表示为${\hat{\mathrm{x}}}_l^{(k)}$；其误差状态表示为${\tilde{\mathrm{x}}}_l$；
2. $l$ 节点的估计值在不同时刻的关系为：${\hat{\mathbf{x}}}_l^{(k+n)}={}^{(k+n)}{\tilde{\mathbf{x}}}_l^{(k)}+{\hat{\mathbf{x}}}_l^{(k)}$，特别的，对于旋转有：${}_G^l{\hat{\mathbf{q}}}^{(k+n)}\approx (\mathbf{I}+\lfloor {}^{(k+n)}{\theta }_l^{(k)}{\rfloor }_{\times }){}_G^l{\hat{\mathbf{q}}}^{(k)}$；

由于整片文章公式太多了，其中Notation有些地方可能不是很对笔者也没有检查出来，同时后面为了简化还有些Notation的复用，但是笔者都进行了说明，有问题希望及时提出，笔者及时改正；

 

---

IMU误差状态传播公式的回顾

简单的回顾一下MSCKF对于IMU误差状态（error-state）的传播过程：

1. 通过IMU运动方程得到误差状态的微分方程：
   $$\dot{\tilde{X}}=\mathbf{F}(\hat{X})\tilde{X}+\mathbf{G}n\text{\hspace{1em}(1)}$$

2. 通过线性系统的离散化得到IMU误差状态递方程的闭式解：
   $$\tilde{\mathbf{X}}\left(t_{k+1}\right)=\mathbf{\Phi }\left(t_{k+1},t_k\right)\tilde{\mathbf{X}}\left(t_k\right)+{\int }_{t_k}^{t_{k+1}}\mathbf{\Phi }\left(t_{k+1},\tau \right)\mathbf{G}(\tau )\mathbf{n}(\tau )\mathrm{d}\tau \text{\hspace{1em}(2)}$$
   其中$\mathbf{\Phi }\left(t_{k+1},t_k\right)=\exp {\int }_{t_k}^{t_{k+1}}\mathbf{F}(t)\mathrm{d}t$

对于上述的推导过程，我们容易看到，整个推导过程建立在数值分析的层次，也就是闭式解本身是一个数值解，当然有的小伙伴会认为如果假设一段时间内微分量是不变的，直接乘以时间是不是就有理论意义？诚然，笔者认为这样的方法可以，但是不精确，不精确的解去分析整个系统的特性带来的结果必然也是不精确的；所以在最开始，我们需要从理论的角度来重新推导整个IMU误差状态的传导过程。

 

---

IMU误差状态传播公式的再推导

如上所述，本节主要是对误差状态传播公式的理论推导，这里主要分析三个参数的传播公式：

1. 旋转部分；
2. 速度部分；
3. 位移部分；

bias部分因为不涉及到能观性的问题，所以这里暂不引入，实际上，引入之后得到的结论也是一样的，后续会稍微提到一下。

注：以下公式均在时间为$l$的时刻。

 

旋转部分的推导

主要依赖两个基本的公式：
$$\{\begin{array}{l}\begin{array}{rl}{}_G^{I_{l+1}}{R}^{(l)}& ={}_{I_l}^{I_{l+1}}{R}^{(l)}{}_G^{I_l}{R}^{(l)}\\ {}_G^{I_l}{R}^{(l)}& =(\mathbf{I}-{\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_G^{I_l}{\hat{R}}^{(l)}\end{array}\end{array}\text{\hspace{1em}(3)}$$
将第二行的公式带入到第一行中可以得到：
$$\begin{array}{rl}{}_G^{I_{l+1}}{R}^{(l)}& ={}_{I_l}^{I_{l+1}}{R}^{(l)}{}_G^{I_l}{R}^{(l)}\\ (\mathbf{I}-{\left[{}^{I_{l+1}}\tilde{\theta }\right]}_{\times }){}_G^{I_{l+1}}{\hat{R}}^{(l)}& =(\mathbf{I}-{\left[{}_{I_l}^{I_{l+1}}\tilde{\theta }\right]}_{\times }){}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}(\mathbf{I}-{\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_G^{I_l}{\hat{R}}^{(l)}\\ {}_G^{I_{l+1}}{\hat{R}}^{(l)}-({\left[{}^{I_{l+1}}\tilde{\theta }\right]}_{\times }){}_G^{I_{l+1}}{\hat{R}}^{(l)}& ={}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}{}_G^{I_l}{\hat{R}}^{(l)}-({\left[{}_{I_l}^{I_{l+1}}\tilde{\theta }\right]}_{\times }){}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}{}_G^{I_l}{\hat{R}}^{(l)}-{}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}({\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_G^{I_l}{\hat{R}}^{(l)}+\mathbf{O}(2)\\ {\left[{}^{I_{l+1}}\tilde{\theta }\right]}_{\times }& \approx {\left[{}_{I_l}^{I_{l+1}}\tilde{\theta }\right]}_{\times }+\underset{{\left[{}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}{}^{I_l}\tilde{\theta }\right]}_{\times }}{\underbrace{{}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}({\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_{I_{l+1}}^{I_l}{\hat{R}}^{(l)}}}\end{array}\text{\hspace{1em}(4)}$$
对公式（4）的最后一行使用vee操作可得：
$${}^{I_{l+1}}\tilde{\theta }={}_{I_l}^{I_{l+1}}\tilde{\theta }+{}_{I_l}^{I_{l+1}}{\hat{R}}^{(l)}{}^{I_l}\tilde{\theta }\text{\hspace{1em}(5)}$$
 

速度部分的推导

依赖三个基本公式：
$$\{\begin{array}{l}\begin{array}{rl}{}^G{v}_{I_{l+1}}^{(l)}& ={}^G{v}^{(l)}+({}_G^{I_l}{R}^{(l)}{)}^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_l}^{(l)}& ={}^G{\hat{v}}_{I_l}^{(l)}+{}^G{\tilde{v}}_{I_l}^{(l)}\\ {}_G^{I_l}{R}^{(l)}& =(\mathbf{I}-{\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_G^{I_l}{\hat{R}}^{(l)}\end{array}\end{array}\text{\hspace{1em}(6)}$$
把公式（6）中的第二行和第三行带入第一行之后得到：
G v I l + 1 ( l ) = G v ( l ) + ( G I l R ( l ) ) T ∫ t l t l + 1 ( I l I τ R ) T a m d τ + g Δ t G v ^ I l + 1 ( l ) + G v ~ I l + 1 ( l ) = G v ^ I l ( l ) + G v ~ I l ( l ) + { ( I − [ I l θ ~ ] × ) G I l R ^ ( l ) } T ∫ t l t l + 1 { ( I − [ I τ θ ~ ] × ) I l I τ R ^ } T a m d τ + g Δ t = G v ^ I l ( l ) + ( G I l R ^ ( l ) ) T ∫ t l t l + 1 ( I l I τ R ^ ) T a m d τ + g Δ t + G v ~ I l ( l ) + { ( − [ I τ θ ~ ] × ) I l I τ R ^ } T ∫ t l t l + 1 ( I l I τ R ^ ) T a m d τ + ( G I l R ^ ( l ) ) T ∫ t l t l + 1 { ( − [ I τ θ ~ ] × ) I l I τ R ^ } T a m d τ + O ( 2 ) (7) \begin{aligned} {}^{G}v_{I_{l+1}}^{(l)} &= {}^{G}v^{(l)} + ({}^{I_{l}}_{G}R^{(l)})^{T} \int_{t_{l}}^{t_{l+1}} ({}^{I_{\tau}}_{I_{l}}R)^{T} \mathbf{a_m} d \tau + g \Delta{t} \\ {}^{G}\hat{v}_{I_{l+1}}^{(l)}+{}^{G}\tilde{v}_{I_{l+1}}^{(l)} &= {}^{G}\hat{v}_{I_{l}}^{(l)}+{}^{G}\tilde{v}_{I_{l}}^{(l)} + \{(\mathbf{I}-\left[ {}^{I_{l}}\tilde{\theta} \right]_{\times}){}^{I_{l}}_{G}\hat{R}^{(l)}\}^{T} \int_{t_{l}}^{t_{l+1}} \{(\mathbf{I}-\left[{}^{I_{\tau}}\tilde{\theta}\right]_{\times}){}^{I_{\tau}}_{I_{l}}\hat{R}\}^{T} \mathbf{a_m} d \tau + g \Delta{t} \\ &={}^{G}\hat{v}_{I_{l}}^{(l)} + ({}^{I_{l}}_{G}\hat{R}^{(l)})^{T}\int_{t_{l}}^{t_{l+1}} ({}^{I_{\tau}}_{I_{l}}\hat{R})^{T} \mathbf{a_m} d \tau + g \Delta{t}\\ &+{}^{G}\tilde{v}_{I_{l}}^{(l)} +\{(-\left[{}^{I_{\tau}}\tilde{\theta}\right]_{\times}){}^{I_{\tau}}_{I_{l}}\hat{R}\}^{T}\int_{t_{l}}^{t_{l+1}} ({}^{I_{\tau}}_{I_{l}}\hat{R})^{T} \mathbf{a_m} d \tau+({}^{I_{l}}_{G}\hat{R}^{(l)})^{T}\int_{t_{l}}^{t_{l+1}} \{(-\left[{}^{I_{\tau}}\tilde{\theta}\right]_{\times}){}^{I_{\tau}}_{I_{l}}\hat{R}\}^{T} \mathbf{a_m} d \tau + \mathbf{O(2)} \end{aligned} \tag{7} GvIl+1​(l)​Gv^Il+1​(l)​+Gv~Il+1​(l)​​=Gv(l)+(GIl​​R(l))T∫tl​tl+1​​(Il​Iτ​​R)Tam​dτ+gΔt=Gv^Il​(l)​+Gv~Il​(l)​+{(I−[Il​θ~]×​)GIl​​R^(l)}T∫tl​tl+1​​{(I−[Iτ​θ~]×​)Il​Iτ​​R^}Tam​dτ+gΔt=Gv^Il​(l)​+(GIl​​R^(l))T∫tl​tl+1​​(Il​Iτ​​R^)Tam​dτ+gΔt+Gv~Il​(l)​+{(−[Iτ​θ~]×​)Il​Iτ​​R^}T∫tl​tl+1​​(Il​Iτ​​R^)Tam​dτ+(GIl​​R^(l))T∫tl​tl+1​​{(−[Iτ​θ~]×​)Il​Iτ​​R^}Tam​dτ+O(2)​(7)
此时我们将积分值进行替换，规则如下：
s ^ ( l ) = ∫ t l t l + 1 ( I l I τ R ^ ) T a m d τ = G I l R ( l ) ( G v ^ l + 1 ( l ) − G v ^ l ( l ) − g Δ t ) s ~ ( l ) = ∫ t l t l + 1 { ( − [ I τ θ ~ ] × ) I l I τ R ^ } T a m d τ (8) \begin{aligned} \mathrm{\hat{s}}^{(l)}&=\int_{t_{l}}^{t_{l+1}} ({}^{I_{\tau}}_{I_{l}}\hat{R})^{T} \mathbf{a_m} d \tau = {}^{I_{l}}_{G}R^{(l)}({}^{G}\hat{v}_{l+1}^{(l)}-{}^{G}\hat{v}_{l}^{(l)}-g \Delta{t}) \\ \mathrm{\tilde{s}}^{(l)} &= \int_{t_{l}}^{t_{l+1}} \{(-\left[{}^{I_{\tau}}\tilde{\theta}\right]_{\times}){}^{I_{\tau}}_{I_{l}}\hat{R}\}^{T} \mathbf{a_m} d \tau \end{aligned} \tag{8} s^(l)s~(l)​=∫tl​tl+1​​(Il​Iτ​​R^)Tam​dτ=GIl​​R(l)(Gv^l+1(l)​−Gv^l(l)​−gΔt)=∫tl​tl+1​​{(−[Iτ​θ~]×​)Il​Iτ​​R^}Tam​dτ​(8)
公式（8）中的等价关系由公式（6）的第一行推导出，将公式（8）带入到公式（7）中可以得到：
$${}^G{\tilde{v}}_{I_{l+1}}^{(l)}={}^G{\tilde{v}}_{I_l}^{(l)}-({}_G^{I_l}{\hat{R}}^{(l)}{)}^T{\left[{\hat{\mathrm{s}}}^{(l)}\right]}_{\times }{}^{I_l}\tilde{\theta }+({}_G^{I_l}{\hat{R}}^{(l)}{)}^T{\tilde{\mathrm{s}}}^{(l)}\text{\hspace{1em}(9)}$$
 

位移部分的推导

与速度相同，这部分推导依赖四个基础公式：
$$\{\begin{array}{l}\begin{array}{rl}{}^G{p}_{I_{l+1}}^{(l)}& ={}^G{p}_{I_l}^{(l)}+{}^G{v}_{I_l}^{(l)}\Delta t+({}_G^{I_l}{R}^{(l)}{)}^T{\int }_{t_l}^{t_{l+1}}{\int }_{t_l}^s({}_{I_l}^{I_{\tau }}R{)}^T{\mathbf{a}}_{\mathbf{m}}d\tau ds+\frac{1}{2}g\Delta t^2\\ {}^G{p}_{I_l}^{(l)}& ={}^G{\hat{p}}_{I_l}^{(l)}+{}^G{\tilde{p}}_{I_l}^{(l)}\\ {}^G{v}_{I_l}^{(l)}& ={}^G{\hat{v}}_{I_l}^{(l)}+{}^G{\tilde{v}}_{I_l}^{(l)}\\ {}_G^{I_l}{R}^{(l)}& =(\mathbf{I}-{\left[{}^{I_l}\tilde{\theta }\right]}_{\times }){}_G^{I_l}{\hat{R}}^{(l)}\end{array}\end{array}\text{\hspace{1em}(10)}$$
同样按照速度的推理，可以得到如下结论：
$${}^G{\tilde{p}}_{I_{l+1}}^{(l)}={}^G{\tilde{p}}_{I_l}^{(l)}+{}^G{\tilde{v}}_{I_l}^{(l)}\Delta t-({}_G^{I_l}{\hat{R}}^{(l)}{)}^T{\left[{\hat{\mathrm{y}}}^{(l)}\right]}_{\times }{}^{I_l}\tilde{\theta }+({}_G^{I_l}{\hat{R}}^{(l)}{)}^T{\tilde{\mathrm{y}}}^{(l)}\text{\hspace{1em}(11)}$$
其中的变量$\mathrm{y}$和速度中的一样，也是一个替换变量：
y ^ ( l ) = ∫ t l t l + 1 ∫ t l s ( I l I τ R ^ ) T a m d τ d s = G I l R ( l ) ( G p ^ l + 1 ( l ) − G p ^ l ( l ) − G v I l ( l ) Δ t − 1 2 g Δ t 2 ) y ~ ( l ) = ∫ t l t l + 1 ∫ t l s { ( − [ I τ θ ~ ] × ) I l I τ R ^ } T a m d τ d s (12) \begin{aligned} \mathrm{\hat{y}}^{(l)}&=\int_{t_{l}}^{t_{l+1}} \int_{t_l}^{s} ({}^{I_{\tau}}_{I_{l}}\hat{R})^{T} \mathbf{a_m} d \tau d s = {}^{I_{l}}_{G}R^{(l)}({}^{G}\hat{p}_{l+1}^{(l)}-{}^{G}\hat{p}_{l}^{(l)}-{}^{G}v_{I_l}^{(l)}\Delta{t}-\frac{1}{2}g \Delta{t}^2) \\ \mathrm{\tilde{y}}^{(l)} &= \int_{t_{l}}^{t_{l+1}} \int_{t_l}^{s} \{(-\left[{}^{I_{\tau}}\tilde{\theta}\right]_{\times}){}^{I_{\tau}}_{I_{l}}\hat{R}\}^{T} \mathbf{a_m} d \tau d s \end{aligned} \tag{12} y^​(l)y~​(l)​=∫tl​tl+1​​∫tl​s​(Il​Iτ​​R^)Tam​dτds=GIl​​R(l)(Gp^​l+1(l)​−Gp^​l(l)​−GvIl​(l)​Δt−21​gΔt2)=∫tl​tl+1​​∫tl​s​{(−[Iτ​θ~]×​)Il​Iτ​​R^}Tam​dτds​(12)
 

状态传递的传递方程

结合公式（5）（9）（12）可以写出在 $l$ 时刻的误差状态的传递方程，对应于KF的预测部分；
$$\left[\begin{array}{c}{}_G^{I_{l+1}}{\tilde{\theta }}^{(l)}\\ {}^G{\tilde{p}}_{I_{l+1}}^{(l)}\\ {}^G{\tilde{v}}_{I_{l+1}}^{(l)}\end{array}\right]=\left[\begin{array}{ccc}{}_{I_l}^{I_{l+1}}{R}^{(l)}& 0& 0\\ -({}_G^{I_l}{R}^{(l)}{)}^T{\left[{\hat{\mathrm{y}}}_l^{(l)}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t\\ -({}_G^{I_l}{R}^{(l)}{)}^T{\left[{\hat{\mathrm{s}}}_l^{(l)}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}{}_G^{I_l}{\tilde{\theta }}^{(l)}\\ {}^G{\tilde{p}}_{I_l}^{(l)}\\ {}^G{\tilde{v}}_{I_l}^{(l)}\end{array}\right]+\left[\begin{array}{c}{}_{I_l}^{I_{l+1}}{\tilde{\theta }}^{(l)}\\ ({}_G^{I_l}{R}^{(l)}{)}^T\left[{\tilde{\mathrm{y}}}_l^{(l)}\right]\\ ({}_G^{I_l}{R}^{(l)}{)}^T\left[{\tilde{\mathrm{s}}}_l^{(l)}\right]\end{array}\right]\text{\hspace{1em}(13)}$$
上式可以写作状态转移方程为：
$${}_G^{I_{l+1}}{\tilde{\mathrm{x}}}^{(l)}=\Phi ({\mathrm{x}}_{I_{l+1}}^{(l)},{\mathrm{x}}_{I_l}^{(l)}){}_G^{I_l}{\tilde{\mathrm{x}}}^{(l)}+{\mathrm{w}}^{(l)}\text{\hspace{1em}(14)}$$

这里有必要引用原文中的一些介绍：以位移的误差状态量的递推公式为例，相当于在原先误差状态变量的基础上加上速度的变化，之后角速度的变化与一个杆臂$-({}_G^{I_l}R{)}^T\left[{\hat{\mathrm{y}}}^{(l)}\right]$相乘来影响位移的误差状态。所以对于公式（14）表示的状态转移过程而言，整个过程也具有一定的物理意义。

 

---

MSCKF的观测模型

下面的所有的下角标 $l$ 表示id为 $l$ 的相机，不表示时间，这里暂时不涉及时间，可以认为是理想的观测模型。

为了分析能观性，这里还需要一个步骤就是观测模型，以单个观测点$P_{f_j}$为例，其观测模型为：
$$\begin{array}{rl}{z}_l& =\pi ({}^{C_l}{\mathrm{p}}_{f_j})+n_l\\ {}^{C_l}{\mathrm{p}}_{f_j}& ={}_I^{C}R{}_l^{G}R({}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_l})+{}^C{\mathrm{p}}_I\end{array}\text{\hspace{1em}(15)}$$

所以观测模型为：
$$\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 }{\underbrace{{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_i}\right]}_{\times }({}_G^{I_l}R{)}^T}}& \underset{\partial e/\partial \mathrm{p}}{\underbrace{-{\mathbf{I}}_{3\times 3}}}& \underset{\partial e/\partial \mathrm{v}}{\underbrace{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}(16)}$$
其中：
$$J_{(f_j|l)}=\frac{1}{Z}\left[\begin{array}{ccc}1& 0& -\frac{X}{Z}\\ 0& 1& -\frac{Y}{Z}\end{array}\right]$$
 

---

能观性分析

能观性的分析主要依赖于能观性矩阵：
$$\mathcal{O}=\left[\begin{array}{c}{\mathrm{H}}_k\\ {\mathrm{H}}_{k+1}{\Phi }_k\\ ⋮\\ {\mathrm{H}}_{k+m}{\Phi }_{k+m-1}\dots {\Phi }_k\end{array}\right]\text{\hspace{1em}(17)}$$
 

---

理想情况下的能观性矩阵

这里先进行理想情况下的能观性矩阵的推导，这个部分的所有变量均使用理想情况下的状态值（公式上来讲就是${\mathbf{X}}_{I_l}^{(l-1)}={\mathbf{X}}_{I_l}^{(l)}=\cdots ={\mathbf{X}}_{I_l}^{l+m}$），也就是状态变量从预测出来的时候，就是真值了，后面一直都不变了（提前剧透了FEJ -.-!!!，不过还是稍有不同，后面会解释）。

于是将公式（13）表示的状态转移矩阵与公式（16）表示的观测矩阵带入公式（17）就可以得到能观性矩阵，这里以 $l$ 时刻为例，有：
$${\mathcal{O}}_l={\mathrm{H}}_l{\Phi }_{l-1}{\Phi }_{l-2}\dots {\Phi }_k\text{\hspace{1em}(18)}$$
这里整个推导十分麻烦，但是好在只要计算前两次乘法我们就能找到规律，下面分别对两次乘法进行展开：
$$\begin{array}{rl}{\mathrm{H}}_l{\Phi }_{l-1}& =J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}R\left\{\underset{S0}{\underbrace{\left[\begin{array}{ccc}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_l}\right]}_{\times }({}_G^{I_l}R{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]{\Phi }_{l-1}}}\dots \mathbf{I}\dots 0\right\}\end{array}\text{\hspace{1em}(19)}$$
可以看到，整个乘积的结果其实更加依赖的是IMU的观测部分与状态转移矩阵的乘积部分，也就是上式中的S0部分，下面单独展开：
$$\begin{array}{rl}S0& =\left[\begin{array}{ccc}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_l}\right]}_{\times }({}_G^{I_l}R{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{ccc}{}_{I_{l-1}}^{I_l}R& 0& 0\\ -({}_G^{I_{l-1}}R{)}^T{\left[{\mathrm{y}}_{l-1}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-1}\\ -({}_G^{I_{l-1}}R{)}^T{\left[{\mathrm{s}}_{l-1}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\\ & =\left[\begin{array}{ccc}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_l}\right]}_{\times }({}_G^{I_{l-1}}R{)}^T+({}_G^{I_{l-1}}R{)}^T{\left[{\mathrm{y}}_{l-1}\right]}_{\times }& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\\ & =\left[\begin{array}{ccc}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_l}\right]}_{\times }({}_G^{I_{l-1}}R{)}^T+{\left[{}^G{p}_l-{}^G{p}_{l-1}-{}^G{v}_{I_{l-1}}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\right]}_{\times }({}_G^{I_{l-1}}R{)}^T& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\\ & =\left[\begin{array}{ccc}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{l-1}}-{}^G{v}_{I_{l-1}}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\right]}_{\times }({}_G^{I_{l-1}}R{)}^T& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\end{array}\text{\hspace{1em}(20)}$$
其中第三行的展开中用到了反对称矩阵的性质：若矩阵R是特殊正交群SO(3)，则有$R[A{]}_{\times }{R}^T=[RA{]}_{\times }$的变换。

接着继续看公式（18）中与${\Phi }_{l-2}$的乘积：
$$\begin{array}{rl}S1& =S0{\Phi }_{l-2}\\ & =\left[\begin{array}{ccc}\underset{m_{l-1}}{\underbrace{{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{l-1}}-{}^G{v}_{I_{l-1}}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\right]}_{\times }}}({}_G^{I_{l-1}}R{)}^T& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\left[\begin{array}{ccc}{}_{I_{l-2}}^{I_{l-1}}R& 0& 0\\ -({}_G^{I_{l-2}}R{)}^T{\left[{\mathrm{y}}_{l-2}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-2}\\ -({}_G^{I_{l-2}}R{)}^T{\left[{\mathrm{s}}_{l-2}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\\ & ={\left[\begin{array}{c}(m_{l-1}+{\left[{}^G{p}_{I_{l-1}}-{}^G{p}_{I_{l-2}}-{}^G{v}_{I_{l-2}}\Delta t_{l-2}-\frac{1}{2}g\Delta t_{l-2}^2\right]}_{\times }+{\left[{}^G{v}_{l-1}-{}^G{v}_{l-2}-g\Delta t_{l-2}\right]}_{\times }\Delta t_{l-1})({}_G^{I_{l-2}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}(\Delta t_{l-2}+\Delta t_{l-1})\end{array}\right]}^T\end{array}\text{\hspace{1em}(21)}$$
把上式中的第一行拿出来分析：
$$\begin{array}{rl}S{1}_1& ={}^G{\mathrm{p}}_{f_j}-{}^G{\mathrm{p}}_{I_{l-1}}-{}^G{v}_{I_{l-1}}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\\ & +{}^G{p}_{I_{l-1}}-{}^G{p}_{I_{l-2}}-{}^G{v}_{I_{l-2}}\Delta t_{l-2}-\frac{1}{2}g\Delta t_{l-2}^2\\ & +({}^G{v}_{l-1}-{}^G{v}_{l-2}-g\Delta t_{l-2})\Delta t_{l-1}\\ & ={}^G{\mathrm{p}}_{f_j}-{}^G{p}_{I_{l-2}}-{}^G{v}_{l-2}(\Delta t_{l-1}+\Delta t_{t-2})-\frac{1}{2}g(\Delta t_{l-1}^2+2\Delta t_{t-1}\Delta t_{t-2}+\Delta t_{t-2}^2)\\ & ={}^G{\mathrm{p}}_{f_j}-{}^G{p}_{I_{l-2}}-{}^G{v}_{l-2}(\Delta t_{l-1}+\Delta t_{t-2})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{t-2}{)}^2\end{array}\text{\hspace{1em}(21A)}$$
上式带入公式（21）可以得到：
$$S1={\left[\begin{array}{c}{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{p}_{I_{l-2}}-{}^G{v}_{l-2}(\Delta t_{l-1}+\Delta t_{t-2})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{t-2}{)}^2\right]}_{\times }({}_G^{I_{l-2}}R{)}^T\\ -\mathbf{I}\\ -\mathbf{I}(\Delta t_{l-2}+\Delta t_{l-1})\end{array}\right]}^T\text{\hspace{1em}(21B)}$$
后面的情况均类似，所以由归纳法可以得到：
$${\mathcal{O}}_l=J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}R\left[\begin{array}{cc}\underset{IMU}{\underbrace{{\left[{}^G{\mathrm{p}}_{f_j}-{}^G{p}_{I_k}-{}^G{v}_k\Delta t-\frac{1}{2}g\Delta t^2\right]}_{\times }({}_G^{I_k}R{)}^T,-\mathbf{I},-\mathbf{I}\Delta t}}& \underset{feature}{\underbrace{\dots ,\mathbf{I},\dots ,0}}\end{array}\right]\text{\hspace{1em}(22)}$$
其中$\Delta t=\Delta t_{l-1}+\cdots +\Delta t_k$。

容易看出（不是论文这么说真心凑不出啊），在理想情况下，能观性矩阵的零空间为：
$$\mathbf{N}=\left[\begin{array}{cc}{0}_3& {}_G^{I_k}\mathbf{R}\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_k\right]}_{\times }\mathbf{g}\\ 0_3& -{\left[{}^G{\mathbf{v}}_k\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{f_1}\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{f_2}\right]}_{\times }\mathbf{g}\\ ⋮& ⋮\\ {\mathbf{I}}_3& -{\left[{}^G{\mathbf{p}}_{f_N}\right]}_{\times }\mathbf{g}\end{array}\right]\text{\hspace{1em}(23)}$$
读者可以将公式（22）与公式（23）相乘，发现均为0，其中最后的g其实是为了凑两个g的叉乘而存在的。

 

小结

在求解理想情况下的能观性矩阵时，主要利用了如下几点，这几点也是对于启发FEJ非常重要的几点：

1. 从公式（20）和公式（21）的旋转矩阵相乘可以看到，由于是理想情况，因此旋转矩阵可以直接消去 $l-1$ 时刻的影响：
   $$({}_G^{I_{l-2}}R{)}^T=({}_G^{I_{l-1}}R{)}^T{}_{I_{l-2}}^{I_{l-1}}R\text{\hspace{1em}(24)}$$

2. 由公式（21）及其附属推导可以看到，整个化简的关键在于位移和速度的量不变化；

 

---

实际情况下的能观性矩阵

下面终于要分析一下实际的能观性矩阵的东西了。

假设程序运行到了${\alpha }_{i+1}$时刻，那么对于第 $l$ 个位姿节点（因为MSCKF在维护一个窗口，窗口内的参数都是会更新的）能观性矩阵的组成部分来说：

1. 状态转移矩阵为${\Phi }_{l-1}({\hat{\mathrm{x}}}_{I_l}^{(l-1)},{\hat{\mathrm{x}}}_{I_{l-1}}^{(l-1)})$，${\Phi }_{l-2}({\hat{\mathrm{x}}}_{I_{l-1}}^{(l-2)},{\hat{\mathrm{x}}}_{I_{l-2}}^{(l-2)}$，$\dots$，${\Phi }_k({\hat{\mathrm{x}}}_{k+1}^{(k)},{\hat{\mathrm{x}}}_{I_k}^{(k)})$，这部分因为是历史递推过来的，所以不能受到后来更新的影响；
2. 依旧对于特征点$P_{f_j} $ ，观测矩阵${\mathbf{H}}_{l|{\alpha }_i}=\partial {\mathbf{e}}_l^{({\alpha }_i+1)}/\partial {\hat{\mathrm{x}}}_l^{({\alpha }_i)}$，此时公式中使用了 $l$ 节点最新的估计值，这里有读者可能比较疑惑为什么变量使用的是${\alpha }_i$时刻的值，因为在${\alpha }_i+1$时刻，在窗口中的变量还没有被最新的观测更新，所以在线性化的时候，值依旧是上一次被更新的值，也就是${\hat{\mathrm{x}}}_l^{{\alpha }_i}$；

针对上面的两个点，这里写出他们的具体形式：

状态转移矩阵

类比公式（13）可得：
$${\Phi }_{l-1}({\hat{\mathrm{x}}}_{I_l}^{(l-1)},{\hat{\mathrm{x}}}_{I_{l-1}}^{(l-1)})=\left[\begin{array}{ccc}{}_{I_{l-1}}^{I_l}{R}^{(l-1)}& 0& 0\\ -({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-1}^{(l-1)}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-1}\\ -({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T{\left[{\hat{\mathrm{s}}}_{l-1}^{(l-1)}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\text{\hspace{1em}(25)}$$

观测矩阵

类比公式（16），这里直接写出综合形式：
$${\mathrm{H}}_l^{({\alpha }_i)}=J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}{R}^{({\alpha }_i)}\left\{\left[\begin{array}{ccc}{\left[{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}\right]}_{\times }({}_G^{I_l}{R}^{({\alpha }_i)}{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]\dots \mathbf{I}\dots 0\right\}\text{\hspace{1em}(26)}$$
 

能观矩阵的构建

这里的推导就超级麻烦了，好在我们依旧可以从前两次乘法运算之后看到问题所在，因此类比于公式（19），先展开第一个乘积：
$$\begin{array}{rl}M{0}^{({\alpha }_i)}& =\left[\begin{array}{ccc}{\left[{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}\right]}_{\times }({}_G^{I_l}{R}^{({\alpha }_i)}{)}^T& -{\mathbf{I}}_{3\times 3}& 0_{3\times 3}\end{array}\right]\left[\begin{array}{ccc}{}_{I_{l-1}}^{I_l}{R}^{(l-1)}& 0& 0\\ -({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-1}^{(l-1)}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-1}\\ -({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T{\left[{\hat{\mathrm{s}}}_{l-1}^{(l-1)}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\\ & =\left[\begin{array}{ccc}{\left[{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}\right]}_{\times }({}_G^{I_l}{R}^{({\alpha }_i)}{)}^T({}_{I_{l-1}}^{I_l}{R}^{(l-1)})+({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-1}^{(l-1)}\right]}_{\times }& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\end{array}\text{\hspace{1em}(27)}$$
可以看到，重点是在上式中的第一个元素，将该部分单独展开：
$$\begin{array}{rl}M{0}_1^{({\alpha }_i)}& =\left[\underset{part1}{\underbrace{{\left[{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}\right]}_{\times }}}\underset{D1}{\underbrace{({}_G^{I_l}{R}^{({\alpha }_i)}{)}^T({}_G^{I_l}{R}^{(l-1)})}}+\underset{part2}{\underbrace{{\left[{}^G{\hat{p}}_l^{(l-1)}-{}^G{\hat{p}}_{l-1}^{(l-1)}-{}^G{\hat{v}}_{I_{l-1}}^{(l-1)}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\right]}_{\times }}}\right]({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T\\ & =\left[part1\underset{D1}{\underbrace{{\left((\mathbf{I}-\lfloor {}^{({\alpha }_i)}{\theta }_l^{(l-1)}{\rfloor }_{\times })({}_G^{I_l}{R}^{(l-1)})\right)}^T({}_G^{I_l}{R}^{(l-1)})}}+part2\right]({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T\\ & =\left[\underset{D2}{\underbrace{part1+part2}}+part1\underset{D3}{\underbrace{\left(({}_G^{I_l}{R}^{(l-1)}{)}^T\lfloor {}^{({\alpha }_i)}{\theta }_l^{(l-1)}{\rfloor }_{\times }({}_G^{I_l}{R}^{(l-1)})\right)}}\right]({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T\\ & =\left[\lfloor {}^G{\hat{\mathrm{p}}}_{f_j}\underset{D4}{\underbrace{-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}+{}^G{\hat{p}}_l^{(l-1)}}}-{}^G{\hat{p}}_{l-1}^{(l-1)}-{}^G{\hat{v}}_{I_{l-1}}^{(l-1)}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2{\rfloor }_{\times }+part1\times D3\right]({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T\\ & =\left[\underset{part3}{\underbrace{\lfloor {}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{l-1}^{(l-1)}-{}^G{\hat{v}}_{I_{l-1}}^{(l-1)}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2{\rfloor }_{\times }}}+\underset{D5}{\underbrace{\lfloor -{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}+{}^G{\hat{p}}_{I_l}^{(l-1)}{\rfloor }_{\times }}}+\underset{D6}{\underbrace{part1\times D3}}\right]({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T\end{array}\text{\hspace{1em}(28)}$$

其中：

1. 第一行的化简将$\lfloor {\hat{\mathrm{y}}}_l^{(l-1)}{\rfloor }_{\times }$直接展开，且使用$\lfloor Ra{\rfloor }_{\times }=R\lfloor a{\rfloor }_{\times }{R}^T$的性质形成了part2部分；
2. 第三行的化简使用$\lfloor a{\rfloor }_{\times }^T=-\lfloor a{\rfloor }_{\times }$的性质，把负号消除形成D3部分；
3. 最后一行的化简，因为希望凑出一个类似于公式（20）最后一行的形式，所以这里直接将D4部分移了出来；

至此第一次乘积部分我们就完成了；下面来看第二次乘积：
$$\begin{array}{rl}M{1}^{({\alpha }_i)}& =\left[\begin{array}{ccc}M{0}_1^{{\alpha }_i}({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T& -\mathbf{I}& -\mathbf{I}\Delta t_{l-1}\end{array}\right]\left[\begin{array}{ccc}{}_G^{I_{l-1}}{R}^{(l-2)}({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T& 0& 0\\ -({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-2}^{(l-2)}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-2}\\ -({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T{\left[{\hat{\mathrm{s}}}_{l-2}^{(l-2)}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\\ & ={\left[\begin{array}{c}\left[M{0}_1^{{\alpha }_i}\underset{D7}{\underbrace{({}_G^{I_{l-1}}{R}^{(l-1)}{)}^T({}_G^{I_{l-1}}{R}^{(l-2)})}}+\underset{part4}{\underbrace{\lfloor {\hat{\mathrm{y}}}_{l-2}^{(l-2)}{\rfloor }_{\times }+\lfloor {\hat{\mathrm{s}}}_{l-2}^{(l-2)}\Delta t_{l-1}{\rfloor }_{\times }}}\right]({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T\\ -\mathbf{I}\\ -\mathbf{I}(\Delta t_{l-1}+\Delta t_{l-2})\end{array}\right]}^T\end{array}\text{\hspace{1em}(29)}$$

这里休息一下，因为其中做了一些变换和notation上的偷懒：

1. 把公式（28）中方括号中的部分复用了标示$M{0}_1^{{\alpha }_i}$；

2. 复用了标示${\hat{\mathrm{y}}}_{l-2}^{(l-2)}$，如下：
   $$-({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-2}^{(l-2)}\right]}_{\times }=-\lfloor \underset{{\hat{\mathrm{y}}}_{l-2}^{(l-2)}}{\underbrace{{}^G{\hat{p}}_{I_{l-1}}^{(l-2)}-{}^G{\hat{p}}_{I_{l-2}}^{(l-2)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-2)}\Delta t_{l-2}-\frac{1}{2}g\Delta t_{l-2}^2}}{\rfloor }_{\times }({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T$$

3. 复用了标示${\hat{\mathrm{s}}}_{l-2}^{(l-2)}$，如下：
   $$-({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T{\left[{\hat{\mathrm{s}}}_{l-2}^{(l-2)}\right]}_{\times }=-\lfloor \underset{{\hat{\mathrm{s}}}_{l-2}^{(l-2)}}{\underbrace{{}^G{\hat{v}}_{I_{l-1}}^{(l-2)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-2)}-g\Delta t_{l-2}}}{\rfloor }_{\times }({}_G^{I_{l-2}}{R}^{(l-2)}{)}^T$$

回到公式（29）的化简上来，可以看到重点其实在首个元素的最开始的乘法中，这里单独展开有：
$$\begin{array}{rl}M{0}_1^{{\alpha }_i}\times D7& =M{0}_1^{{\alpha }_i}{\left((\mathbf{I}-\lfloor {}^{(l-1)}{\theta }_{l-1}^{(l-2)}{\rfloor }_{\times }){}_G^{I_{l-1}}{R}^{(l-2)}\right)}^T({}_G^{I_{l-1}}{R}^{(l-2)})\\ & =M{0}_1^{{\alpha }_i}+\underset{D8}{\underbrace{M{0}_1^{{\alpha }_i}\lfloor ({}_G^{I_{l-1}}{R}^{(l-2)}{)}^T({}^{(l-1)}{\theta }_{l-1}^{(l-2)}){\rfloor }_{\times }}}\end{array}\text{\hspace{1em}(30)}$$
推导到这个地方的时候，结合公式（28）和（30），相信读者可以明显的看到以下几个事实：

1. 公式（28）中的part3和公式（29）中的part4结合就可以得到类似于理想值下的结果，也就是公式（21）的首个元素：
   $$\begin{array}{rl}part3+part4& ={}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{l-1}^{(l-1)}-{}^G{\hat{v}}_{I_{l-1}}^{(l-1)}\Delta t_{l-1}-\frac{1}{2}g\Delta t_{l-1}^2\\ & +{}^G{\hat{p}}_{I_{l-1}}^{(l-2)}-{}^G{\hat{p}}_{I_{l-2}}^{(l-2)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-2)}\Delta t_{l-2}-\frac{1}{2}g\Delta t_{l-2}^2\\ & +{}^G{\hat{v}}_{I_{l-1}}^{(l-2)}\Delta t_{l-1}-{}^G{\hat{v}}_{I_{l-2}}^{(l-2)}\Delta t_{l-1}-g\Delta t_{l-2}\Delta t_{l-1}\\ & =\underset{part5}{\underbrace{{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_{l-2}}^{(l-2)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-2)}(\Delta t_{l-1}+\Delta t_{l-2})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{l-2}{)}^2}}\\ & +\underset{D9}{\underbrace{({}^G{\hat{v}}_{I_{l-1}}^{(l-2)}-{}^G{\hat{v}}_{I_{l-1}}^{(l-1)})\Delta t_{l-1}+({}^G{\hat{p}}_{I_{l-1}}^{(l-2)}-{}^G{\hat{p}}_{I_{l-1}}^{(l-1)})}}\end{array}\text{\hspace{1em}(31)}$$
   可以看到，part5已经和公式（21）形式上一抹一样了，只不过除了这部分之外，多了D9的部分；

2. 在对公式（28）和公式（29）进行化简的时候，发现D1和D7部分真的是惊人的相似，都是说对于同一个变量，使用了两个时刻的估计值，从而造成在化简的时候多出了角度的扰动量，形成了D3和D8部分；

3. 同理，也能发现D5和D9部分也是一样的问题，同样的变量因为使用了不同时刻的估计值，产生了扰动项；

后面笔者就不进行展开了，十分麻烦，详细可以看参考1中的公式（42）（43）。

 

---

First-Estimate-Jacobian

通过上面三点的分析，不难发现，既然同一个变量使用了不同时刻的估计会产生扰动项，那么都使用一个时刻的估计值不就好了么，其实这就是FEJ做的事情！

如果我们按照这个思路，那么将公式（25）到公式（31）中的所有的${\mathrm{x}}_k^{(k+)}$都变为${\mathrm{x}}_k^{k-1}$，之后回代到公式（28）（30）（31）中：

1. $D3=\left(({}_G^{I_l}{R}^{(l-1)}{)}^T\lfloor {}^{({\alpha }_i)}{\theta }_l^{(l-1)}{\rfloor }_{\times }({}_G^{I_l}{R}^{(l-1)})\right)$，对于 $l$ 节点，在时刻 ${\alpha }_i$ 与时刻 $l-1$ 使用的都是相同的估计值，所以扰动项 ${}^{({\alpha }_i)}{\theta }_l^{(l-1)}={}^{(l-1)}{\theta }_l^{(l-1)}=0$，所以整个D3值为0，导致后面的D6=part1 x D3也为0；
2. $D5=-{}^G{\hat{\mathrm{p}}}_{I_l}^{({\alpha }_i)}+{}^G{\hat{p}}_{I_l}^{(l-1)}=-{}^G{\hat{\mathrm{p}}}_{I_l}^{(l-1)}+{}^G{\hat{p}}_{I_l}^{(l-1)}=0$；
3. D8与D3同理，由${}^{(l-1)}{\theta }_{l-1}^{(l-2)}=0$导致D8为0；
4. D9与D5同理，也全为0；

这么一代入，公式（29）得到了一个非常nice的形式：
$$\begin{array}{rl}M{1}^{({\alpha }_i)}& ={\left[\begin{array}{c}\lfloor {}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_{l-2}}^{(l-3)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-3)}(\Delta t_{l-1}+\Delta t_{l-2})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{l-2}{)}^2{\rfloor }_{\times }({}_G^{I_{l-2}}{R}^{(l-3)}{)}^T\\ -\mathbf{I}\\ -\mathbf{I}(\Delta t_{l-1}+\Delta t_{l-2})\end{array}\right]}^T\end{array}\text{\hspace{1em}(32)}$$

可以看到，这个形式已经十分贴合公式（21）所表示的理想情况下的能观矩阵了；

这里再往下再算一次矩阵乘法验证，也就是$M{2}^{({\alpha }_i)}{\Phi }_{l-3}({\hat{\mathrm{x}}}_{l-2}^{(l-3)},{\hat{\mathrm{x}}}_{l-3}^{(l-4)})$，单独展开得到：
$$\begin{array}{r}M{2}^{({\alpha }_i)}=\left[\begin{array}{ccc}M{1}_1^{{\alpha }_i}({}_G^{I_{l-2}}{R}^{(l-3)}{)}^T& -\mathbf{I}& -\mathbf{I}(\Delta t_{l-1}+\Delta t_{l-2})\end{array}\right]\left[\begin{array}{ccc}{}_G^{I_{l-2}}{R}^{(l-3)}({}_G^{I_{l-3}}{R}^{(l-4)}{)}^T& 0& 0\\ -({}_G^{I_{l-3}}{R}^{(l-4)}{)}^T{\left[{\hat{\mathrm{y}}}_{l-3}^{(l-4)}\right]}_{\times }& \mathbf{I}& \mathbf{I}\Delta t_{l-3}\\ -({}_G^{I_{l-3}}{R}^{(l-4)}{)}^T{\left[{\hat{\mathrm{s}}}_{l-3}^{(l-4)}\right]}_{\times }& 0& \mathbf{I}\end{array}\right]\end{array}\text{\hspace{1em}(33)}$$
重点展开首个元素有：
$$\begin{array}{rl}M{2}_1^{({\alpha }_i)}& ={}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_{l-2}}^{(l-3)}-{}^G{\hat{v}}_{I_{l-2}}^{(l-3)}(\Delta t_{l-1}+\Delta t_{l-2})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{l-2}{)}^2\\ & +{}^G{\hat{p}}_{I_{l-2}}^{(l-3)}-{}^G{\hat{p}}_{I_{l-3}}^{(l-4)}-{}^G{\hat{v}}_{I_{l-3}}^{(l-4)}\Delta t_{l-3}-\frac{1}{2}g\Delta t_{l-3}^2\\ & +{}^G{\hat{v}}_{I_{l-2}}^{(l-3)}(\Delta t_{l-1}+\Delta t_{l-2})-{}^G{\hat{v}}_{I_{l-3}}^{(l-4)}(\Delta t_{l-1}+\Delta t_{l-2})-g\Delta t_{l-3}(\Delta t_{l-1}+\Delta t_{l-2})\\ & ={}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_{l-3}}^{(l-4)}-{}^G{\hat{v}}_{I_{l-3}}^{(l-4)}(\Delta t_{l-1}+\Delta t_{l-2}+\Delta t_{l-3})-\frac{1}{2}g(\Delta t_{l-1}+\Delta t_{l-2}+\Delta t_{l-3}{)}^2\end{array}$$
将以上结果回代入公式（33）可得：
$$M{2}^{({\alpha }_i)}={\left[\begin{array}{c}\lfloor {}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_{l-3}}^{(l-4)}-{}^G{\hat{v}}_{I_{l-3}}^{(l-4)}(\Delta t_{1-3})-\frac{1}{2}g(\Delta t_{1-3}{)}^2{\rfloor }_{\times }({}_G^{I_{l-3}}{R}^{(l-4)}{)}^T\\ -\mathbf{I}\\ -\mathbf{I}\Delta t_{1-3}\end{array}\right]}^T\text{\hspace{1em}(34)}$$
可以预见到，一直乘积下去的结果最终的形式和公式（22）一样，如下：
$${\hat{\mathcal{O}}}_l=J_{(f_j|l)}{}_I^{C}R{}_G^{I_l}{\hat{R}}^{(l-1)}\left[\begin{array}{c}\underset{IMU}{\underbrace{{\left[{}^G{\hat{\mathrm{p}}}_{f_j}-{}^G{\hat{p}}_{I_k}^{(k-1)}-{}^G{\hat{v}}_k^{(k-1)}\Delta t-\frac{1}{2}g\Delta t_{k-{\alpha }_i}^2\right]}_{\times }({}_G^{I_k}{\hat{R}}^{(k-1)}{)}^T,-\mathbf{I},-\mathbf{I}\Delta t_{k-{\alpha }_i}}}\\ \underset{feature}{\underbrace{\dots ,\mathbf{I},\dots ,0}}\end{array}\right]\text{\hspace{1em}(35)}$$
公式中最后递推到了第 k 个节点，使用的是 $k-1$ 时刻的估计值。

所以整个系统的零空间为：
$$\hat{\mathbf{N}}=\left[\begin{array}{cc}{0}_3& {}_G^{I_k}{\hat{\mathbf{R}}}^{(k-1)}\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\hat{\mathbf{p}}}_k^{(k-1)}\right]}_{\times }\mathbf{g}\\ 0_3& -{\left[{}^G{\hat{\mathbf{v}}}_k^{(k-1)}\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\hat{\mathbf{p}}}_{f_1}\right]}_{\times }\mathbf{g}\\ {\mathbf{I}}_3& -{\left[{}^G{\hat{\mathbf{p}}}_{f_2}\right]}_{\times }\mathbf{g}\\ ⋮& ⋮\\ {\mathbf{I}}_3& -{\left[{}^G{\hat{\mathbf{p}}}_{f_N}\right]}_{\times }\mathbf{g}\end{array}\right]\text{\hspace{1em}(36)}$$

 

---

关于零空间的一些理解

对比公式（36）和公式（23），不难发现：

1. 公式（23）的零空间的基底使用真值表示，亦即这些值不再随时间变化了；
2. 公式（36）的零空间的基底使用的是 k-1​ 时刻估计出的 k 节点的值表示，k 节点此时的值并不一定是最优的！

所以两个零空间一样么？显然是不一样的，但是两个零空间的物理意义却是相同的！

具体而言：

• 前三维影响IMU系的位置和特征点的位置，相当于把整个系统shift起来了；
• 最后一个维度影响以重力轴为旋转轴的旋转，也就是世界系的yaw方向，也相当于在yaw方向上shift了整个系统；

这部分读者感兴趣可以看参考1中的附录部分。

其实整个推导下来之后，笔者认为FEJ更多的其实是通过固定节点的优化方向，进而保证整个线性系统能观矩阵的零空间一直保持一致，但是需要明确的是，该优化方向与零空间并不是正交的。

进一步可以看到，其实该零空间是第一个观测方程的零空间，即在 k 时刻，整个能观性矩阵为：
$${\hat{\mathcal{O}}}^{(k)}=\left[{\mathrm{H}}_k^{(k-1)}\right]\text{\hspace{1em}(37)}$$
而该能观性矩阵的零空间就是如公式（36）所示的零空间；

所以FEJ维护的零空间在笔者看来其实就是以 k 时刻为起点的系统，在 k 时刻的零空间，从实际的角度来说，当系统在 k 时刻的状态确定了，那么整个系统的位置和yaw轴不可观其实都是跟起点紧密相关的（相当于把起点shift起来了），而后面使用FEJ都在维护这样的状态；

 

---

总结

本文较为详细的推导了：

1. 不同线性化点是如何影响整个能观性的；
2. FEJ为什么可以保持整个系统的能观性不受影响；
3. FEJ不仅保持零空间的维度，同时保持了零空间的物理意义不变了；

还有需要说明的是：本文在解决能观性不同的问题上没有使用参考1中的方法，不过参考1中的方法将整个旋转部分去除的方法确实很强，篇幅所限这里就不展开了，感兴趣的可以自行阅读一下～

下一篇笔者打算写一下强制让优化方向与零空间正交的OC-KF的方法，该方法也是开源工程S-MSCKF中使用的方法。