火柴的初心的博客

PVQ 增量的误差、 协方差及 Jacobian 根据协方差矩阵的性质： y=Ax,x∈N(0,Σx)Σy=AΣxAT \begin{array}{c} y = Ax, x\in N\left(0, \Sigma _{x}\right) \\ \Sigma_y = A \Sigma_{x}A^{T} \end{array} y=Ax,x∈N(0,Σx​)Σy​=AΣx​AT​ 则相邻时刻误差的线性传递方程： ηik=Fk−1ηik−1+Gk−1nk−1 \eta_{ik} = F_{k-1} \eta_{i

PVQ 增量的误差、 协方差及 Jacobian 5.离散形式 PVQ 增量误差在离散形式下的矩阵形式： [δαk+1δθk−1δβk+1δbak+1δbwk+1]=[If12δtf14f150f2200−δt0f32If34f35000I00000I][δαkδθkδβkδbakδbwk]+[g11g12g13g14000−12δt0−12δt00−12Rkδtg32−12Rkδtg34000000δt000000δt][naknwknak+1nwk+1nbnnbw]=F[δαkδθkδβkδbakδbw

​的状态改变并不会对其产生影响，因此将其作为非线性优化变量，可以避免状态的重复传递。，这将导致的问题是：当每次迭代时，我们得到一个新的bais ，还需要根据预积分项。解决方法：当bias变换很小时，对bias进行一阶近似调整。对第k帧和第k+1帧之间的IMU预积分，时刻的状态有关，做优化时需要迭代更新第。与IMU的bais有关，而。时刻的状态，导致需要每次。帧的位置、速度以及旋转。进行中值积分，然后得到。进行积分，可以得到第。计算积分，为了不要使。，避免重新计算，但是。

残差 由预积分 [pwbjqwbjvjwbjabjg]=[pwbi+viwΔt−12gwΔt2+qwbiαbibjqwbiqbibjviw−gwΔt+qwbiβbibjbiabig] \left[\begin{array}{c} \mathbf{p}_{w b_{j}} \\ \mathbf{q}_{w b_{j}} \\ \mathbf{v}_{j}^{w} \\ \mathbf{b}_{j}^{a} \\ \mathbf{b}_{j}^{g} \end{array}\right]=\left[\begi