零空间投影

slam中的问题描述

在MSCKF更新模块中，系统的误差方程为 \[ \tilde{z} = z - h(X,P) \approx H_x \tilde{x} + H_f \tilde{p}_f \] 其中X为系统滑窗中的帧集合状态，P为参与更新的3d点集合。此处测量误差其实使用的是重投影误差。因为是一开始使用了系统滑窗帧状态对视觉特征进行三角化得到了3d路标，然后利用这些3d路标投影得到了重投影误差。因此此处上述式子中X与P是相互耦合的，即上述式子中X与P不是无关的，因此不能直接进行EKF更新，此处的做法是消去其中P这一项。

数学原理

对于一个矩阵A，其左零空间的定义为：所有满足\(v^TA=0\)的向量\(v\)的集合。

对于一个类似如下的问题 \[ Y = F_1X_1 + F_2X_2 \] 我们对于上面这个关系式，只想对\(X_1\)进行讨论，不关心\(X_2\)，因此需要把等式转换成与\(X_2\)无关的形式。此处可以分成两种情况：

1. 当\(X_1,X_2\)互相独立时，此时直接把\(X_2\)部分当做常数处理即可；
2. 当\(X_1,X_2\)相关时，此时\(X_2\)部分对\(X_1\)求导导数不为0（两个中一个变了，另一个也会相关的改变）。此时的做法是可以将整个式子投影到\(F_2\)的零空间上，在\(F_2\)的零空间上讨论\(Y\)与\(X_1\)的相关性，此时整个式子与\(X_2\)无关。

此处开始讨论上面的第二种情况，零空间投影。其实很简单，只需要找到\(F_2\)左零空间所有的基向量，然后将这些基向量并列组成一个矩阵A（每一列都是一个基向量）。则可以得到： \[ A^TF_2=0 \] 使用\(A^T\)同时乘以整个等式可以得到 \[ A^TY = A^TF_1X_1 \]

这个操作本质就是将整个等式投影到了\(F_2\)的零空间上，从而消除了等式中\(F_2X_2\)这一项。

• 维度变换讨论

假设Y维度为\((m,1)\)，\(X_1\)维度为\((n_1,1)\)，\(X_2\)维度为\((n_2,1)\)，则整个式子一开始的维度变化如下： \[ (m,1) = (m,n_1)(n_1,1) + (m,n_2)(n_2,1) \] 假设矩阵\(F_2\)不客观的自由度为\(m_1\)（一般是\(F_2\)的行数减去列数即\(m_1=m-n_2\)），则零空间投影过程中系统的维度变化为 \[ (m_1,m)(m,1) = (m_1,m)(m,n_1)(n_1,1) \\ (m_1,1) = (m_1,n_1)(n_1,1) \]

类似slam系统误差方程，可以发现经过零空间投影，残差的维度变成了\((m_1,1)\)。

实际操作（openvins的解释）

一个观测的重投影残差维度为2，因此每个特征的r的维度为\(m=2*观测数量\)，该特征的3d表示的维度为\(n_2=3\ or\ 1\)，因此\(H_f\)矩阵的维度\((m,n_2)\)行数远远大于列数，不可观的零空间维度为\(m-n_2\)，可以通过QR分解对其进行分解（可以发现其中\(Q_2\)部分其实是可以被约掉的） \[ H_f={\begin{bmatrix} Q_1 & Q_2 \end{bmatrix}} {\begin{bmatrix} R_1 \\ 0 \end{bmatrix}} = Q_1R_1 \] 其中${ \[\begin{bmatrix} Q_1 & Q_2 \end{bmatrix}\]

} \(是一个正交矩阵（整体维度\)(m,m)\(），\)R_1\(是一个上三角矩阵（\)(n_2,n_2)\(），\)Q_1\(（维度\)(m,n_2)\(）是\)H_f\(可观的部分，\)Q_2\(部分（维度\)(m,m-n_2)\(）刚好就是\)H_f\(不可观的部分，因此\)A=Q_2$。

对于某个特征f，其在系统中的表示的残差为\(\tilde{p}_f\)，其对应测量残差为\(\tilde{z}_m\)，系统中与该特征关联的状态的误差为\(\tilde{x}_k\)，则零空间投影前的状态误差方程为 \[ \tilde{z}_m \approx H_x \tilde{x} + Q_1 R_1 \tilde{p}_f \] 对上式进行零空间投影可得（此处可以发现新的测量\(z_o\)变成了\(m-n_2\)维度） \[ Q_2^T \tilde{z}_m \approx Q_2^T H_x \tilde{x} \\ \tilde{z}_o \approx H_{o,x} \tilde{x} \] 可以发现经过零空间投影后得到关于特征f的新的系统误差方程，遍历每一个特征，得到整个系统的误差方程（只和系统状态有关，与路标无关的），便可以进行EKF更新了。（对了，上面系统的测量噪声矩阵也需要相应的变换一下）

在openvins的代码中，并没有直接求\(Q_2\)，而是利用givens rotation直接对残差还有\(H_x\)进行变换（相当于乘以\(Q^T\)），然后取出下面\(m-n_2\)行就是最终的结果。

实际操作（我的解释，也是代码中的情况）：

对于 \[ \tilde{z} = z - h(X,P) \approx H_x \tilde{x} + H_f \tilde{p}_f = H_x \tilde{x} + {\begin{bmatrix} Q_1 & Q_2 \end{bmatrix}} {\begin{bmatrix} R_1 \\ 0 \end{bmatrix}} \tilde{p}_f \] 使用givens rotation等价于等式两边同时乘以\(Q^T\)，是为了构造如下形式： \[ Q^T \tilde{z} = Q^T H_x \tilde{x} + {\begin{bmatrix} R_1 \\ 0 \end{bmatrix}} \tilde{p}_f \] 得到上述形式之后，只需要取下面的\(m-n_2\)行就可以得到零空间投影的结果（消去\(\tilde{p}_f\)）。

其实givens rotation在msckf中有不少应用，比如在最后矩阵压缩求解中：此时也会使用上述原理，不过最后保留的是等式的前n行（n是状态的维度），去掉剩下原理上应该为0的行，这样得到系数矩阵H是一个上三角方阵，求解速度会很快。

参考文献

[1] https://zhuanlan.zhihu.com/p/150641671

[2] https://link.zhihu.com/?target=https%3A//docs.openvins.com/pages.html