预备知识
1.IMU测量模型(B系)Bω~,Ba~
2.世界系下运动模型(W系)RWBW,Wv,Wp
2.1 运动模型
2.2 IMU测量帧运动积分
2.3 离散形式(单位时间间隔保持加速度不变)
2.4 代入测量值
3.两视觉关键帧间运动积分(IMU测量帧叠加)(W系)
4.消除线性化时间点的影响,定义相对运动增量(B系)ΔRij,Δvij,Δpij
其中应该为21gΔtij2
5.分离测量值和噪声ΔRij~,Δvij~,Δpij~,δϕij,δvij,δpij
6.预积分测量模型
7.噪声信息矩阵求解 ηijΔ=[δϕijTδvijTδpijT]T,ηkd=[ηkgdηkad]
7.1 噪声分布形式
7.2 噪声增量式更新
7.3 信息矩阵更新
8. bias更新时测量值的更新 ΔRij^~,ΔRijˉ~,Δvij^~,Δvijˉ~,Δpij^~,Δpijˉ~,δbig,δbia
9. 残差及Jacobians计算
残差rΔpij,rΔvij,rΔRij(B系)
状态变量Ri,vi,pi,Rj,vj,pj,δbig,δbia (W系)
增量Bδϕbkbk+1,Bδpbkbk+1,Wδvbkbk+1,δbg~,δba~
9.1 残差计算
9.2 状态变量及对应增量
9.3 残差关于状态变量的Jacobians
(1) rΔpij
(2) rΔvij
(3) rΔRij