由于里面的零偏
b
a
,
b
w
b_{a},b_{w}
ba,bw 也是待优化变量,每次求解一次之后
b
a
,
b
w
b_{a},b_{w}
ba,bw 都会被改变以满足残差最小,那这样整个积分就得重新积分,这样会很耗时
由上节给出的更新公式
Δ
x
k
+
1
=
F
Δ
x
k
+
G
n
Δx_{k+1}=FΔx_{k}+Gn
Δxk+1=FΔxk+Gn ,
F
∈
15
×
15
F∈15×15
F∈15×15 ,求协方差矩阵的时候
=
F
⋅
P
⋅
F
T
=F·P·F^{T}
=F⋅P⋅FT,此时是3个15×15的矩阵相乘(一个IMU数据),如果每迭代一次就要把所有的IMU数据都这样重新相乘的话计算量会非常大。
问题:
Δ
x
Δx
Δx的方差P怎么来的
可以根据一阶泰勒展开来近似,来进行更新,避免重新预积分
f
(
x
+
Δ
b
)
=
f
(
x
)
+
J
⋅
Δ
b
f(x+Δb)=f(x)+J·Δb
f(x+Δb)=f(x)+J⋅Δb
预积分原本的公式如下
在离散时间下的 k 到 k+1的状态更新看上一节,实际也是用上一节的方式来进行更新,这里只是为了方便理解,因为雅可比
J
J
J 的计算还是用连续时间预积分的公式来计算的,现在这里是对零偏
b
b
b 进行求导,上一节是对时间
Δ
t
Δt
Δt 进行求导,只是在变成离散时间下会用中值积分进行近似而已
当零偏
b
a
,
b
w
b_{a},b_{w}
ba,bw 经过优化发生变化后的更新方式如下
下面
J
b
a
α
,
J
b
a
β
,
J
b
w
α
,
J
b
w
β
,
J
b
w
γ
J^{α}_{b_{a}},J^{β}_{b_{a}},J^{α}_{b_{w}},J^{β}_{b_{w}},J^{γ}_{b_{w}}
Jbaα,Jbaβ,Jbwα,Jbwβ,Jbwγ分别是
α
,
β
,
γ
α,β,γ
α,β,γ 对零偏
b
b
b 的求导
用一阶线性近似的方式来进行更新
现在关键在于如何求出上面列写的雅可比矩阵
J
J
J
Δ
b
=
b
k
−
b
k
+
1
Δb = b_{k}-b_{k+1}
Δb=bk−bk+1 这个比较好计算出来,用优化前后的两个零偏相减即可
定义一下:
Δ
α
=
J
b
a
α
⋅
Δ
b
a
Δα = J^{α}_{b_{a}}·Δb_{a}
Δα=Jbaα⋅Δba ,这个
α
α
α 对
b
a
b_{a}
ba 的雅可比乘上
b
a
b_{a}
ba 的变化量等于
α
α
α 的变化量
这里的雅可比
J
J
J 不进行一个个的求导,而是自己对自己求导
定义
x
=
[
α
β
γ
b
a
b
w
]
x=\begin{bmatrix} α\\ β\\ γ\\ b_{a}\\ b_{w}\\ \end{bmatrix}
x=
αβγbabw
,对自己求导
σ
x
σ
x
\frac{σx}{σx}
σxσx
=
J
=
[
σ
α
σ
α
σ
α
σ
β
σ
α
σ
γ
σ
α
σ
b
a
σ
α
σ
b
w
σ
β
σ
α
σ
β
σ
β
σ
β
σ
γ
σ
β
σ
b
a
σ
β
σ
b
w
σ
γ
σ
α
.
.
.
σ
γ
σ
γ
.
.
.
.
.
.
σ
b
a
σ
α
.
.
.
.
.
.
σ
b
a
σ
b
a
.
.
.
σ
b
w
σ
α
.
.
.
.
.
.
.
.
.
σ
b
w
σ
b
w
]
=J=\begin{bmatrix} \frac{σα}{σα}&\frac{σα}{σβ}&\frac{σα}{σγ}&\frac{σα}{σb_{a}}&\frac{σα}{σb_{w}}\\ \frac{σβ}{σα}& \frac{σβ}{σβ}& \frac{σβ}{σγ}& \frac{σβ}{σb_{a}}& \frac{σβ}{σb_{w}}\\ \frac{σγ}{σα}&...&\frac{σγ}{σγ}&...&...\\ \frac{σb_{a}}{σα}&...&...&\frac{σb_{a}}{σb_{a}}&...\\ \frac{σb_{w}}{σα}&...&...&...&\frac{σb_{w}}{σb_{w}}\\ \end{bmatrix}
=J=
σασασασβσασγσασbaσασbwσβσασβσβ.........σγσασγσβσγσγ......σbaσασbaσβ...σbaσba...σbwσασbwσβ......σbwσbw
在什么积分都没有做的情况下这个雅可比矩阵
J
J
J 为一个单位矩阵,对角线上自己对自己求导就是单位矩阵,在IMU数据还没有来之前,
x
x
x 中的变量两两间是没有联系的,则求导为0。
求这个雅可比就是为了获得
α
,
β
,
γ
α,β,γ
α,β,γ 对
b
a
,
b
w
b_{a},b_{w}
ba,bw 的导数。
接下来开始推导:
x
k
x_{k}
xk 是 k 时刻的状态值,也是预积分量,
σ
x
k
σ
x
\frac{σx_{k}}{σx}
σxσxk 可以求出第
k
k
k 时刻预积分量对自己的求导,但是现在需要将零偏从第
k
k
k 时刻转移到第
k
+
1
k+1
k+1时刻,根据上一节的更新公式
Δ
x
k
+
1
=
F
Δ
x
k
+
G
n
Δx_{k+1}=FΔx_{k}+Gn
Δxk+1=FΔxk+Gn ,这个公式描述的是状态量从
k
k
k 到
k
+
1
k+1
k+1 时刻的转移,这里面的
F
F
F 相当于是
σ
x
k
+
1
σ
x
k
\frac{σx_{k+1}}{σx_{k}}
σxkσxk+1 ,相乘得到
J
k
+
1
=
F
⋅
J
k
=
σ
x
k
+
1
σ
x
k
⋅
σ
x
k
σ
x
=
σ
x
k
+
1
σ
x
J_{k+1}=F·J_{k}=\frac{σx_{k+1}}{σx_{k}}·\frac{σx_{k}}{σx}=\frac{σx_{k+1}}{σx}
Jk+1=F⋅Jk=σxkσxk+1⋅σxσxk=σxσxk+1,这个矩阵里面就包含了各个状态量关于
b
a
,
b
w
b_{a},b_{w}
ba,bw 的导数。这个
J
k
+
1
J_{k+1}
Jk+1 是一个k+1时刻自己对自己的雅可比
为什么要算 k+1 时刻的 J:
根据一阶泰勒近似
f
(
x
+
Δ
b
)
=
f
(
x
)
+
J
b
x
⋅
Δ
b
f(x+Δb)=f(x)+J^{x}_{b}·Δb
f(x+Δb)=f(x)+Jbx⋅Δb ,这样当零偏经过优化发生变化后,只需要将变化量乘上导数加进去就能近似下一时刻的状态值,也是为了进行一阶近似才需要计算上面那个雅可比矩阵。
因为经过优化后,预积分量和零偏都会得到优化,但是预积分量还是用上一时刻的零偏进行计算的,获得新的零偏后,此时的预积分量应该根据新的零偏进行更新才对,但是进行重新积分很耗时,又由于零偏的变化量不大,所以用一阶近似,所以要计算雅可比矩阵。
协方差矩阵的传递
方差传递基本性质
a
a
a ~
A
A
A
K
a
Ka
Ka ~
K
2
A
K^{2}A
K2A
a
⃗
\vec{a}
a ~
A
A
A
F
a
⃗
F\vec{a}
Fa ~
F
⋅
A
⋅
F
T
F·A·F^{T}
F⋅A⋅FT,
F
F
F 是一个矩阵
a
⃗
\vec{a}
a~
A
A
A
b
⃗
\vec{b}
b ~
B
B
B
a
⃗
+
b
⃗
\vec{a}+\vec{b}
a+b ~
A
+
B
A+B
A+B
所以
Δ
x
k
+
1
:
Δx_{k+1}:
Δxk+1:
P
k
+
1
P_{k+1}
Pk+1
Δ
x
k
:
Δx_{k}:
Δxk:
P
k
P_{k}
Pk
n
k
:
n_{k}:
nk:
V
k
V_{k}
Vk
状态变换公式:
Δ
x
k
+
1
=
F
Δ
x
k
+
G
n
Δx_{k+1}=FΔx_{k}+Gn
Δxk+1=FΔxk+Gn
协方差更新公式:
P
k
+
1
=
F
⋅
P
k
⋅
F
T
+
G
⋅
V
k
⋅
G
T
P_{k+1}=F·P_{k}·F^{T}+G·V_{k}·G^{T}
Pk+1=F⋅Pk⋅FT+G⋅Vk⋅GT