OKVIS IMU 误差公式版本

公式和思路和预积分论文是一致的，和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)

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