本章说
题头诗：
重走SLAM路，之前学的全忘光
今日重拾起，痛定思痛写总结
长征路漫漫，君当切实行
第九讲主要内容集中在卡尔曼滤波以及非线性优化方法中的稀疏化和边缘化
讲内容
卡尔曼滤波
卡尔曼滤波器就是利用已知的高斯误差分布从而确定最优无偏估计的轨迹和误差的过程
这里先以KF为例介绍一下：
1.预测
已知k-1时刻状态（包含相机位姿和路标点位置）的后验概率分布为 N ( x k − 1 ^ , p k − 1 ^ ) ) N(\hat{x_{k-1}},\hat{p_{k-1}})) N(xk−1^,pk−1^))，可根据运动和观测方程确定k时刻的先验分布
2.更新
更新的逻辑在于 P ( x k ∣ z k ) ∝ P ( z k ∣ x k ) P ( x k ∣ x 0 , . . . , u k − 1 , z k − 1 ) P(x_k|z_k) \varpropto P(z_k|x_k)P(x_k|x_0,...,u_{k-1},z_{k-1}) P(xk∣zk)∝P(zk∣xk)P(xk∣x0,...,uk−1,zk−1)，写出后两项的分布，根据概率相乘公式即可确定后验
EKF也是类似的逻辑，只不过由于观测和运动方程的非线性，所以去了一阶泰勒近似，从而保持高斯分布成立
稀疏性和边缘化
- 设定 x = T 1 , . . . , T m , P 1 , . . . , P n x={T_1,...,T_m,P_1,...,P_n} x=T1,...,Tm,P1,...,Pn
- z = h ( x ) z=h(x) z=h(x)，即由路标导图像上像素点的过程
- 目标 m i n x ∑ z i j − h ( x ) min_x\sum z_{ij}-h(x) minx∑zij−h(x)
单取一个
z
−
h
(
x
)
z-h(x)
z−h(x)出来研究，对于某一个观测
z
i
j
z_{ij}
zij它会关联某一个相机位姿i和路标点j，记这个误差为
e
i
j
e_{ij}
eij
J
i
j
=
d
e
i
j
d
x
=
[
∂
e
i
j
∂
T
i
,
0
,
∂
e
i
j
∂
P
j
,
0
]
J_{ij}=\frac{de_{ij}}{dx}=[\frac{\partial e_{ij}}{\partial T_i},0,\frac{\partial e_{ij}}{\partial P_j},0]
Jij=dxdeij=[∂Ti∂eij,0,∂Pj∂eij,0]
所有
J
i
j
J_{ij}
Jij迭代得到
J
J
J，是一个箭头型分块矩阵，对应的H也有类似特征，据此可得出：
H
=
[
B
E
E
T
C
]
H=\begin{bmatrix} B & E \\ E^T & C \end{bmatrix}
H=[BETEC]
又由
H
Δ
x
=
g
H\Delta x=g
HΔx=g
H
=
[
B
E
E
T
C
]
[
Δ
x
T
Δ
x
P
]
=
[
v
w
]
H=\begin{bmatrix} B & E \\ E^T & C \end{bmatrix}\begin{bmatrix}\Delta x_T \\ \Delta x_P\end{bmatrix}=\begin{bmatrix}v \\ w\end{bmatrix}
H=[BETEC][ΔxTΔxP]=[vw]
由于B,C均是对较快矩阵，求逆是简单的，因此用高斯消元去除E，化简得到：
由1行固定
Δ
x
P
\Delta x_P
ΔxP求出了
Δ
x
T
\Delta x_T
ΔxT，带入2行求出
Δ
x
P
\Delta x_P
ΔxP，即
P
(
Δ
x
T
∣
Δ
x
P
)
P
(
Δ
x
P
)
P(\Delta x_T|\Delta x_P)P(\Delta x_P)
P(ΔxT∣ΔxP)P(ΔxP)，这也是这个过程叫做边缘化的原因，因为求得的是
Δ
x
P
\Delta x_P
ΔxP的边缘概率
1965
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
