项目地址VINS-GPS-Wheel，欢迎交流学习。 VINS-Mono集成轮式编码器和GPS（一）：预积分 VINS-Mono集成轮式编码器和GPS(二):鲁棒的初始化 VINS-Mono集成轮式编码器和GPS(三):后端优化 VINS-Mono集成轮式编码器和GPS(四):融合GPS

前两篇博客介绍了带轮速计的预积分，并利用轮速计的预积分数据改进了初始化过程。本文介绍了如何构建它IMU与轮速计的残差，以及求导过程。

min X { ∥ r p ? J p X ∥ 2 ∑ k ∈ B ∥ r B ( z ^ b k 1 b k , X ) ∥ P b k 1 b k 2 ∑ ( l , j ) ∈ C ∥ r C ( z ^ l C j , X ) ∥ P l C j 2 } \min_{X}\left \{ \left \| r_p-J_pX \right \|^2 \sum_{k\in B}\left \| r_B(\hat{z}_{b_{k 1}}^{b_k}, X) \right \|_{P_{b_{k 1}}^{b_k}}^2 \sum_{(l,j)\in C}\left \| r_C(\hat{z}_{l}^{C_j}, X) \right \|_{P_l^{C_j}}^2 \right \} Xmin???∥rp?JpX∥2+k∈B∑​∥∥∥​rB​(z^bk+1​bk​​,X)∥∥∥​Pbk+1​bk​​2​+(l,j)∈C∑​∥∥∥​rC​(z^lCj​​,X)∥∥∥​PlCj​​2​⎭⎬⎫​ r B ( z ^ b k + 1 b k , X ) = [ δ α b k + 1 b k δ θ b k + 1 b k δ β b k + 1 b k δ η b k + 1 b k δ b a δ b g ] = [ R w b k ( p b k + 1 w − p b k w − v b k w Δ t k + 1 2 g w Δ t k 2 ) − α b k + 1 b k 2 [ ( γ ~ b k + 1 b k ) − 1 ⊗ q b k w − 1 ⊗ q b k + 1 w ] v e c R w b k ( v k + 1 w − v k w + g w Δ t k ) − β b k + 1 b k R w b k ( p b k + 1 w − p b k w ) − p o b + R w b k R b k + 1 w p o b − η b k + 1 b k b a k + 1 − b a k b ω k + 1 − b ω k ] r_B(\hat{z}_{b_{k+1}}^{b_k}, X)=\begin{bmatrix} \delta \alpha _{b_{k+1}}^{b_k} \\ \delta \theta _{b_{k+1}}^{b_k}\\ \delta \beta _{b_{k+1}}^{b_k}\\ \delta \eta _{b_{k+1}}^{b_k}\\ \delta b_{a}\\ \delta b_{g} \end{bmatrix}=\begin{bmatrix} R_{w}^{b_k}(p_{b_{k+1}}^w-p_{b_k}^w-v_{b_k}^w\Delta t_k+\frac{1}{2}g^w\Delta t_k^2)-\alpha _{b_{k+1}}^{b_k}\\ 2\left [ (\tilde{\gamma}_{b_{k+1}^{b_k}} )^{-1}\otimes {q_{b_k}^w}^{-1}\otimes q_{b_{k+1}}^{w} \right ]_{vec} \\ R_{w}^{b_k}(v_{k+1}^w-v_k^w+g^w\Delta t_k)-\beta_{b_{k+1}}^{b_k}\\ R_{w}^{b_k}(p_{b_{k+1}}^w-p_{b_k}^w)-p_o^b+R_w^{b_k}R_{b_{k+1}}^{w}p_o^b - \eta_{b_{k+1}}^{b_k}\\ b_{a_{k+1}}-b_{a_k}\\ b_{\omega _{k+1}}-b_{ \omega _k} \end{bmatrix} rB​(z^bk+1​bk​​,X)=⎣⎢⎢⎢⎢⎢⎢⎢⎡​δαbk+1​bk​​δθbk+1​bk​​δβbk+1​bk​​δηbk+1​bk​​δba​δbg​​⎦⎥⎥⎥⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎢⎢⎢⎢⎡​Rwbk​​(pbk+1​w​−pbk​w​−vbk​w​Δtk​+21​gwΔtk2​)−αbk+1​bk​​2[(γ~​bk+1bk​​​)−1⊗qbk​w​−1⊗q