这篇博客主要介绍了如何推导IMU因子中角度误差的雅克比矩阵，并通过编写测试代码验证推导的正确性。作者遇到虽然误差能减小，但梯度检测未通过的问题。
OVKIS的阅读笔记
这次主要介绍的是IMU因子中构造的角度误差的雅克比矩阵的推导
由于实在看不懂OKVIS中IMU因子构造的角度误差的雅克比矩阵,索性自己推一遍,并且编写程序检查是否自己的推导是否正确,只是测试了误差可以下降,但是梯度检测没有通过,不知道为啥
IMU因子构造的角度误差
误差的定义如下所示
E ( θ ) = 2 ∗ ( Δ q ∗ q j − 1 ∗ q i ) . v e c ( ) E(\theta) = 2*(\Delta q*q_j^{-1} *q_i).vec() E(θ)=2∗(Δq∗qj−1∗qi).vec()
注意,为了和OKVIS对角度的更新一致,下面的推导对角度施加的是左扰动,VINS-Momo施加的是右扰动,(Notice OKIVS对误差施加的是右扰动,对变量施加的是左扰动)
E x p ( δ ϕ ) Δ R R j − 1 R i = Δ R R j − 1 E x p ( − δ ϕ j ) R i E x p ( δ ϕ ) = Δ R R j − 1 E x p ( − δ ϕ j ) ∗ ( Δ R R j − 1 ) − 1 E x p ( δ ϕ ) = E x p ( − Δ R R j − 1 δ ϕ j ) δ ϕ = − Δ R R j − 1 δ ϕ j Exp(\delta \phi)\Delta RR_j^{-1}R_i = \Delta RR_j^{-1}Exp(-\delta \phi_j)R_i \\ Exp(\delta \phi) = \Delta RR_j^{-1}Exp(-\delta \phi_j)* (\Delta RR_j^{-1})^{-1} \\ Exp(\delta \phi) = Exp(-\Delta RR_j^{-1}\delta \phi_j)\\ \delta \phi = -\Delta RR_j^{-1}\delta \phi_j Exp(δϕ)ΔRRj−1Ri=ΔRRj−1Exp(−δϕj)RiExp(δϕ)=ΔRRj−1Exp(−δϕj)∗(ΔRRj−1)−1Exp(δϕ)=Exp(−ΔRRj−1δϕj)δϕ=−ΔRRj−1
最低0.47元/天 解锁文章
扫一扫
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
