零偏更新以及预积分的更新

中值积分形式的预积分（一）：https://blog.csdn.net/ergevv/article/details/143165323?spm=1001.2014.3001.5501
中值积分形式的预积分（二）：https://blog.csdn.net/ergevv/article/details/143262065?spm=1001.2014.3001.5502
零偏更新以及预积分的更新：https://blog.csdn.net/ergevv/article/details/143274896?spm=1001.2014.3001.5501

接前文：中值积分形式的预积分（二）：https://blog.csdn.net/ergevv/article/details/143262065?spm=1001.2014.3001.5502

前文介绍的预积分的协方差递推公式的推导，里面有一个矩阵是F，根据之前的推导可以知道，F矩阵里每个公式都代表着前一刻状态量的变化是如何影响到下一刻的，即：

F = δ x k + 1 δ x k = [ I f 12 I δ t − 1 4 ( q b i b k + q b i b k + 1 ) δ t 2 f 15 0 I − [ ω ] × d t 0 0 − I δ t 0 f 32 I − 1 2 ( q b i b k + q b i b k + 1 ) δ t f 35 0 0 0 I 0 0 0 0 0 I ] F = \frac {\delta x_{k+1}}{\delta x_{k}}=\left[ \begin{matrix} I &f_{12}& I\delta t& -\frac{1}{4}(q_{bibk}+q_{bibk+1})\delta t^2 &f_{15} \\ 0 & I-[ω]_×dt &0&0&-I \delta t\\ 0& f_{32}& I& -\frac{1}{2}(q_{bibk}+q_{bibk+1})\delta t & f_{35}\\ 0& 0& 0& I& 0\\ 0 & 0& 0 &0 & I\\ \end{matrix} \right] F=δxk​δxk+1​​= ​I0000​f12​I−[ω]×​dtf32​00​Iδt0I00​−41​(qbibk​+qbibk+1​)δt20−21​(qbibk​+qbibk+1​)δtI0​f15​−Iδtf35​0I​ ​

预积分过程中零偏是固定的，但是在后续优化过程中，零偏会改变，可能会更加接近正确值，因此当零偏改变时，需要对预积分量进行更新。但是当零偏发生变化时，若仍按照前述公式，预积分测量值需要整个重新计算一遍，这个计算成本是比较大的。为了解决这个问题，需要利用线性化来进行零偏变化时预积分项的一阶近似更新方法。

更新后的值 = 更新前的值 + 误差变化量 更新后的值 = 更新前的值 + 误差变化量 更新后的值=更新前的值+误差变化量
exp ⁡ ( [ θ k + 1 n e w ] × ) ≈ exp ⁡ ( [ θ k + 1 o l d ] × ) exp ⁡ ( [ ∂ δ θ k + 1 o l d ∂ δ b 0 g Δ b i g ] × ) α k + 1 n e w ≈ α k + 1 o l d + ∂ δ α k + 1 o l d ∂ δ b 0 g Δ b i g + ∂ δ α k + 1 o l d ∂ δ b 0 a Δ b i a β k + 1 n e w ≈ β k + 1 o l d + ∂ δ β k + 1 o l d ∂ δ b 0 g Δ b i g + ∂ δ β k + 1 o l d ∂ δ b 0 a Δ b i a \begin{align*} \exp([\theta^{new}_{k+1}]_\times) &\approx \exp([\theta^{old}_{k+1}]_\times) \exp([\frac{\partial \delta \theta^{old}_{k+1}}{\partial \delta b^g_0}\Delta b^g_i]_\times) \\ \alpha^{new}_{k+1}& \approx \alpha^{old}_{k+1} + \frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^g_0}\Delta b^g_i + \frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^a_0}\Delta b^a_i \\ \beta^{new}_{k+1}& \approx \beta^{old}_{k+1} + \frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^g_0}\Delta b^g_i + \frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^a_0}\Delta b^a_i \\ \end{align*} exp([θk+1new​]×​)αk+1new​βk+1new​​≈exp([θk+1old​]×​)exp([∂δb0g​∂δθk+1old​​Δbig​]×​)≈αk+1old​+∂δb0g​∂δαk+1old​​Δbig​+∂δb0a​∂δαk+1old​​Δbia​≈βk+1old​+∂δb0g​∂δβk+1old​​Δbig​+∂δb0a​∂δβk+1old​​Δbia​​

现在目的是求出 ∂ δ θ k + 1 o l d ∂ δ b 0 g 、 ∂ δ α k + 1 o l d ∂ δ b 0 g 、 ∂ δ α k + 1 o l d ∂ δ b 0 a 、 ∂ δ β k + 1 o l d ∂ δ b 0 g 、 ∂ δ β k + 1 o l d ∂ δ b 0 a \frac{\partial \delta \theta^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^a_0}、\frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^a_0} ∂δb0g​∂δθk+1old​​、∂δb0g​∂δαk+1old​​、∂δb0a​∂δαk+1old​​、∂δb0g​∂δβk+1old​​、∂δb0a​∂δβk+1old​​，因为零偏是从一开始就存在的，所以可以看到这是预积分结束后的误差状态量对一开始的零偏的求导，即 δ x k + 1 δ x 0 \frac {\delta x_{k+1}}{\delta x_{0}} δx0​δxk+1​​。
假设存在雅可比矩阵
J = δ x k δ x 0 = [ ∂ δ α k ∂ δ α 0 ∂ δ α k ∂ δ θ 0 ∂ δ α k ∂ δ β 0 ∂ δ α k ∂ δ b 0 a ∂ δ α k ∂ δ b 0 g ∂ δ θ k ∂ δ α 0 ∂ δ θ k ∂ δ θ 0 ∂ δ θ k ∂ δ β 0 ∂ δ θ k ∂ δ b 0 a ∂ δ θ k ∂ δ b 0 g ∂ δ β k ∂ δ α 0 ∂ δ β k ∂ δ θ 0 ∂ δ β k ∂ δ β 0 ∂ δ β k ∂ δ b 0 a ∂ δ β k ∂ δ b 0 g ∂ δ b k a ∂ δ α 0 ∂ δ b k a ∂ δ θ 0 ∂ δ b k a ∂ δ β 0 ∂ δ b k a ∂ δ b 0 a ∂ δ b k a ∂ δ b 0 g ∂ δ b k g ∂ δ α 0 ∂ δ b k g ∂ δ θ 0 ∂ δ b k g ∂ δ β 0 ∂ δ b k g ∂ δ b 0 a ∂ δ b k g ∂ δ b 0 g ] J = \frac {\delta x_{k}}{\delta x_{0}}= \left[ \begin{matrix} \frac{\partial \delta \alpha_{k}}{\partial \delta \alpha_0} &\frac{\partial \delta \alpha_{k}}{\partial \delta \theta_0} & \frac{\partial \delta \alpha_{k}}{\partial \delta \beta_0} & \frac{\partial \delta \alpha_{k}}{\partial \delta b^a_0} & \frac{\partial \delta \alpha_{k}}{\partial \delta b^g_0}\\ \frac{\partial \delta \theta_{k}}{\partial \delta \alpha_0} &\frac{\partial \delta \theta_{k}}{\partial \delta \theta_0} & \frac{\partial \delta \theta_{k}}{\partial \delta \beta_0} & \frac{\partial \delta \theta_{k}}{\partial \delta b^a_0} & \frac{\partial \delta \theta_{k}}{\partial \delta b^g_0}\\ \frac{\partial \delta \beta_{k}}{\partial \delta \alpha_0} &\frac{\partial \delta \beta_{k}}{\partial \delta \theta_0} & \frac{\partial \delta \beta_{k}}{\partial \delta \beta_0} & \frac{\partial \delta \beta_{k}}{\partial \delta b^a_0} & \frac{\partial \delta \beta_{k}}{\partial \delta b^g_0}\\ \frac{\partial \delta b^a_{k}}{\partial \delta \alpha_0} &\frac{\partial \delta b^a_{k}}{\partial \delta \theta_0} & \frac{\partial \delta b^a_{k}}{\partial \delta \beta_0} & \frac{\partial \delta b^a_{k}}{\partial \delta b^a_0} & \frac{\partial \delta b^a_{k}}{\partial \delta b^g_0}\\ \frac{\partial \delta b^g_{k}}{\partial \delta \alpha_0} &\frac{\partial \delta b^g_{k}}{\partial \delta \theta_0} & \frac{\partial \delta b^g_{k}}{\partial \delta \beta_0} & \frac{\partial \delta b^g_{k}}{\partial \delta b^a_0} & \frac{\partial \delta b^g_{k}}{\partial \delta b^g_0}\\ \end{matrix} \right] J=δx0​δxk​​= ​∂δα0​∂δαk​​∂δα0​∂δθk​​∂δα0​∂δβk​​∂δα0​∂δbka​​∂δα0​∂δbkg​​​∂δθ0​∂δαk​​∂δθ0​∂δθk​​∂δθ0​∂δβk​​∂δθ0​∂δbka​​∂δθ0​∂δbkg​​​∂δβ0​∂δαk​​∂δβ0​∂δθk​​∂δβ0​∂δβk​​∂δβ0​∂δbka​​∂δβ0​∂δbkg​​​∂δb0a​∂δαk​​∂δb0a​∂δθk​​∂δb0a​∂δβk​​∂δb0a​∂δbka​​∂δb0a​∂δbkg​​​∂δb0g​∂δαk​​∂δb0g​∂δθk​​∂δb0g​∂δβk​​∂δb0g​∂δbka​​∂δb0g​∂δbkg​​​ ​
因为 F = δ x k + 1 δ x k F = \frac {\delta x_{k+1}}{\delta x_{k}} F=δxk​δxk+1​​， J = δ x k δ x 0 J = \frac {\delta x_{k}}{\delta x_{0}} J=δx0​δxk​​，所以：
δ x k + 1 δ x 0 = δ x k + 1 δ x k ∗ δ x k δ x 0 = F J \frac {\delta x_{k+1}}{\delta x_{0}} = \frac {\delta x_{k+1}}{\delta x_{k}} * \frac {\delta x_{k}}{\delta x_{0}} = FJ δx0​δxk+1​​=δxk​δxk+1​​∗δx0​δxk​​=FJ
因此，每次预积分时只需要更新 F J FJ FJ，当需要零偏更新，需要重新计算预积分量时，只需要到 F J FJ FJ去提取 ∂ δ θ k + 1 o l d ∂ δ b 0 g 、 ∂ δ α k + 1 o l d ∂ δ b 0 g 、 ∂ δ α k + 1 o l d ∂ δ b 0 a 、 ∂ δ β k + 1 o l d ∂ δ b 0 g 、 ∂ δ β k + 1 o l d ∂ δ b 0 a \frac{\partial \delta \theta^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \alpha^{old}_{k+1}}{\partial \delta b^a_0}、\frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^g_0}、\frac{\partial \delta \beta^{old}_{k+1}}{\partial \delta b^a_0} ∂δb0g​∂δθk+1old​​、∂δb0g​∂δαk+1old​​、∂δb0a​∂δαk+1old​​、∂δb0g​∂δβk+1old​​、∂δb0a​∂δβk+1old​​，再代入零偏变化时预积分项的一阶近似更新公式