zysss_的博客

这里将从IMU是如何在EKF中使用为主流程然后去逐步讲解里面的公式推导，即从应用出发返回去理解公式。

里面，所以第二次优化时相对约束中就会带有尺度信息进行优化，虽然里面也包含了GPS的抖动误差，因为会把世界系变换到真实尺度下面去，毕竟相对约束里面要的是相对值，世界系去到哪里都无所谓。表示同一时刻的值，所以这个是一个局部变换，因为GPS自身就存在很大的抖动，然后后面这个系统还会把这个大抖动乘上去重复上面的步骤，其实就是去靠近这个抖动值。，即相同世界系，那么我感觉这个值其实就是一个稳定值的VIO值到一个抖动的GPS均值的变化，然后把这个值当作GPS到VIO的外参。

滑窗中固定只能有11帧，当来了新的帧的时候旧的帧就需要抹掉，但是不能直接把旧的帧的全部信息抹掉，因为旧的帧的经历过11次优化，其地图点中的优化信息是有用，边缘化的操作就是为了只把最旧帧的变量去掉，但是保留其对应的优化信息留在窗口里面，因为那些信息仍然具有约束作用。

用逆深度是因为这样可以在优化中从优化3个变量降低到1个，降低优化的维度加快求解速度用逆深度是因为当距离很远的时候，x1​x就会无穷大，而3D点很近的情况也一般不会有，这也是为了数值稳定性用逆深度的话就要和其中一帧进行绑定，这个就是和观测到该点的第一帧进行绑定，这样才能表示一个3D点信息划窗中维护的全部都是IMU下的位姿，所以相机要通过外参变换到IMU坐标系下这里就构成了视觉误差，需要求关于优化变量的雅可比矩阵，这里约束了第i帧和第j。

δα这里是求预积分对约束的参数块进行求导，有这个雅可比矩阵才能进行优化步长的计算，这个是预积分这个约束因子对各个优化变量的求导，后面还有相机的观测残差块中的θ是3维的，但是参数块中的四元数是4维的，因为相减后残差只剩虚部了，但是参数是从4个参数变过来的预积分的残差具体如下，总共有15维的自由度，即y有15维而参数块x， 维护的是k和k1时刻的PQVBaBgP是3维，Q是四元数有4维，因为是过参数化的形式，而VBaBg总共是9维的参数块所以整个参数块x。

也是优化变量，而预积分又与零偏有关系，零偏改变了理论上是要重新积分的，但是由于这个零偏量变化很小，所以这里采用一阶近似的方式来更新预积分，更新的预积分继续作为这个约束，零偏更新建模看这篇文章。是用 IMU 预积分获得的增量（前面用了大篇幅去推导，可以看之前的文章），左边是优化获得的增量，整体就是优化后的值不能离预积分的增量太远，太远的话误差值就会变大。由于现在设置的预积分约束因子，所以前面计算的预积分置信度就是用在这里计算的残差上面，公式为。这个残差计算是每次优化后都会进来计算的，开根号，那么新的残差用。

这里是对两个相乘的旋转矩阵中的其中一个旋转进行求导，上面的左乘扰动例子是对矩阵相乘向量进行求导的，所以可以直接对矩阵进行扰动。用李代数表示旋转会有个问题，就是周期性，就是多个李代数可以对应一个旋转矩阵，如果固定旋转角度在±π时就是唯一对应的。，这个形式比较复杂，工程中不用这个方法，都是用扰动模型，所以只看扰动模型。的导数，这样就变成矩阵对向量的求导，因为扰动量是可以用向量表示的，的正切空间，因为这里李群是9维的，所以切向量也是一个空间。是微量，相当于是对旋转的导数，则等于。的导数性质，所以李代数是李群。

其实这个切平面的向量是可以任意方向的，毕竟绕着重力向量旋转的任意正交向量都符合，这个temp向量只是一个临时向量去固定其中一个平面，找到对应的向量而已。我们一定是在球面上运动的，我们以向之前求出的向量的起点为圆心，9.81作为半径，与球面的切平面取相互正交的两个向量。这样调整的目的是因为，上一节求解的方程中的已知量中也是包含的重力的误差影响在里面的，需要这样对误差调整回来。，这两个向量相乘就是重力方向，这样就能对重力方向进行调整，三个量是必然互相垂直的。是重力归一化的结果，则这两个点乘的结果就是。

整体思想就是根据预积分的公式，把已知量和未知量各放到一边，因为前面的数据都是变换到$c_{0}$下的，不是真正意义上和重力对齐的世界坐标，然后位移和速度的预积分中会用到加速度计获取的重力加速度g，但是这个重力g是没有和世界的重力方向对齐的，所以里面就存在了未知量，通过每组预积分的数据联立起来构成一个大矩阵对重力向量进行求解，这里是求解了重力方向（对于起始帧$c_{0}$的重力方向

拉取完后创建容器，但是为了能在docker中能打开窗口，将使用以下参数，只需要修改名字即可，your_name 改成你想要的名字，必须英文。然后就可以运行容器啦，$containerId 换成容器名字也可以。运行下面这句，把 $containerId 替换为容器的 id。这个时候必然会报错，上步的目的是为了创建有对应参数的容器，然后把下面的3行指令按顺序分别执行在3个终端中。此时进入容器了，再在容器里面运行这句。每次进入容器前都需要先启动容器。查看刚刚创建的容器的 id。然后再运行以下进入容器。

之前估计的旋转等我外参，加速度计的零偏不在这里进行估计，会在后面滑窗优化的时候进行估计。，这个是用视觉算出来旋转乘上外参算出来的，这样就和imu坐标系有关联了，后面才乘上个。这样就可以更新零偏然后知道满足结果为0为止，前面计算的对零偏的雅克比也是在这里这样用的。会比较大，所以才会重新传播，后面都是用雅克比矩阵进行一阶近似来传播的，算出来的是。（这个在前面零偏建模的时候有过推导是怎么来的），然后通过这样的方式更新旋转量。指的是取虚部，因为只有需要发生变化，实部一直都是1，整个公式的未知量只有。

前面预积分对IMU的数据进行预处理，现在需要对视觉的信息进行处理，在VINS中视觉初始化的处理就是使用SFM，但是这里的三角化他没有用opencv给的函数，而是用自己的方法进行三角化。这里SFM的方式就是现在滑窗里面找到枢纽帧，然后枢纽帧和最后一帧进行三角化获得3D点，然后通过PNP计算滑窗中其他关键帧的位姿，同时也三角化出更多新的点，顺序是先从枢纽帧向右再向左，然后再遍历只被中间帧看到的点进行三角化。

最后结果还要检查前三个奇异值，比0大才是有效的，最后一个奇异值一般在0附近，检查的前三个奇异值都接近0的话就证明可能在0空间附近运动，证明激励不足够。左边的矩阵可以定为 A 矩阵，问题则变成 Ax=0 ，A为超定方程，奇异值矩阵为对角矩阵，但不是严格的N×N的对角，具体形式为。也是正交矩阵，只是旋转的方向相反，和上面同理，这里定义。每一行都是 4×4 的矩阵，总共为 4(N+1)×4行，，因为相机到 IMU 的旋转外参不会变化的，公式变成。，因为旋转矩阵不会改变大小，只会改变向量的方向。

这个雅可比的作为是为了在零偏 $b_{a},b_{w}$ 在经过优化数值发生变化后不需要再重新进行预积分，因为实际上 $k+1$ 时刻的预积分是用 $k$ 时刻的零偏 $b$ 来进行的，当经过优化后 $k+1$ 时刻的零偏 $b$ 就会发生变化，理论上 $k+1$ 时刻的预积分应该是用 $k+1$ 时刻的零偏 $b$ 来进行才对，由于是优化后才获得，但是重新进行预积分又非常耗时，所以采用 $f(x+Δb)=f(x)+J^{x}_{b}·Δb$ 这样的方式来进行近似更新，从这个公式可以看出里面就必须要对零偏

因为VINS是一个基于优化框架的VIO，优化问题非常依赖初始值，因为基本都是找离初始值最近的局部最小值，这个值作为全局最优值，初始值越近迭代次数越少，每次迭代都要计算雅可比和求解Hx=g的方差，这样减少计算时间。

由于里面的零偏ba​bw​也是待优化变量，每次求解一次之后ba​bw​都会被改变以满足残差最小，那这样整个积分就得重新积分，这样会很耗时由上节给出的更新公式Δxk1​FΔxk​GnF∈15×15，求协方差矩阵的时候F⋅P⋅FT，此时是3个15×15的矩阵相乘（一个IMU数据），如果每迭代一次就要把所有的IMU数据都这样重新相乘的话计算量会非常大。Δx的方差P怎么来的可以根据一阶泰勒展开来近似，来进行更新，避免重新预积分fx。

为了求出连续时间的变化量，所以要求出连续时间误差量的变化量的导数，因为是误差卡尔曼滤波，所以是对误差量进行求导。，这里面认为零偏是不会变化的，所以没有k+1时刻的零偏的噪声，零偏的导数就是高斯噪声。代入进去，剩下的就是整理归纳系数了，自己归纳一下就能得到上面对应的值了。现在要根据前面连续时间的方程推导出离散下的变化。时刻的误差，代码中也是构造 F ，G矩阵。，现在要求出离散时间下的变化关系是什么，而实际系统的数据肯定是离散的。先列出连续时间积分的角度的导数。先列出连续时间积分的速度的导数。

我们最后拿到IMU的值是离散的，我们要从连续推导离散的一个形式，这个只是一个中间形式，离散的形式才是最终的形式，将在下一节讲解

为什么需要误差卡尔曼滤波旋转的误差量一般为很小的值，通常用旋转向量来表示旋转误差，因为四元数是过参数化的表示形式，用一个4自由度的协方差矩阵来描述一个3自由度旋转就存在问题，为了避免这个问题则引入李代数（旋转向量）来进行表述，由于误差量很小，∈[Π，−Π]∈[Π，−Π] 之间，且在0附近，则可以避免周期性的问题 。

在滤波中，来了图像帧的位姿其实是相当于观测值，两帧图像间大概会有10个IMU数据，图像的频率一般在20Hz~30Hz，IMU的一般为100Hz，当来到第k+1时刻的位姿时，会把相机的位姿和IMU积分得到的位姿同时扔给滤波器中获得置信度最高的位姿，然后把该位姿赋值给第k时刻的变量，循环往复。就是预积分量，此时可以看出这3个量没有任何相邻两帧之间的关系，即没有直接引入相邻两帧的姿态或者速度来作为它的观测值（相机的重投影位姿），在滤波的积分中的第k时刻的PVQ就是融合了相机观测值的结果，

这样做的好处就是，四元数的乘法通过这个矩阵变换变成矩阵的乘法，且这样可以有两个表示形式，两种形式的结果都是一样的。，由于上面需要进行矩阵化的两个向量的实部都是1，在相减后是会被约掉的，然后只剩下。这样就获得了旋转微量的表达式，积分的时候只需要乘上Δt即可。这两个矩阵构造都是有相似的形式，可以整理成比较一致的表达。写成积分后，积分内部的形式和上面求导的形式是一致的。的需要加个负号，所以两个矩阵的构造还是相似的。内部用的是矩阵的乘法，外面是用的四元数乘法。为 k 到 k+1 时刻的姿态的变换，

运算方便，使用9个量描述3自由度旋转（roll，pitch，yaw），引入额外约束（正交矩阵，行列式=1），求导困难，要求导只能对李代数（也是旋转向量）求导，无法对旋转矩阵进行求导。单位四元数才能表示旋转，额外约束的引进就会使得表示旋转也会过参数化，即4个变量描述3个自由度，当需要描述旋转误差的协方差矩阵时，其对应的矩阵为。这里采用了旋转向量进行表示，虽然旋转向量有周期性的问题，但是后面是采用误差卡尔曼进行计算，维护的是一个角度的误差量，误差量都是在0附近的，起码。紧凑的表示旋转，没有额外的约束；