网页托管网站,国内软件开发培训机构,工伤保险网站,萧山建设局网站视觉SLAM十四讲|【四】误差Jacobian推导
预积分误差递推公式 ω 1 2 ( ( ω b k n k g − b k g ) ( w b k 1 n k 1 g − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^kn_k^g-b_k^g)(w_b^{k1}n_{k1}^g-b_{k1}^g)) ω21((ωbknkg−bkg)(wbk1nk1g−bk1g)) …视觉SLAM十四讲|【四】误差Jacobian推导
预积分误差递推公式 ω 1 2 ( ( ω b k n k g − b k g ) ( w b k 1 n k 1 g − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^kn_k^g-b_k^g)(w_b^{k1}n_{k1}^g-b_{k1}^g)) ω21((ωbknkg−bkg)(wbk1nk1g−bk1g)) 其中 w b k w_b^k wbk为 k k k时刻下body坐标系的角速度 n k g n_k^g nkg为 k k k时刻下陀螺仪白噪声 b k g b_k^g bkg为 k k k时刻下陀螺仪偏置量。 n k a n_k^a nka为 k k k时刻下加速度白噪声 b k a b_k^a bka为 k k k时刻下加速度偏置量。 k 1 k1 k1时刻下记号同理。 q b i b k 1 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T q_{b_i b_{k1}} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T qbibk1qbibk⊗[1,21ωδt]T a 1 2 ( q b i b k ( a b k n b k − b k a ) q b i b k 1 ( a b k 1 n b k 1 − b k 1 a ) ) a \frac{1}{2}(q_{b_i b_{k}} (a_b^k n_b^k -b_k^a) q_{b_i b_{k1}} (a_b^{k1} n_b^{k1} - b_{k1}^a)) a21(qbibk(abknbk−bka)qbibk1(abk1nbk1−bk1a)) α b i b k 1 α b i b k β b i b k δ t 1 2 a δ t 2 \alpha_{b_i b_{k1}} \alpha_{b_i b_{k}} \beta_{b_i b_k} \delta t \frac{1}{2}a \delta t^2 αbibk1αbibkβbibkδt21aδt2 β b i b k 1 β b i b k a δ t \beta_{b_i b_{k1}} \beta_{b_i b_{k}} a\delta t βbibk1βbibkaδt b k 1 a b k a n b k a δ t b_{k1}^a b_k^a n_{b_k^a}\delta t bk1abkanbkaδt b k 1 g b k g n b k g δ t b_{k1}^g b_k^g n_{b_k^g}\delta t bk1gbkgnbkgδt
示例1 f 15 δ α b i b k 1 δ b k g f_{15} \frac{\delta \alpha_{b_i b_{k1}}}{\delta b_k^g} f15δbkgδαbibk1 由上面的递推公式可知 α b i b k 1 α b i b k β b i b k δ t 1 2 a δ t 2 \alpha_{b_i b_{k1}} \alpha_{b_i b_{k}} \beta_{b_i b_k} \delta t \frac{1}{2}a \delta t^2 αbibk1αbibkβbibkδt21aδt2 其中 α b i b k \alpha_{b_i b_{k}} αbibk、 β b i b k δ t \beta_{b_i b_k}\delta t βbibkδt都与 b k g b_k^g bkg无关可以省略而很容易看出 a a a中含有 q b i b k 1 q_{b_i b_{k1}} qbibk1项其中进一步含有对 b k g b_k^g bkg相关的元素必须保留。因此进一步推得 f 15 δ 1 2 a δ t 2 δ b k g f_{15} \frac{\delta \frac{1}{2} a \delta t^2}{\delta b_k^g} f15δbkgδ21aδt2 其中 a 1 2 ( q b i b k ( a b k n b k − b k a ) q b i b k 1 ( a b k 1 n b k 1 − b k 1 a ) ) a \frac{1}{2}(q_{b_i b_{k}} (a_b^k n_b^k -b_k^a) q_{b_i b_{k1}} (a_b^{k1} n_b^{k1} - b_{k1}^a)) a21(qbibk(abknbk−bka)qbibk1(abk1nbk1−bk1a)) q b i b k ( a b k n b k − b k a ) q_{b_i b_{k}} (a_b^k n_b^k -b_k^a) qbibk(abknbk−bka)依然与 b k g b_k^g bkg无关可以省略。 f 15 δ 1 4 q b i b k 1 ( a b k 1 n b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{\delta \frac{1}{4} q_{b_i b_{k1}} (a_b^{k1} n_b^{k1} - b_{k1}^a) \delta t^2}{\delta b_k^g} f15δbkgδ41qbibk1(abk1nbk1−bk1a)δt2 白噪声项不可知拿掉 f 15 δ 1 4 q b i b k 1 ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{\delta \frac{1}{4} q_{b_i b_{k1}} (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta b_k^g} f15δbkgδ41qbibk1(abk1−bk1a)δt2 f 15 δ 1 4 q b i b k 1 ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{\delta \frac{1}{4} q_{b_i b_{k1}} (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta b_k^g} f15δbkgδ41qbibk1(abk1−bk1a)δt2 q b i b k 1 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T q_{b_i b_{k1}} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T qbibk1qbibk⊗[1,21ωδt]T f 15 δ 1 4 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{\delta \frac{1}{4} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta b_k^g} f15δbkgδ41qbibk⊗[1,21ωδt]T(abk1−bk1a)δt2 其中 ω 1 2 ( ( ω b k n k g − b k g ) ( w b k 1 n k 1 g − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^kn_k^g-b_k^g)(w_b^{k1}n_{k1}^g-b_{k1}^g)) ω21((ωbknkg−bkg)(wbk1nk1g−bk1g)) 去除不可知的白噪声项 ω 1 2 ( ( ω b k − b k g ) ( w b k 1 − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^k-b_k^g)(w_b^{k1}-b_{k1}^g)) ω21((ωbk−bkg)(wbk1−bk1g)) 由于 k 1 k1 k1时刻的信息并不知道在此处如果不使用中值积分直接使用初始值有 ω ω b k − b k g \omega \omega_b^k-b_k^g ωωbk−bkg f 15 δ 1 4 q b i b k ⊗ [ 1 , 1 2 ( ω b k − b k g ) δ t ] T ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{\delta \frac{1}{4} q_{b_i b_k} \otimes [1, \frac{1}{2} (\omega_b^k-b_k^g) \delta t]^T (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta b_k^g} f15δbkgδ41qbibk⊗[1,21(ωbk−bkg)δt]T(abk1−bk1a)δt2 此时为了便于计算我们需要把四元数表示旋转转换为用旋转矩阵表示矩阵的旋转得到 f 15 1 4 δ R b i b k exp ( ( ( w b k − b k g ) δ t ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((w_b^k-b_k^g)\delta t)^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibkexp(((wbk−bkg)δt)∧)(abk1−bk1a)δt2 观察式子我们要想办法把 b k g b_k^g bkg拆出来。回顾上一章李代数旋转有性质 l n ( R e x p ( ϕ ∧ ) ) ∨ l n ( R ) ∨ J r − 1 ϕ ln(Rexp(\phi^{\land}))^{\vee}ln(R)^{\vee}J_r^{-1}\phi ln(Rexp(ϕ∧))∨ln(R)∨Jr−1ϕ 类似的对于非对数情况有 exp ( ( ϕ δ ϕ ) ∧ ) exp ( ϕ ∧ ) exp ( ( J r ( ϕ ) δ ϕ ) ∧ ) \exp( (\phi \delta\phi)^{\wedge} ) \exp(\phi^{\wedge})\exp((J_r(\phi)\delta\phi)^{\wedge}) exp((ϕδϕ)∧)exp(ϕ∧)exp((Jr(ϕ)δϕ)∧) lim ϕ → 0 J r ( ϕ ) I \lim_{\phi \rightarrow 0} J_r(\phi)I ϕ→0limJr(ϕ)I exp ( ( ( w b k − b k g ) δ t ) ∧ exp ( ( w b k δ t ) ∧ ) exp ( ( J r ( w b k δ t ) ( − b k g δ t ) ) ∧ ) \exp(((w_b^k-b_k^g)\delta t)^{\wedge}\exp((w_b^k\delta t)^{\wedge})\exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge}) exp(((wbk−bkg)δt)∧exp((wbkδt)∧)exp((Jr(wbkδt)(−bkgδt))∧) f 15 1 4 δ R b i b k exp ( ( ( w b k − b k g ) δ t ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((w_b^k-b_k^g)\delta t)^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibkexp(((wbk−bkg)δt)∧)(abk1−bk1a)δt2 f 15 1 4 δ R b i b k exp ( ( w b k δ t ) ∧ ) exp ( ( J r ( w b k δ t ) ( − b k g δ t ) ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((w_b^k\delta t)^{\wedge})\exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibkexp((wbkδt)∧)exp((Jr(wbkδt)(−bkgδt))∧)(abk1−bk1a)δt2 w b k δ t → 0 w_b^k\delta t \rightarrow0 wbkδt→0 f 15 1 4 δ R b i b k exp ( ( J r ( w b k δ t ) ( − b k g δ t ) ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((J_r(w_b^k\delta t)(-b_k^g \delta t))^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibkexp((Jr(wbkδt)(−bkgδt))∧)(abk1−bk1a)δt2 f 15 1 4 δ R b i b k exp ( ( − b k g δ t ) ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp((-b_k^g \delta t))^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibkexp((−bkgδt))∧)(abk1−bk1a)δt2 f 15 1 4 δ R b i b k ( I ( − b k g δ t ) ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} (I(-b_k^g \delta t))^{\wedge})(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibk(I(−bkgδt))∧)(abk1−bk1a)δt2 f 15 1 4 δ R b i b k ( − b k g δ t ) ∧ ( a b k 1 − b k 1 a ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} (-b_k^g \delta t)^{\wedge}(a_b^{k1} - b_{k1}^a)\delta t^2}{\delta b_k^g} f1541δbkgδRbibk(−bkgδt)∧(abk1−bk1a)δt2 使用伴随性质有 f 15 1 4 δ R b i b k ( a b k 1 − b k 1 a ) ∧ ( b k g δ t ) δ t 2 δ b k g f_{15}\frac{1}{4} \frac{\delta R_{b_i b_k} (a_b^{k1} - b_{k1}^a)^{\wedge}(b_k^g \delta t)\delta t^2}{\delta b_k^g} f1541δbkgδRbibk(abk1−bk1a)∧(bkgδt)δt2 f 15 1 4 R b i b k ( a b k 1 − b k 1 a ) ∧ δ t 2 δ t f_{15}\frac{1}{4} R_{b_i b_k} (a_b^{k1} - b_{k1}^a)^{\wedge} \delta t^2 \delta t f1541Rbibk(abk1−bk1a)∧δt2δt
示例2 g 12 δ α b i b k 1 δ n k g g_{12}\frac{\delta \alpha_{b_i b_{k1}}}{\delta n_k^g} g12δnkgδαbibk1 一看 n k g n_k^g nkg就知道又要找和旋转有关的量了。回顾递推公式有 ω 1 2 ( ( ω b k n k g − b k g ) ( w b k 1 n k 1 g − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^kn_k^g-b_k^g)(w_b^{k1}n_{k1}^g-b_{k1}^g)) ω21((ωbknkg−bkg)(wbk1nk1g−bk1g)) q b i b k 1 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T q_{b_i b_{k1}} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T qbibk1qbibk⊗[1,21ωδt]T a 1 2 ( q b i b k ( a b k n b k − b k a ) q b i b k 1 ( a b k 1 n b k 1 − b k 1 a ) ) a \frac{1}{2}(q_{b_i b_{k}} (a_b^k n_b^k -b_k^a) q_{b_i b_{k1}} (a_b^{k1} n_b^{k1} - b_{k1}^a)) a21(qbibk(abknbk−bka)qbibk1(abk1nbk1−bk1a)) α b i b k 1 α b i b k β b i b k δ t 1 2 a δ t 2 \alpha_{b_i b_{k1}} \alpha_{b_i b_{k}} \beta_{b_i b_k} \delta t \frac{1}{2}a \delta t^2 αbibk1αbibkβbibkδt21aδt2 有 g 12 δ α b i b k 1 δ n k g g_{12}\frac{\delta \alpha_{b_i b_{k1}}}{\delta n_k^g} g12δnkgδαbibk1 g 12 δ 1 2 a δ t 2 δ n k g g_{12}\frac{\delta \frac{1}{2}a \delta t^2}{\delta n_k^g} g12δnkgδ21aδt2 a 1 2 ( q b i b k ( a b k n b k − b k a ) q b i b k 1 ( a b k 1 n b k 1 − b k 1 a ) ) a \frac{1}{2}(q_{b_i b_{k}} (a_b^k n_b^k -b_k^a) q_{b_i b_{k1}} (a_b^{k1} n_b^{k1} - b_{k1}^a)) a21(qbibk(abknbk−bka)qbibk1(abk1nbk1−bk1a)) g 12 δ 1 4 q b i b k 1 ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{\delta \frac{1}{4}q_{b_i b_{k1}} (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g12δnkgδ41qbibk1(abk1−bk1a)δt2 又因为 q b i b k 1 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T q_{b_i b_{k1}} q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T qbibk1qbibk⊗[1,21ωδt]T 所以有 g 12 δ 1 4 q b i b k ⊗ [ 1 , 1 2 ω δ t ] T ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{\delta \frac{1}{4}q_{b_i b_k} \otimes [1, \frac{1}{2} \omega \delta t]^T (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g12δnkgδ41qbibk⊗[1,21ωδt]T(abk1−bk1a)δt2 ω 1 2 ( ( ω b k n k g − b k g ) ( w b k 1 n k 1 g − b k 1 g ) ) \omega \frac{1}{2}((\omega_b^kn_k^g-b_k^g)(w_b^{k1}n_{k1}^g-b_{k1}^g)) ω21((ωbknkg−bkg)(wbk1nk1g−bk1g)) g 12 δ 1 4 q b i b k ⊗ [ 1 , 1 2 ( ω b k 1 2 n k g ) δ t ] T ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{\delta \frac{1}{4}q_{b_i b_k} \otimes [1, \frac{1}{2} (\omega_b^k\frac{1}{2}n_k^g)\delta t]^T (a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g12δnkgδ41qbibk⊗[1,21(ωbk21nkg)δt]T(abk1−bk1a)δt2 g 12 1 4 δ R b i b k exp ( ( ( ω b k 1 2 n k g ) δ t ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{1}{4} \frac{\delta R_{b_i b_k} \exp(((\omega_b^k\frac{1}{2}n_k^g)\delta t)^{\wedge})(a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g1241δnkgδRbibkexp(((ωbk21nkg)δt)∧)(abk1−bk1a)δt2 g 12 1 4 δ R b i b k ( exp ( ( ω b k δ t ) ∧ ) ) ( exp ( ( J r ( ω b k δ t ) 1 2 n k g δ t ) ∧ ) ) ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{1}{4} \frac{\delta R_{b_i b_k} (\exp((\omega_b^k\delta t)^{\wedge}))(\exp((J_r(\omega_b^k\delta t)\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g1241δnkgδRbibk(exp((ωbkδt)∧))(exp((Jr(ωbkδt)21nkgδt)∧))(abk1−bk1a)δt2 g 12 1 4 δ R b i b k ( exp ( ( J r ( ω b k δ t ) 1 2 n k g δ t ) ∧ ) ) ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{1}{4} \frac{\delta R_{b_i b_k}(\exp((J_r(\omega_b^k\delta t)\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g1241δnkgδRbibk(exp((Jr(ωbkδt)21nkgδt)∧))(abk1−bk1a)δt2 g 12 1 4 δ R b i b k ( exp ( ( 1 2 n k g δ t ) ∧ ) ) ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{1}{4} \frac{\delta R_{b_i b_k}(\exp((\frac{1}{2}n_k^g \delta t)^{\wedge}))(a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g1241δnkgδRbibk(exp((21nkgδt)∧))(abk1−bk1a)δt2 g 12 1 4 δ R b i b k ( ( 1 2 n k g δ t ) ∧ ) ( a b k 1 − b k 1 a ) δ t 2 δ n k g g_{12}\frac{1}{4} \frac{\delta R_{b_i b_k}((\frac{1}{2}n_k^g \delta t)^{\wedge})(a_b^{k1} - b_{k1}^a) \delta t^2}{\delta n_k^g} g1241δnkgδRbibk((21nkgδt)∧)(abk1−bk1a)δt2 g 12 − 1 4 δ R b i b k ( a b k 1 − b k 1 a ) ∧ ( 1 2 n k g δ t ) δ t 2 δ n k g g_{12}-\frac{1}{4} \frac{\delta R_{b_i b_k}(a_b^{k1} - b_{k1}^a)^{\wedge} (\frac{1}{2}n_k^g \delta t)\delta t^2}{\delta n_k^g} g12−41δnkgδRbibk(abk1−bk1a)∧(21nkgδt)δt2 g 12 − 1 4 R b i b k ( a b k 1 − b k 1 a ) ∧ ( 1 2 δ t ) δ t 2 g_{12}-\frac{1}{4} R_{b_i b_k}(a_b^{k1} - b_{k1}^a)^{\wedge} (\frac{1}{2} \delta t)\delta t^2 g12−41Rbibk(abk1−bk1a)∧(21δt)δt2 g 12 − 1 8 R b i b k ( a b k 1 − b k 1 a ) ∧ ( δ t ) δ t 2 g_{12}-\frac{1}{8} R_{b_i b_k}(a_b^{k1} - b_{k1}^a)^{\wedge} (\delta t)\delta t^2 g12−81Rbibk(abk1−bk1a)∧(δt)δt2
Levenberg-Marquardt方法证明
Levenberg (1944) 和 Marquardt (1963) 先后对高斯牛顿法进行了改进求解过程中引入了阻尼因子 ( J T J μ I ) Δ x l m − J T f , μ 0 (J^TJ\mu I) \Delta x_{lm} -J^Tf,\mu 0 (JTJμI)Δxlm−JTf,μ0 J δ F δ x J \frac{\delta F}{\delta x} JδxδF
