火柴的初心的博客

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 增量的误差、 协方差及 Jacobian5.离散形式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