公式和思路和预积分论文是一致的，和OKVIS IJRR论文里说的不一样！ ceres 优化时，迭代更新状态量，需要计算IMU的error。因为预积分时需要用到状态量IMU的偏置，而状态量在迭代中是变化的，所以当偏置变化小时，根据状态量对偏置的雅克比更新偏置变化后的积分值，当偏置变化大时，再重新积分。

和 ceres 接口 Evaluate 函数

输入参数： 上一帧 sensor 系到世界系的转换矩阵： TWS0 当前帧 sensor 系到世界系的转换矩阵： TWS1 上一帧 sensor 系到世界系转换矩阵旋转部分： CWS0=C{TWS0} CSW0=CTWS0 上一帧： v0bg0ba0b0=[bg0 ba0] 当前帧： v1bg1ba1b1=[bg1 ba1]

Δt=t1−t0 Δba=ba0−bgr Δbg=bg0−bgr Δb=[Δbg;Δba]

if redo_ || ||Δbg||Δt>0.0001

redoPreintegration(T_WS_0, speedAndBiases_0); redoCounter_++; Delta_b.setZero(); redo_ = false;

end if

actual propagation output

读入的配置参数，重力加速度： g redoPreintegration 中赋值： Δ′qdαdbg′ gW=g∗(0,0,1)T F0=I(15,15) Wδp=t{TWS0}−t{TWS1}+v0∗Δt−0.5∗gW∗Δt∗Δt Wδv=v0−v1−gW∗Δt Dq=δQ{−dαdbg′∗Δbg}∗Δ′q

redoPreintegration 中赋值: dpdbg′dαdbg′∫C∬C F0(0:2,0:2)=CSW0 F0(0:2,3:5)=CSW0∗[Wδp]× F0(0:2,6:8)=CSW0∗I(3,3)∗Δt F0(0:2,9:11)=dpdbg′ F0(0:2,12:14)=−∬C F0(3:5,3:5)=(qL{Dq∗q{TWS1}−1}∗qR{TWS0})(0:2,0:2 F0(3:5,9:11)=(qR{q{TWS1}−1∗q{TWS0}}∗qR{Dq})(0:2,0:2)∗−dαdbg′ F0(6:8,3:5)=CSW0∗Wδv F0(6:8,6:8)=CSW0 F0(6:8,9:11)=dvdbg F0(6:8,12:14)=−∫C

F0=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪CSW00000CSW0∗[Wδp]×(qL{Dq∗q{TWS1}−1}∗qR{TWS0})(0:2,0:2CSW0∗Wδv00CSW0∗I(3,3)∗Δt0CSW000dpdbg′(qR{q{TWS1}−1∗q{TWS0}}∗qR{Dq})(0:2,0:2)∗−dαdbg′dvdbg00−∬C0−∫C00⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

F1=−I(15,15) F1(0:2,0:2)=−CSW0 F1(3:5,3:5)=−(qL{Dq}∗qR{q{TWS0}}∗qL{q{TWS1}−1})(0:2,0:2) F1(6:8,6:8)=−CSW0

F1=⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪−CSW00000−(qL{Dq}∗qR{q{TWS0}}∗qL{q{TWS1}−1})(0:2,0:2)0000−CSW00000−I(6,6)⎫⎭⎬⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪

输出的 error

e(0:2)=CSW0∗Wδp+∬a+F0(0:2,9:14)∗Δb e(3:5)=2∗(Dq∗(q{TWS1}−1∗q{TWS0}))v e(6:8)=CSW0∗Wδv+∫a+F0(6:8,9:14)∗Δb e(9:14)=b0−b1

e(0,15)={CSW0∗Wδp+∬a+F0(0:2,9:14)∗Δb2∗(Dq∗(q{TWS1}−1∗q{TWS0}))vCSW0∗Wδv+∫a+F0(6:8,9:14)∗Δbb0−b1}

r=Σ−1−−−√∗e

输出雅各比

需要计算对参数： TWS0speedAndBiases0TWS0speedAndBiases1 雅各比： J0J1J2J3

redoPreintegration 中赋值： Σ−1−−−√ J0mini=Σ−1−−−√∗F0(0:14,0:5) Jlift=liftJacobian(TWS0) J0=J0mini∗Jlift

J1(15,9)=Σ−1−−−√∗F0(0:14,6:14)

J2mini=Σ−1−−−√∗F1(0:14,0:5) Jlift=liftJacobian(TWS1) J2=J2mini∗Jlift

J3mini=Σ−1−−−√∗F1(0:14,6:14)

版权声明：本文为博主原创文章，未经博主允许不得转载。