SLAM--位姿估计 (扩展卡尔曼滤波EKF/高斯牛顿非线性优化)
一、扩展卡尔曼滤波EKF求解
1.矩阵伴随基础
李代数的伴随矩阵：
a
d
(
ξ
∧
)
=
ξ
⋏
=
[
ρ
ϕ
]
⋏
=
[
ϕ
∧
ρ
∧
0
ϕ
∧
]
ad(\xi^\land )=\xi ^⋏=\begin{bmatrix} \rho \\ \phi \end{bmatrix} ^ ⋏= \begin{bmatrix} \phi^ \land \quad \rho^\land \\ 0 \quad \phi ^\land \end{bmatrix}
ad(ξ∧)=ξ⋏=[ρϕ]⋏=[ϕ∧ρ∧0ϕ∧]
李群的伴随：
A
d
(
exp
(
ξ
∧
)
)
=
exp
(
a
d
(
ξ
∧
)
)
=
A
d
(
[
C
r
0
T
1
]
)
=
[
C
r
∧
C
0
ϕ
∧
]
Ad(\exp(\xi ^ \land))=\exp (ad(\xi^\land ))=Ad(\begin{bmatrix} C \quad r\\ 0^T \quad1 \end{bmatrix})\\ ~\\=\begin{bmatrix} C \quad r^\land C \\ ~\\ 0 \quad \phi^ \land \end{bmatrix}
Ad(exp(ξ∧))=exp(ad(ξ∧))=Ad([Cr0T1]) =⎣⎡Cr∧C 0ϕ∧⎦⎤
2.卡尔曼滤波基础
3.位姿推导
针对离散模型：
标
称
运
动
公
式
：
T
k
=
exp
(
Δ
t
k
ϖ
k
∧
)
T
k
−
1
测
量
模
型
：
y
j
k
=
D
T
T
k
p
j
\begin{aligned} 标称运动公式&：T_k=\exp(\Delta t_k ϖ_k^\land)T_{k-1}\\~\\ 测量模型&：y_{jk}=D^TT_kp_j \end{aligned}
标称运动公式 测量模型：Tk=exp(Δtkϖk∧)Tk−1：yjk=DTTkpj
其中
ϖ
k
ϖ_k
ϖk为广义的速度；
扰动运动：
δ
ξ
ˇ
k
=
exp
(
Δ
t
k
ϖ
k
⋏
)
δ
ξ
^
k
−
1
+
ω
k
δ
y
j
k
=
D
T
(
T
ˇ
k
p
j
)
⊙
δ
ξ
ˇ
k
+
n
j
k
\begin{aligned} \delta \check \xi_k&=\exp(\Delta t_k ϖ_k ^ ⋏)\delta \hat \xi_{k-1}+\omega_k\\~\\ \delta y_{jk}&=D^T(\check T_kp_j) ^⊙\delta \check \xi_k+n_{jk} \end{aligned}
δξˇk δyjk=exp(Δtkϖk⋏)δξ^k−1+ωk=DT(Tˇkpj)⊙δξˇk+njk
其中：
F
k
−
1
=
exp
(
Δ
t
k
ϖ
k
⋏
)
G
j
k
=
D
T
(
T
ˇ
k
p
j
)
⊙
F_{k-1} = \exp(\Delta t_k ϖ_k ^ ⋏)\\~\\ G_{jk}=D^T(\check T_kp_j) ^⊙
Fk−1=exp(Δtkϖk⋏) Gjk=DT(Tˇkpj)⊙
则由卡尔曼滤波：
预测：
P
ˇ
k
=
F
k
−
1
P
^
k
−
1
F
k
−
1
T
+
Q
k
T
ˇ
k
=
exp
(
δ
ξ
∧
)
T
^
k
−
1
\check P_k = F_{k-1} \hat P_{k-1} F_{k-1}^T+Q_k\\~\\ \check T_k = \exp(\delta \xi ^\land) \hat T_{k-1}
Pˇk=Fk−1P^k−1Fk−1T+Qk Tˇk=exp(δξ∧)T^k−1
更新：
K
k
=
P
ˇ
k
⋅
G
k
T
(
G
k
P
ˇ
k
G
k
T
+
R
k
)
−
1
P
^
k
=
(
1
−
K
k
G
k
)
P
k
ˇ
T
^
k
=
exp
(
(
K
k
(
y
k
−
y
ˇ
k
)
)
∧
)
T
ˇ
k
\begin{aligned} K_k&=\bm {\check{P}_k}\cdot \bm G_k^T(\bm G_k\bm{\check{P}_k}\bm G_k^T+\bm R_k)^{-1}\\ ~\\ \bm {\hat{P}_k}&=(1-K_k\bm G_k)\bm {\check{P_k}}\\ ~\\ \bm {\hat{T}_k}&= \exp((K_k(y_k-\check y_k))^ \land)\check T_k \end{aligned}
Kk P^k T^k=Pˇk⋅GkT(GkPˇkGkT+Rk)−1=(1−KkGk)Pkˇ=exp((Kk(yk−yˇk))∧)Tˇk
二、非线性优化（高斯牛顿迭代）
1.高斯牛顿迭代基础
2.推导
对于运动方程和观测方程：
T
ˇ
k
=
exp
(
δ
ξ
∧
)
T
^
k
−
1
y
ˇ
j
k
=
u
m
(
T
ˇ
k
p
j
)
\check T_k = \exp(\delta \xi ^\land) \hat T_{k-1}\\~\\ \check y_{jk} = u_m(\check T_kp_{j})
Tˇk=exp(δξ∧)T^k−1 yˇjk=um(Tˇkpj)
优化的代价函数：
L
(
T
ˇ
k
)
=
1
2
∣
∣
y
j
k
−
u
m
(
T
ˇ
k
p
j
)
∣
∣
2
L(\check T_k)=\frac 1 2 {||y_{jk}-u_m(\check T_kp_{j}) ||}^2
L(Tˇk)=21∣∣yjk−um(Tˇkpj)∣∣2
令
e
(
T
ˇ
k
)
=
y
j
k
−
u
m
(
T
ˇ
k
p
j
)
e( \check T_k) =y_{jk}-u_m(\check T_kp_{j})
e(Tˇk)=yjk−um(Tˇkpj)
计算雅可比矩阵，由链式求导法则：
J
(
T
ˇ
k
)
=
∂
e
(
T
ˇ
k
)
∂
δ
ξ
=
∂
u
m
(
T
ˇ
k
p
j
)
∂
T
ˇ
k
p
j
⋅
∂
T
ˇ
k
p
j
∂
δ
ξ
J(\check T_k)=\frac{\partial e(\check T_k)}{\partial \delta \xi}=\frac{\partial u_m(\check T_kp_{j})}{\partial \check T_kp_j}\cdot \frac{\partial \check T_kp_j}{\partial \delta \xi}
J(Tˇk)=∂δξ∂e(Tˇk)=∂Tˇkpj∂um(Tˇkpj)⋅∂δξ∂Tˇkpj
扰动模型的导数：
∂
T
ˇ
k
p
j
∂
δ
ξ
=
(
T
ˇ
k
p
j
)
⊙
\frac{\partial \check T_kp_j}{\partial \delta \xi}=(\check T_kp_j) ^⊙
∂δξ∂Tˇkpj=(Tˇkpj)⊙
所以雅可比矩阵为：
J
=
∂
u
m
(
x
)
∂
x
⋅
(
T
ˇ
k
p
j
)
⊙
J = \frac{\partial u_m(x)}{\partial x}\cdot (\check T_kp_j) ^⊙
J=∂x∂um(x)⋅(Tˇkpj)⊙
求解
Δ
x
\Delta x
Δx:
J
(
T
ˇ
k
)
T
J
(
T
ˇ
k
)
δ
ϵ
=
−
J
(
T
ˇ
k
)
T
⋅
e
(
T
ˇ
k
)
J(\check T_k)^TJ(\check T_k)\delta ϵ=-J(\check T_k)^T \cdot e(\check T_k)
J(Tˇk)TJ(Tˇk)δϵ=−J(Tˇk)T⋅e(Tˇk)
迭代更新：
T
o
p
=
exp
(
δ
ϵ
∧
)
T
o
p
T_{op} = \exp(\delta ϵ ^ \land)T_{op}
Top=exp(δϵ∧)Top
北京人形旗下天工造物具身智能开源社区，聚焦具身天工与慧思开物两大平台
更多推荐
1
0- 0
已为社区贡献6条内容
所有评论(0)