为什么不能用2dof的姿态估计：因为平面假设只是近似满足，实际还是有很多不平整、颠簸、低高度从而可驶过的地板等

为什么不能直接用3dof的姿态估计：因为运动受限（近似平面运动），从而导致额外的不可观的自由度

一个单目系统，6自由度位姿和1自由度尺度不可观

一个单目IMU系统，3自由度平移（trans）和1自由度旋转（yaw）不可观，但是这是在IMU有足够的3D运动的时候情况

在近似平面假设的自动驾驶车辆上，额外的一个自由度变得不可观了，这就是尺度

这样的情况例如：车辆在恒定加速度下运动，特别的，在完全已知陀螺仪偏差bg的简化假设下，对于某些运动，VINS的初始状态不能唯一的确定

我们来看一看，在更一般的情况下：未知的陀螺仪偏差bg，来确定不可观的自由度

单目VINS系统，状态向量：

这里写的比较乱，其实就是pvq bg ba  和逆深度，不知道为什么这里没写外参bcb

还要注意的是，q是G（大地坐标）在I（IMU坐标，或者叫机体坐标的位置），而p和v是更好理解的I在G中的位置

为什么q要这么写？继续往下看

imu的测量值，显然是在imu坐标系下的，我们看到，真实的imu对地加速度减去重力加速度（对地），肯定是在大地坐标系也就是g系下，要将其转换到imu坐标，需要的是在I系的基向量在g系中表示，那为什么用的q是G系在I中的表示呢？这里也留下一个疑问

而且这里为什么状态只有一帧的，如果这是状态的一部分，为什么特征点深度也在这里？

根据上面这个，推导连续系统微分方程：

基本就是由刚才那两个imu方程写出来的 非常简单

然后是图像特征：

zj可以理解为归一化平面的像点，z=1所以省略，fj本来是在g系的图像坐标，减去自车位置P，再进行自车旋转，并用外参C变换到相机坐标系

那么，在恒定加速度情况下：

定理一：这个VINS系统除了3轴平移和yaw不可观以外，还有一个不可观的方向，当且仅当刚才这个条件5满足

这个不可观的方向就是尺度

证明：

我们来检验线性化VINS模型的能观性矩阵的右零空间，为了表示清楚，我们只在状态向量中包含一个特征点(扩展到多个特征点很简单)

能观性矩阵的任意块行是这样的结构：

下面补充一下知识：

下面的 x k+1|k 的这种表示 应该是条件概率 

另外，下面这两个传播和测量方程写得可能有些迷惑人，但实际上 第一个矩阵就是误差状态转移矩阵雅克比，第二个矩阵也就是H就是观测矩阵雅克比

---

而能观性矩阵的零空间为：（以下的O我确认了就是上面的M），注意M有m行块，这个m是啥？就是观测方程的个数，也就是特征点的数量

注意上面这句话，rankM就是能观维度，rankN就是不能观维度

以EKF为例

以EKF为例，L是一个时刻，在这一时刻观测到了第i个特征点，那么就有Hil的定义 看，对于N的零空间的任意一列，都有对于任意i（也就是O的任意一行）和L（时刻，也就是不同时刻不同的O）都有ON=0

---

回到刚才的Mk，其中Ti矩阵的定义就不写在这里了，从这个Mk中可以得到：想要尺度维不可观，当且仅当对于任意的K，都有MkNs=0

而Ns是这样的

论文原话是：想要尺度维，Ns，不可观的充分必要条件是MKNS=0

这里的NS构建就是根据状态向量构建的，第一个0对应q旋转 第二个0对应bg陀螺仪偏置，第三个对应速度，第四个对应ba加速度偏置，这里应该是认为在恒定加速度假设下，加速度偏置就等于恒定的加速度，这个似乎是很自然的

然后我们就看看，MKNS是否真的等于0

32等式成立是由于相机投影矩阵（观测矩阵）的雅克比Hck的右零空间是imu坐标下的特征点位置，完全看不懂在说什么，但是在公式上我们注意到，如果我们有定理“Hck的右零空间是imu坐标系下的特征点位置”，从32看出，特征点f在g中的位置-当前imu坐标系在g中的位置就等于特征点f在i中的位置，那就必有Hck * f=0

而且，这个新的N的零空间的向量Ns是和之前的4个不可观维度N1不一样的，因为N1的第四个块元是0，而Ns中不是，所以这时一个新的不可观维度

我们来整理一下刚刚到底做了什么

首先我们在时刻L有一个m行的能观性矩阵M或者叫O，对于VINS系统，我们已经知道零空间有四个维度，就是N1，然后现在我们再构建一个N向量叫做Ns，这个向量是根据我们目前的假设（系统在恒定加速度下运动）而构建的，那么接下来我们就要证明MkNs总是等于0（对于M的任意一行），最终，通过一系列计算，和定理“Hck的右零空间是imu坐标系下的特征点位置”，我们证明了这一点，而且我们确认Ns并不在N1撑出的空间之中，因为Ns第四维不等于0，而N1所有向量第四维都是0，所以我们找到了一个新的不可观向量

证明结束

注意，这个不可观的维度对应的就是尺度

再额外放一点，

下面这是，滑动窗口中，或者说理想状态是在真实值附近展开，而现在只能在当前估计下展开，从而会导致能观性变化

要记住的就只是，在这种真实情况下，比起理想，能观性矩阵多了一下Δ，叫做扰动项，所以O的零空间最后一列（也就是零空间最后一列的Ni，在理想状态下零空间一共有4列，现在最后一列不再满足Ni*M=0了）不再满足，在这里就是绕重力的旋转从不可观变成可观，因为它不在零空间中了

前者是状态转移得到的首次估计，后者是观测更新后的估计

回到正文：

这个结论的物理解释是，当局部加速度不变是，人们无法区分真实机体加速度的大小和加速度偏置的大小，因为他们暂时都是不变的，从而真实加速度大小可以是任意的，所以导致了尺度的模糊

我们应该要意识到，在实践中，自动驾驶车辆都是恒定加速度的，比如沿着直线恒速行驶（这时，恒定加速度就是重力加速度）或者恒加速度行驶，或者沿着恒定速度圆弧转弯，这导致了VINS系统尺度的模糊（或者叫不可观）

来看第二种情况，零旋转

例如，在一段时间上，旋转保持不动

定理二：当且仅当7满足，VINS系统就有以下不可观方向（或者说M的零空间多了一个向量No）

这个的证明暂时不写了

注意，这时的不可观测方向是所有三个自由度的全局旋转方向，而不仅仅是yaw

物理解释：当没有旋转时，无法从恒定的旋转中区分局部重力加速度的方向和加速度偏置的方向，从而roll和pitch都变得不可观（本来yaw就不可观了，所以所有三个自由度旋转都不可观）

也就是当自动驾驶车辆纯直线运动的时候，由于能观性的缺失，旋转估计变得不准确

结论：当车辆恒定加速度或者纯直线运动的时候，会有额外的不可观方向。我们要注意到，在实践中并不会有绝对的恒加速度或者绝对的直线运动，但当汽车近似于这种运动的时候，其在不可观方向得到的信息很少，从而信息矩阵是病态的，甚至缺秩，从而影响定位性能

解决方式，允许车辆偏离直线路径可以解决全局定向问题，但是尺度问题不太好解决，需要车辆不断的改变加速度，这不现实也会造成机构磨损，所以我们选择使用轮速计来解决这个问题