代码下载：

https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/第九章 基于优化的建图方法

shenlan_ws 为按照课程ppt公式推导的代码

shenlan2_ws 代码松鹏哥

分别为：位置残差、姿态残差、速度残差，accel_bias 残差， gyro_bias 残差 [ r p r q r v r b a r b g ] = [ q w b i ? ( p w b j ? p w b i ? v i w Δ t 1 2 g w Δ t 2 ) ? α b i b j 2 [ q b i b j ? ? ( q w b i ? ? q w b j ) ] x y z q w b i ? ( v j w ? v i w g w Δ t ) ? β b i b j b j a ? b i a b j g ? b i g ] \begin{bmatrix} r_p \\ r_q \\ r_v \\ r_{ba} \\ r_{bg} \end{bmatrix} = \begin{bmatrix} q_{wb_i}^{*}(p_{wb_j} - p_{wb_i} - v_i^w \Delta t \frac{1}{2}g^w \Delta t^2) - \alpha_{b_i b_j} \\ 2[q_{b_i b_j}^{*} \bigotimes (q_{wb_i}^{*} \bigotimes q_{w b_j})]_{xyz} \\ q_{w b_i}^{*}(v_j^w - v_i^w g^w \Delta t) - \beta_{b_i b_j} \\ b_j^a - b_i^a \\ b_j^g - b_i^g \end{bmatrix} ??????rprqrv​rba​rbg​​⎦⎥⎥⎥⎥⎤​=⎣⎢⎢⎢⎢⎡​qwbi​∗​(pwbj​​−pwbi​​−viw​Δt+21​gwΔt2)−αbi​bj​​2[qbi​bj​∗​⨂(qwbi​∗​⨂qwbj​​)]xyz​qwbi​∗​(vjw​−viw​+gwΔt)−βbi​bj​​bja​−bia​bjg​−big​​⎦⎥⎥⎥⎥⎤​

分别为：

i时刻的相对位置，i时刻的相对姿态，i时刻的速度，i时刻的accel_bias，i时刻的gyto_bias

j时刻的相对位置，j时刻的相对姿态，j时刻的速度，j时刻的accel_bias，j时刻的gyto_bias [ p w b i q w b i v i w b i a b i g ] [ p w b j q w b j V j w b j a b j g ] \left[\begin{array}{lllll} \mathbf{p}_{w b_{i}} & \mathbf{q}_{w b_{i}} & \mathbf{v}_{i}^{w} & \mathbf{b}_{i}^{a} & \mathbf{b}_{i}^{g} \end{array}\right]\left[\begin{array}{lllll} \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] [pwbi​​​qwbi​​​viw​​bia​​big​​][pwbj​​​qwbj​​​Vjw​​bja​​bjg​​] 但在实际使用中，往往都是使用扰动量，因此实际是以下变量求雅克比 [ δ p w b i δ θ w b i δ v i w δ b i a δ b i g ] [ δ p w b j δ θ w b j δ v j w δ b j a δ b j g ] \begin{aligned} &{\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_{i}} & \delta \theta_{w b_{i}} & \delta \mathbf{v}_{i}^{w} & \delta \mathbf{b}_{i}^{a} & \delta \mathbf{b}_{i}^{g} \end{array}\right]} \\ &{\left[\begin{array}{lllll} \delta \mathbf{p}_{w b_{j}} & \delta \theta_{w b_{j}} & \delta \mathbf{v}_{j}^{w} & \delta \mathbf{b}_{j}^{a} & \delta \mathbf{b}_{j}^{g} \end{array}\right]} \end{aligned} ​[δpwbi​​​δθwbi​​​δviw​​δbia​​δb