- ORB SLAM3系统初始化
- ORB SLAM3 构建Frame
- ORB_SLAM3 单目初始化
- ORB_SLAM3 双目匹配
- ORB_SLAM3_IMU预积分理论推导(预积分项)
- ORB_SLAM3_IMU预积分理论推导(噪声分析)
- ORB_SLAM3_IMU预积分理论推导(更新)
- ORB_SLAM3_IMU预积分理论推导(残差)
- ORB_SLAM3_优化方法 Pose优化
- ORB_SLAM3 闭环检测
误差
1.确定性误差
- Bias:偏置
- Scale:实际数值和传感器输出值之间的比值。
- Misalignment:非正交误差。
- 标定的方法:
六面法
标定加速度
。
2.随机误差
- Allan方差
- 随机游走
IMU器件测量模型
1.角标符号说明
b
:body坐标系a
:加速计(acc)g
:陀螺仪(gyro)w
:世界坐标系d
:离散(discrete)
2.假设
- 假设:地球是静止的,忽略自转,运行平面为水平面,重力指向固定,模值恒定。
- 运行场景小;
- 运行时间短;
- 精度低(mems);
3.测量模型(gyro/acc)
- 陀螺仪
ω ~ w b b ( t ) = ω w b b ( t ) + b g ( t ) + η g ( t ) \tilde{\omega } ^{b}_{wb}\left ( t \right ) = \omega ^{b}_{wb}\left ( t \right ) +b_g\left ( t\right ) +\eta _g\left ( t \right ) ω~wbb(t)=ωwbb(t)+bg(t)+ηg(t) - 加速计
a b ( t ) = R b w T ( a w − g w ) + b a ( t ) + η a ( t ) a^b\left ( t \right ) = {\color{Red} R_{b}^{wT}} \left ( a^{w}-g^{w} \right ) +b_a\left ( t\right ) +\eta _a \left( t \right ) ab(t)=RbwT(aw−gw)+ba(t)+ηa(t)
- ω ~ , a b ( t ) \widetilde{\omega}, a^b\left ( t \right ) ω ,ab(t):测量值
- ω , a w \omega,a^{w} ω,aw:真实值
偏置项
和噪声项
都位于t时刻的载体坐标系(b)
-
R
b
w
R_{b}^{w}
Rbw:从
载体坐标系(b)
到世界坐标系(w)
的旋转 -
g
w
g^{w}
gw:
世界坐标系下
的重力加速度
预积分
估计的状态:[ R b w ( t ) , p w ( t ) , v w ( t ) R_b^w(t), p^{w}(t), v^{w}(t) Rbw(t),pw(t),vw(t)]
- p w ( t ) p^{w}(t) pw(t)和 v w ( t ) v^{w}(t) vw(t)为世界坐标系下IMU的位置和速度
- [
R
b
w
(
t
)
,
p
w
(
t
)
R_b^w(t), p^{w}(t)
Rbw(t),pw(t)]将帧从
B
映射到W
1.运动方程
使用欧拉积分,可以得到运动方程的离散形式为:
R
b
(
t
+
Δ
t
)
w
=
R
b
(
t
)
w
Exp
(
ω
w
b
b
(
t
)
⋅
Δ
t
)
v
w
(
t
+
Δ
t
)
=
v
w
(
t
)
+
a
w
(
t
)
⋅
Δ
t
p
w
(
t
+
Δ
t
)
=
p
w
(
t
)
+
v
w
(
t
)
⋅
Δ
t
+
1
2
a
w
(
t
)
⋅
Δ
t
2
\begin{array}{l} \mathbf{R}_{b(t+\Delta t)}^{w}=\mathbf{R}_{b(t)}^{w} {\color{Red}\operatorname{Exp} } \left({\color{Red} \boldsymbol{\omega}_{w b}^{b}(t)} \cdot \Delta t\right) \\ \mathbf{v}^{w}(t+\Delta t)=\mathbf{v}^{w}(t)+{\color{Red} \mathbf{a}^{w}(t)} \cdot \Delta t \\ \mathbf{p}^{w}(t+\Delta t)=\mathbf{p}^{w}(t)+\mathbf{v}^{w}(t) \cdot \Delta t+{\color{Red} \frac{1}{2} \mathbf{a}^{w}(t)} \cdot \Delta t^{2} \end{array}
Rb(t+Δt)w=Rb(t)wExp(ωwbb(t)⋅Δt)vw(t+Δt)=vw(t)+aw(t)⋅Δtpw(t+Δt)=pw(t)+vw(t)⋅Δt+21aw(t)⋅Δt2
其中,
w
w
b
b
(
t
)
w_{wb}^b(t)
wwbb(t)表示t时刻角速度矢量
在b系下的坐标,
w
w
b
b
(
t
)
⋅
Δ
t
w_{wb}^b(t)\cdot \Delta t
wwbb(t)⋅Δt表示旋转矢量
在b系下的坐标,
Exp
(
ω
w
b
b
(
t
)
⋅
Δ
t
)
{\color{Red}\operatorname{Exp} } \left({\color{Red} \boldsymbol{\omega}_{w b}^{b}(t)} \cdot \Delta t\right)
Exp(ωwbb(t)⋅Δt)表示在b系下从
t
+
Δ
t
t+\Delta t
t+Δt时刻到
t
t
t时刻的旋转变换(
R
b
(
t
+
Δ
t
)
b
(
t
)
R_{b \left (t + \Delta t \right)}^{b \left ( t \right)}
Rb(t+Δt)b(t))。
在采样频率不变,也就是
Δ
t
\Delta t
Δt不变,将测量模型代入离散运动方程为:
R
k
+
1
=
R
k
⋅
Exp
(
(
ω
~
k
−
b
k
g
−
η
k
g
d
)
⋅
Δ
t
)
v
k
+
1
=
v
k
+
R
k
⋅
(
f
~
k
−
b
k
a
−
η
k
a
d
)
⋅
Δ
t
+
g
⋅
Δ
t
p
k
+
1
=
p
k
+
v
k
⋅
Δ
t
+
1
2
g
⋅
Δ
t
2
+
1
2
R
k
⋅
(
f
~
k
−
b
k
a
−
η
k
a
d
)
⋅
Δ
t
2
\begin{array}{l} \mathbf{R}_{k+1}=\mathbf{R}_{k} \cdot \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{k}^{g}-\boldsymbol{\eta}_{k}^{g d}\right) \cdot \Delta t\right) \\ \mathbf{v}_{k+1}=\mathbf{v}_{k}+\mathbf{R}_{k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{k}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t+{\color{Red}\mathbf{g} \cdot \Delta t} \\ \mathbf{p}_{k+1}=\mathbf{p}_{k}+\mathbf{v}_{k} \cdot \Delta t+\frac{1}{2} \mathbf{g} \cdot \Delta t^{2}+\frac{1}{2} \mathbf{R}_{k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t^{2} \end{array}
Rk+1=Rk⋅Exp((ω~k−bkg−ηkgd)⋅Δt)vk+1=vk+Rk⋅(f~k−bka−ηkad)⋅Δt+g⋅Δtpk+1=pk+vk⋅Δt+21g⋅Δt2+21Rk⋅(f~k−bka−ηkad)⋅Δt2
其中,
R
(
t
)
≐
R
b
(
t
)
w
;
ω
(
t
)
≐
ω
w
b
b
(
t
)
;
f
(
t
)
=
f
b
(
t
)
;
v
(
t
)
≐
v
w
(
t
)
;
p
(
t
)
≐
p
w
(
t
)
;
g
≐
g
w
\mathbf{R}(t) \doteq \mathbf{R}_{b(t)}^{w} ; \boldsymbol{\omega}(t) \doteq \boldsymbol{\omega}_{w b}^{b}(t) ; \mathbf{f}(t)=\mathbf{f}^{b}(t) ; \mathbf{v}(t) \doteq \mathbf{v}^{w}(t) ; \mathbf{p}(t) \doteq \mathbf{p}^{w}(t) ; \mathbf{g} \doteq \mathbf{g}^{w}
R(t)≐Rb(t)w;ω(t)≐ωwbb(t);f(t)=fb(t);v(t)≐vw(t);p(t)≐pw(t);g≐gw
2.预积分
根据欧拉积分
,可以利用
i
i
i时刻到
j
−
1
j-1
j−1时刻的所有IMU测量,其中
j
j
j时刻的
R
j
,
v
j
,
p
j
R_j, v_j, p_j
Rj,vj,pj(世界坐标系
)可以直接由
i
i
i时刻的
R
i
,
v
i
,
p
i
R_i, v_i,p_i
Ri,vi,pi(世界坐标系
)更新得到。
-
R j R_j Rj
R j = R i ∏ k = i j − 1 E x p ( ( ω ~ k − b k g − η k g d ) ⋅ △ t ) R_{j} =R_{i} \prod_{k=i}^{j-1} Exp\left ( \left ( \widetilde{\omega}_{k} -b_k^g-\eta _{k}^{gd} \right ) \cdot \bigtriangleup t \right ) Rj=Rik=i∏j−1Exp((ω k−bkg−ηkgd)⋅△t) -
v j v_j vj
v j = v i + g ⋅ Δ t i j + ∑ k = i j − 1 R k ⋅ ( f k ~ − b k a − η k a d ) ⋅ Δ t v_{j} =v_{i} +{\color{Purple} g\cdot \Delta t_{ij}} +\sum_{k=i}^{j-1} {\color{Red} R_k} \cdot \left ( \widetilde{f_k}-b_k^a-\eta _{k}^{ad} \right )\cdot \Delta t vj=vi+g⋅Δtij+k=i∑j−1Rk⋅(fk −bka−ηkad)⋅Δt
其中, ( f k ~ − b k a − η k a d ) ⋅ Δ t \left ( \widetilde{f_k}-b_k^a-\eta _{k}^{ad} \right )\cdot \Delta t (fk −bka−ηkad)⋅Δt相对于的是IMU坐标系,需要转换到世界坐标系
。 -
p j p_j pj
p j = p i + ∑ k = i j − 1 v k ⋅ Δ t + j − i 2 g ⋅ Δ t 2 + 1 2 ∑ k = i j − 1 R k ⋅ ( f k ~ − b k a − η k a d ) ⋅ Δ t 2 = p i + ∑ k = i j − 1 [ v k ⋅ Δ t + 1 2 g ⋅ Δ t 2 + 1 2 R k ⋅ ( f k ~ − b k a − η k a d ) ⋅ Δ t 2 ] \begin{align} p_j & = p_i+\sum_{k = i}^{j-1} v_k\cdot \Delta t+{\color{Purple} \frac{j-i}{2} g\cdot \Delta t^2} +\frac{1}{2} \sum_{k = i}^{j-1}{\color{Red} R_k} \cdot \left ( \widetilde{f_k}-b_k^a-\eta _k^{ad} \right )\cdot \Delta t^2 \\ & = p_i+\sum_{k=i}^{j-1}\left [ v_k\cdot \Delta t+\frac{1}{2} g\cdot \Delta t^2+\frac{1}{2} {\color{Red} R_k} \cdot \left ( \widetilde{f_k}-b_k^a-\eta _k^{ad} \right )\cdot \Delta t^2 \right ] \end{align} pj=pi+k=i∑j−1vk⋅Δt+2j−ig⋅Δt2+21k=i∑j−1Rk⋅(fk −bka−ηkad)⋅Δt2=pi+k=i∑j−1[vk⋅Δt+21g⋅Δt2+21Rk⋅(fk −bka−ηkad)⋅Δt2]
其中, Δ t i j = ∑ k = i j − 1 Δ t = ( j − i ) Δ t \Delta t_{ij}=\sum_{k=i}^{j-1} \Delta t=\left ( j-i \right ) \Delta t Δtij=∑k=ij−1Δt=(j−i)Δt
为了避免每次更新初始的
R
i
,
v
i
,
p
i
R_i, v_i, p_i
Ri,vi,pi都需要重新计算
R
j
,
v
j
,
p
j
R_j, v_j, p_j
Rj,vj,pj,引出预积分项:
Δ
R
i
j
≜
R
i
T
R
j
=
∏
k
=
i
j
−
1
Exp
(
(
ω
~
k
−
b
k
g
−
η
k
g
d
)
⋅
Δ
t
)
Δ
v
i
j
≜
R
i
T
(
v
j
−
v
i
−
g
⋅
Δ
t
i
j
)
=
∑
k
=
i
j
−
1
Δ
R
i
k
⋅
(
f
~
k
−
b
k
a
−
η
k
a
d
)
⋅
Δ
t
Δ
p
i
j
≜
R
i
T
(
p
j
−
p
i
−
v
i
⋅
Δ
t
i
j
−
1
2
g
⋅
Δ
t
i
j
2
)
=
∑
k
=
i
j
−
1
[
Δ
v
i
k
⋅
Δ
t
+
1
2
Δ
R
i
k
⋅
(
f
~
k
−
b
k
a
−
η
k
a
d
)
⋅
Δ
t
2
]
\begin{aligned} \Delta \mathbf{R}_{i j} & \triangleq \mathbf{R}_{i}^{T} \mathbf{R}_{j} \\ &=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\mathbf{\omega}}_{k}-\mathbf{b}_{k}^{g}-\eta_{k}^{g d}\right) \cdot \Delta t\right) \\ \Delta \mathbf{v}_{i j} & \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right) \\ &=\sum_{k=i}^{j-1} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{k}^{a}-\eta_{k}^{a d}\right) \cdot \Delta t \\ \Delta \mathbf{p}_{i j} & \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right) \\ &=\sum_{k=i}^{j-1}\left[\Delta \mathbf{v}_{i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{k}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \end{aligned}
ΔRijΔvijΔpij≜RiTRj=k=i∏j−1Exp((ω~k−bkg−ηkgd)⋅Δt)≜RiT(vj−vi−g⋅Δtij)=k=i∑j−1ΔRik⋅(f~k−bka−ηkad)⋅Δt≜RiT(pj−pi−vi⋅Δtij−21g⋅Δtij2)=k=i∑j−1[Δvik⋅Δt+21ΔRik⋅(f~k−bka−ηkad)⋅Δt2]
预积分测量值和测量噪声
假设预积分计算区间内的bias
相等,即
b
i
g
=
b
i
+
1
g
=
.
.
.
=
b
j
g
b_i^g = b_{i+1}^g =...=b_j^{g}
big=bi+1g=...=bjg以及
b
i
a
=
b
i
+
1
a
=
.
.
.
=
b
j
a
b_i^a = b_{i+1}^a =...=b_j^a
bia=bi+1a=...=bja
-
Δ R i j \Delta R_{ij} ΔRij项
根据性质“当 δ ϕ → \delta \overrightarrow{\phi } δϕ ”是小量
时,有
Exp ( ϕ ⃗ + δ ϕ ⃗ ) ≈ Exp ( ϕ ⃗ ) ⋅ Exp ( J r ( ϕ ⃗ ) ⋅ δ ϕ ⃗ ) \operatorname{Exp}(\vec{\phi}+\delta \vec{\phi}) \approx \operatorname{Exp}(\vec{\phi}) \cdot \operatorname{Exp}\left(\mathbf{J}_{r}(\vec{\phi}) \cdot \delta \vec{\phi}\right) Exp(ϕ+δϕ)≈Exp(ϕ)⋅Exp(Jr(ϕ)⋅δϕ)
指数映射的Adjoint性质:
R ⋅ Exp ( ϕ ⃗ ) ⋅ R T = exp ( R ϕ ⃗ ∧ R T ) = Exp ( R ϕ ⃗ ) ⇔ Exp ( ϕ ⃗ ) ⋅ R = R ⋅ Exp ( R T ϕ ⃗ ) \begin{array}{l} \mathbf{R} \cdot \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R}^{T} = \exp \left(\mathbf{R} \vec{\phi}^{\wedge} \mathbf{R}^{T}\right) = \operatorname{Exp}(\mathbf{R} \vec{\phi}) \\ \Leftrightarrow \operatorname{Exp}(\vec{\phi}) \cdot \mathbf{R} = \mathbf{R} \cdot \operatorname{Exp}\left(\mathbf{R}^{T} \vec{\phi}\right) \end{array} R⋅Exp(ϕ)⋅RT=exp(Rϕ∧RT)=Exp(Rϕ)⇔Exp(ϕ)⋅R=R⋅Exp(RTϕ)
Δ R i j = ∏ k = i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ⏟ ϕ → − η k g d Δ t ⏟ δ ϕ → ) ≈ ( 1 ) ∏ k = i j − 1 { Exp ( ( ω ~ k − b i g ) Δ t ) ⋅ Exp ( − J r ( ( ω ~ k − b i g ) Δ t ) ⋅ η k g d Δ t ) } = ( 2 ) Δ R ~ i j ⋅ ∏ k = i j − 1 Exp ( − Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ t ) \begin{aligned} \Delta \mathbf{R}_{i j} &=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\underbrace{\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t}_{\overrightarrow{\phi } }\underbrace{-\boldsymbol{\eta}_{k}^{g d} \Delta t}_{\delta \overrightarrow{\phi } } \right) \\ & \stackrel{(1)}{\approx} \prod_{k=i}^{j-1}\left\{\operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \operatorname{Exp}\left(-\mathbf{J}_{r}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \cdot \boldsymbol{\eta}_{k}^{g d} \Delta t\right)\right\} \\ & \stackrel{(2)}{=} \Delta \tilde{\mathbf{R}}_{i j} \cdot \prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \boldsymbol{\eta}_{k}^{g d} \Delta t\right) \end{aligned} ΔRij=k=i∏j−1Exp ϕ (ω~k−big)Δtδϕ −ηkgdΔt ≈(1)k=i∏j−1{Exp((ω~k−big)Δt)⋅Exp(−Jr((ω~k−big)Δt)⋅ηkgdΔt)}=(2)ΔR~ij⋅k=i∏j−1Exp(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt)
其中,令:
J r k = J r ( ( ω ~ k − b i g ) Δ t ) Δ R ~ i j = ∏ k = i j − 1 Exp ( ( ω ~ k − b i g ) Δ t ) Exp ( − δ ϕ ⃗ i j ) = ∏ k = i j − 1 Exp ( − Δ R ~ k + 1 j T ⋅ J r k ⋅ η k g d Δ t ) \begin{array}{l} \mathbf{J}_{r}^{k}=\mathbf{J}_{r}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right)\\ \Delta \tilde{\mathbf{R}}_{i j}=\prod_{k=i}^{j-1} \operatorname{Exp}\left(\left(\tilde{\boldsymbol{\omega}}_{k}-\mathbf{b}_{i}^{g}\right) \Delta t\right) \\ \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right)=\prod_{k=i}^{j-1} \operatorname{Exp}\left(-\Delta \tilde{\mathbf{R}}_{k+1 j}^{T} \cdot \mathbf{J}_{r}^{k} \cdot \boldsymbol{\eta}_{k}^{g d} \Delta t\right) \end{array} Jrk=Jr((ω~k−big)Δt)ΔR~ij=∏k=ij−1Exp((ω~k−big)Δt)Exp(−δϕij)=∏k=ij−1Exp(−ΔR~k+1jT⋅Jrk⋅ηkgdΔt)
其中, Δ R ~ j j = I \Delta \widetilde{R}_{jj}=I ΔR jj=I,则可以得到:
Δ R i j ≜ Δ R ~ i j ⋅ Exp ( − δ ϕ ⃗ i j ) \Delta \mathbf{R}_{i j} \triangleq \Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right) ΔRij≜ΔR~ij⋅Exp(−δϕij)
Δ R ~ i j \Delta\widetilde{R}_{ij} ΔR ij即旋转量预积分测量值
,它由陀螺仪测量值
和对陀螺仪bias的估计
得到, δ ϕ → i j \delta\overrightarrow{\phi }_{ij} δϕij为测量的噪声
。 -
Δ v i j \Delta v_{ij} Δvij项:
将 Δ R i j ≜ Δ R ~ i j ⋅ Exp ( − δ ϕ ⃗ i j ) \Delta \mathbf{R}_{i j} \triangleq \Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right) ΔRij≜ΔR~ij⋅Exp(−δϕij)代入到 Δ v i j \Delta v_{ij} Δvij中
Δ v i j = ∑ k = i j − 1 Δ R i k ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ∑ k = i j − 1 Δ R ~ i k ⋅ Exp ( − δ ϕ ⃗ i k ) ⏟ I − δ ϕ ⃗ i k ∧ ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ( 1 ) ∑ k = i j − 1 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t ≈ ( 2 ) ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a ) ⋅ Δ t ⏟ a ∧ ⋅ b = − b ∧ ⋅ a − Δ R ~ i k η k a d Δ t + Δ R ~ i k δ ϕ → i j ∧ η k a d ‾ ] = ( 3 ) ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t + Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a d Δ t ] = ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ⋅ Δ t ] ⏟ Δ v ~ i j + ∑ k = i j − 1 [ Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ ⋅ δ ϕ ⃗ i k ⋅ Δ t − Δ R ~ i k η k a d Δ t ] ⏟ − δ v i j \begin{aligned} \Delta \mathbf{v}_{i j} &=\sum_{k=i}^{j-1} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \approx \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot \underbrace{{\color{Red} \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right)}}_{\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \stackrel{(1)}{\approx} \sum_{k=i}^{j-1} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t \\ & \stackrel{(2)}{\approx} \sum_{k=i}^{j-1}\left[\underbrace{\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t}_{{\color{Red} a^{\wedge } \cdot b = -b^{\wedge }\cdot a} } -\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t +\underline{\Delta\widetilde{R}_{ik}\delta \overrightarrow{\phi} _{ij}^{\wedge }\eta _{k}^{ad}} \right] \\ & \stackrel{(3)}{=} \sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t+\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right] \\ &=\underbrace{\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t\right]}_{\Delta \widetilde{v}_{ij} } +\underbrace{\sum_{k=i}^{j-1}\left[\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \cdot \delta \vec{\phi}_{i k} \cdot \Delta t-\Delta \tilde{\mathbf{R}}_{i k} \boldsymbol{\eta}_{k}^{a d} \Delta t\right]}_{-\delta v_{ij}} \end{aligned} Δvij=k=i∑j−1ΔRik⋅(f~k−bia−ηkad)⋅Δt≈k=i∑j−1ΔR~ik⋅I−δϕik∧ Exp(−δϕik)⋅(f~k−bia−ηkad)⋅Δt≈(1)k=i∑j−1ΔR~ik⋅(I−δϕik∧)⋅(f~k−bia−ηkad)⋅Δt≈(2)k=i∑j−1 a∧⋅b=−b∧⋅a ΔR~ik⋅(I−δϕik∧)⋅(f~k−bia)⋅Δt−ΔR~ikηkadΔt+ΔR ikδϕij∧ηkad =(3)k=i∑j−1[ΔR~ik⋅(f~k−bia)⋅Δt+ΔR~ik⋅(f~k−bia)∧⋅δϕik⋅Δt−ΔR~ikηkadΔt]=Δv ij k=i∑j−1[ΔR~ik⋅(f~k−bia)⋅Δt]+−δvij k=i∑j−1[ΔR~ik⋅(f~k−bia)∧⋅δϕik⋅Δt−ΔR~ikηkadΔt]
可以得到:
Δ v i j ≜ Δ v ~ i j − δ v i j \Delta v_{ij} \triangleq \Delta \widetilde{v}_{ij}-\delta v_{ij} Δvij≜Δv ij−δvij
Δ v ~ i j \Delta \widetilde{v}_{ij} Δv ij为速度增量预积分测量值,它由IMU的测量值和对bias的估计得到, δ v i j \delta v_{ij} δvij为测量噪声。 -
Δ p i j \Delta p_{ij} Δpij项
将 Δ R i j ≜ Δ R ~ i j ⋅ Exp ( − δ ϕ ⃗ i j ) \Delta \mathbf{R}_{i j} \triangleq \Delta \tilde{\mathbf{R}}_{i j} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i j}\right) ΔRij≜ΔR~ij⋅Exp(−δϕij)和 Δ v i j ≜ Δ v ~ i j − δ v i j \Delta v_{ij} \triangleq \Delta \widetilde{v}_{ij}-\delta v_{ij} Δvij≜Δv ij−δvij代入到 Δ p i j \Delta p_{ij} Δpij中
Δ p i j = ∑ k = i j − 1 [ Δ v i k ⋅ Δ t + 1 2 Δ R i k ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ∑ k = i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ Exp ( − δ ϕ ⃗ i k ) ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ( 1 ) ∑ k = i j − 1 [ ( Δ v ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⏟ Exp ( ϕ ⃗ ) = I + ϕ ⃗ ∧ ⋅ ( f ~ k − b i a − η k a d ) ⋅ Δ t 2 ] ≈ ( 2 ) ∑ k = i j − 1 [ ( Δ V ~ i k − δ v i k ) ⋅ Δ t + 1 2 Δ R ~ i k ⋅ ( I − δ ϕ ⃗ i k ∧ ) ⋅ ( f ~ k − b i a ) ⋅ Δ t 2 − 1 2 Δ R ~ i k η k a d Δ t 2 + 1 2 Δ R ~ i k δ ϕ ⃗ i k ∧ η k a d Δ t 2 ‾ ] = ( 3 ) ∑ k = i j − 1 [ Δ V ~ i k Δ t + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) Δ t 2 ⏟ Δ p ~ i j + 1 2 Δ R ~ i k ⋅ ( f ~ k − b i a ) ∧ δ ϕ ⃗ i k Δ t 2 ⏟ a ∧ ⋅ b = − b ∧ ⋅ a − 1 2 Δ R ~ i k η k a d Δ t 2 − δ v i k Δ t ⏟ δ p i j ] \begin{array}{l} \Delta \mathbf{p}_{i j}=\sum_{k=i}^{j-1}\left[\Delta \mathbf{v}_{i k} \cdot \Delta t+\frac{1}{2} \Delta \mathbf{R}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \approx \sum_{k=i}^{j-1}\left[{\color{Red} \left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right)} \cdot \Delta t+\frac{1}{2} {\color{Red} \Delta \tilde{\mathbf{R}}_{i k} \cdot \operatorname{Exp}\left(-\delta \vec{\phi}_{i k}\right)} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\boldsymbol{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \stackrel{(1)}{\approx} \sum_{k=i}^{j-1}\left[\left(\Delta \tilde{\mathbf{v}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t+\frac{1}{2} \underbrace{\Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right)}_{\operatorname{Exp}\left (\vec{ \phi} \right)=I+\vec{\phi}^{\wedge}} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}-\mathbf{\eta}_{k}^{a d}\right) \cdot \Delta t^{2}\right] \\ \stackrel{(2)}{\approx} \sum_{k=i}^{j-1}\left[\left(\Delta \tilde{\mathbf{V}}_{i k}-\delta \mathbf{v}_{i k}\right) \cdot \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\mathbf{I}-\delta \vec{\phi}_{i k}^{\wedge}\right) \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \cdot \Delta t^{2}-\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a d} \Delta t^{2} + \underline{\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \delta \vec{\phi}_{i k}^{\wedge} \mathbf{\eta}_{k}^{a d} \Delta t^{2}} \right] \\ \stackrel{(3)}{=} \sum_{k=i}^{j-1}\left[\underbrace{\Delta \tilde{\mathbf{V}}_{i k} \Delta t+\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right) \Delta t^{2}}_{\Delta \widetilde{p} _{ij}} +\underbrace{\underbrace{\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \cdot\left(\tilde{\mathbf{f}}_{k}-\mathbf{b}_{i}^{a}\right)^{\wedge} \delta \vec{\phi}_{i k} \Delta t^{2}}_{a^{\wedge }\cdot b=-b^{\wedge }\cdot a} -\frac{1}{2} \Delta \tilde{\mathbf{R}}_{i k} \mathbf{\eta}_{k}^{a d} \Delta t^{2}-\delta \mathbf{v}_{i k} \Delta t} _{\delta p_{ij}}\right] \\ \end{array} Δpij=∑k=ij−1[Δvik⋅Δt+21ΔRik⋅(f~k−bia−ηkad)⋅Δt2]≈∑k=ij−1[(Δv~ik−δvik)⋅Δt+21ΔR~ik⋅Exp(−δϕik)⋅(f~k−bia−ηkad)⋅Δt2]≈(1)∑k=ij−1 (Δv~ik−δvik)⋅Δt+21Exp(ϕ)=I+ϕ∧ ΔR~ik⋅(I−δϕik∧)⋅(f~k−bia−ηkad)⋅Δt2 ≈(2)∑k=ij−1[(ΔV~ik−δvik)⋅Δt+21ΔR~ik⋅(I−δϕik∧)⋅(f~k−bia)⋅Δt2−21ΔR~ikηkadΔt2+21ΔR~ikδϕik∧ηkadΔt2]=(3)∑k=ij−1 Δp ij ΔV~ikΔt+21ΔR~ik⋅(f~k−bia)Δt2+δpij a∧⋅b=−b∧⋅a 21ΔR~ik⋅(f~k−bia)∧δϕikΔt2−21ΔR~ikηkadΔt2−δvikΔt
可以得到:
Δ
p
i
j
≜
Δ
p
~
i
j
−
δ
p
i
j
\Delta p_{ij} \triangleq \Delta \widetilde{p}_{ij}-\delta p_{ij}
Δpij≜Δp
ij−δpij
Δ
p
~
i
j
\Delta \widetilde{p}_{ij}
Δp
ij为速度增量预积分测量值,它由IMU的测量值和对bias的估计得到,
δ
p
i
j
\delta p_{ij}
δpij为测量噪声。
预积分理想值:
Δ
R
i
j
≜
R
i
T
R
j
Δ
v
i
j
≜
R
i
T
(
v
j
−
v
i
−
g
⋅
Δ
t
i
j
)
Δ
p
i
j
≜
R
i
T
(
p
j
−
p
i
−
v
i
⋅
Δ
t
i
j
−
1
2
g
⋅
Δ
t
i
j
2
)
\begin{array}{l} \Delta \mathbf{R}_{i j} \triangleq \mathbf{R}_{i}^{T} \mathbf{R}_{j}\\ \Delta \mathbf{v}_{i j} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)\\ \Delta \mathbf{p}_{i j} \triangleq \mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right) \end{array}
ΔRij≜RiTRjΔvij≜RiT(vj−vi−g⋅Δtij)Δpij≜RiT(pj−pi−vi⋅Δtij−21g⋅Δtij2)
将预积分理想值代入:测量值 = 理想值 + 噪声
Δ
R
~
i
j
≈
Δ
R
i
j
Exp
(
δ
ϕ
⃗
i
j
)
=
R
i
T
R
j
Exp
(
δ
ϕ
⃗
i
j
)
Δ
v
~
i
j
≈
Δ
v
i
j
+
δ
v
i
j
=
R
i
T
(
v
j
−
v
i
−
g
⋅
Δ
t
i
j
)
+
δ
v
i
j
Δ
p
~
i
j
≈
Δ
p
i
j
+
δ
p
i
j
=
R
i
T
(
p
j
−
p
i
−
v
i
⋅
Δ
t
i
j
−
1
2
g
⋅
Δ
t
i
j
2
)
+
δ
p
i
j
\begin{array}{l} \Delta \tilde{\mathbf{R}}_{i j} \approx \Delta \mathbf{R}_{i j} \operatorname{Exp}\left(\delta \vec{\phi}_{i j}\right)=\mathbf{R}_{i}^{T} \mathbf{R}_{j} \operatorname{Exp}\left(\delta \vec{\phi}_{i j}\right) \\ \Delta \tilde{\mathbf{v}}_{i j} \approx \Delta \mathbf{v}_{i j}+\delta \mathbf{v}_{i j}=\mathbf{R}_{i}^{T}\left(\mathbf{v}_{j}-\mathbf{v}_{i}-\mathbf{g} \cdot \Delta t_{i j}\right)+\delta \mathbf{v}_{i j} \\ \Delta \tilde{\mathbf{p}}_{i j} \approx \Delta \mathbf{p}_{i j}+\delta \mathbf{p}_{i j}=\mathbf{R}_{i}^{T}\left(\mathbf{p}_{j}-\mathbf{p}_{i}-\mathbf{v}_{i} \cdot \Delta t_{i j}-\frac{1}{2} \mathbf{g} \cdot \Delta t_{i j}^{2}\right)+\delta \mathbf{p}_{i j} \end{array}
ΔR~ij≈ΔRijExp(δϕij)=RiTRjExp(δϕij)Δv~ij≈Δvij+δvij=RiT(vj−vi−g⋅Δtij)+δvijΔp~ij≈Δpij+δpij=RiT(pj−pi−vi⋅Δtij−21g⋅Δtij2)+δpij