本章说
题头诗：
重走SLAM路，之前学的全忘光
今日重拾起，痛定思痛写总结
长征路漫漫，君当切实行
第六讲主要理解位姿和坐标点估计问题如何转换成一个最小二乘问题：即该问题为什么是一个最大后验概率问题，如何转化为极大似然估计问题，又如何转化为最小二乘问题；其次，理解最小二乘法问题的迭代求解方法：首先理解牛顿法的含义，然后理解其局限性以及针对局限性提出的高斯-牛顿法和更进一步的列文伯格-马夸尔特方法。
讲内容
首先，先说说为什么位姿和坐标点估计问题可以转换成一个最小二乘问题
要理清以上疑问，可以遵循以下思路：
- 为什么位姿与坐标点估计问题是一个最大后验概率问题？其为什么可以转换成极大似然估计问题？
- 极大似然估计问题怎么转换成的最小二乘问题？
先说第一个，我们知道，在SLAM问题中，我们已知的输入是
u
u
u与
z
z
z，即位姿增量和观测结果，而我们要求的是
x
x
x和
y
y
y，即实际位姿和实际坐标点位置。因此该问题可概述为一个最大后验概率问题
P
(
x
,
y
∣
u
,
z
)
P(x,y|u,z)
P(x,y∣u,z)。而根据运动方程和观测方程
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
j
=
h
(
y
j
,
x
k
)
+
v
k
j
\left\{ \begin{aligned} x_k & = & \ f(x_{k-1},u_k)+w_k\\ z_{kj} & = & \ h(y_j,x_k)+v_{kj}\\ \end{aligned} \right.
{xkzkj== f(xk−1,uk)+wk h(yj,xk)+vkj
可以发现已知输入求实际的最大后验概率是很难的，因为我们无法预知该过程的误差分布。但是根据一般经验，我们知道从实际到输入过程的误差大多数服从高斯分布，这就为我们带来了解题的思路，那就是将最大后验概率问题转换成极大似然估计问题，从而利用起已知的误差分布。
根据贝叶斯公式，求最大后验概率即求似然估计与先验概率乘积的最大：
忽略先验概率就转换成了求极大似然估计：
综上，我们完成了第一步的理解，即从实际问题->最大后验估计->极大似然估计
第二点，极大似然估计怎么转换成最小二乘问题：
我们从观测方程入手，已知
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
z_{k,j}=h(y_j,x_k)+v_{k,j}
zk,j=h(yj,xk)+vk,j，又知
v
k
,
j
∼
N
(
0
,
Q
k
,
j
)
v_{k,j}\sim N(0,Q_{k,j})
vk,j∼N(0,Qk,j)，所以
z
k
,
j
∼
N
(
h
(
y
j
,
x
k
)
,
Q
k
,
j
)
z_{k,j}\sim N(h(y_j,x_k),Q_{k,j})
zk,j∼N(h(yj,xk),Qk,j)。极大似然估计即是让高斯分布的概率密度函数最大，我们对概率密度函数取负对数，可得高斯分布下求极大似然估计等价于最小化如下式子：
故而可带入
z
k
,
j
z_{k,j}
zk,j的分布，得到如下表达：
同理推导运动方程下的极大似然表达：
已知
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
x_k=f(x_{k-1},u_k)+w_k
xk=f(xk−1,uk)+wk，又知
w
k
∼
N
(
0
,
R
k
)
w_k\sim N(0,R_k)
wk∼N(0,Rk)，可得
f
(
x
k
−
1
,
u
k
)
∼
(
x
k
,
R
k
)
f(x_{k-1},u_k)\sim ( x_k,R_k)
f(xk−1,uk)∼(xk,Rk)。同样的，写出高斯分布并求概率的极大值，通过负对数将求极大转换成求极小，可以推得
(
x
k
)
∗
=
a
r
g
m
i
n
(
(
x
k
−
f
(
x
k
−
1
,
u
k
)
)
T
R
k
−
1
(
x
k
−
f
(
x
k
−
1
,
u
k
)
)
(x_k)^*=arg min((x_k-f(x_{k-1},u_k))^TR_k^{-1}(x_k-f(x_{k-1},u_k))
(xk)∗=argmin((xk−f(xk−1,uk))TRk−1(xk−f(xk−1,uk))。
因此，我们可设如下误差项：
通过误差项，我们可以描述一个批量内（1~k）总的极大似然估计的等价最小化式子：
由此，我们得到了位姿与坐标点估计问题的最小二乘表达。
然后，说说最小二乘法问题的迭代求解方法
最小二乘问题的一般形式为： m i n F ( x ) = 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 min F(x)=\frac{1}{2}||f(x)||_2^2 minF(x)=21∣∣f(x)∣∣22，其中由于x可以是数、向量或者矩阵，所以高斯分布下的位姿与坐标点估计问题是可以写成这样的标准形式的
对于函数表达复杂的最小二乘问题，一般采取迭代求解的方法，即先将 F ( x ) F(x) F(x)泰勒展开，得到展开式如下： F ( x + Δ x ) ≈ F ( x ) + J T Δ x + Δ x T H Δ x F(x+\Delta x) \approx F(x)+\textbf{J}^T\Delta x+\Delta x^T\textbf{H}\Delta x F(x+Δx)≈F(x)+JTΔx+ΔxTHΔx，根据保留一阶梯度还是二阶梯度，可以分成最速下降法和牛顿法。最速下降法取 Δ x = − J \Delta x=-\textbf{J} Δx=−J，这可以保证每次均沿梯度下降，但为了迭代效果一般会加以步长限制。牛顿法保留二阶梯度，即 Δ x ∗ = a r g m i n ( F ( x ) + J T Δ x + Δ x T H Δ x ) \Delta x^* =argmin( F(x)+\textbf{J}^T\Delta x+\Delta x^T\textbf{H}\Delta x) Δx∗=argmin(F(x)+JTΔx+ΔxTHΔx)，我们可在该式中对 Δ x \Delta x Δx求导，导数取零可以求得使该式最小的 Δ x \Delta x Δx值，即求解方程 H Δ x = − J \textbf{H}\Delta x=-\textbf{J} HΔx=−J
由于对 F ( x ) F(x) F(x)求一阶梯度和二阶梯度的行为是复杂的，特别是在最小二乘问题中，这意味着一阶梯度是一定会包含自变量 x x x在内。因此，我们需要考虑其他的近似算法，消除自变量 x x x在梯度中的影响。
第一种近似算法是高斯-牛顿法，它对
f
(
x
)
f(x)
f(x)做一阶泰勒展开，得
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
T
Δ
x
f(x+\Delta x)\approx f(x)+\textbf{J}^T\Delta x
f(x+Δx)≈f(x)+JTΔx，再写出
F
(
x
+
Δ
x
)
=
1
2
∣
∣
f
(
x
)
+
J
T
Δ
x
∣
∣
2
2
F(x+\Delta x)=\frac{1}{2}||f(x)+\textbf{J}^T\Delta x||_2^2
F(x+Δx)=21∣∣f(x)+JTΔx∣∣22，对该式展开，展开结果为：
上式求
Δ
x
\Delta x
Δx的导数，并取导数为0，即求取
Δ
x
∗
=
a
r
g
m
i
n
(
F
(
x
+
Δ
x
)
)
\Delta x^* =argmin( F(x+\Delta x))
Δx∗=argmin(F(x+Δx))，得到方程
J
(
x
)
J
(
x
)
T
Δ
x
=
−
J
(
x
)
f
(
x
)
\textbf{J}(x)\textbf{J}(x)^T\Delta x=-\textbf{J}(x)f(x)
J(x)J(x)TΔx=−J(x)f(x)，左式中
J
(
x
)
J
(
x
)
T
\textbf{J}(x)\textbf{J}(x)^T
J(x)J(x)T写做
H
\textbf{H}
H，右式写做
g
\textbf{g}
g，完成了利用
J
(
x
)
J
(
x
)
T
\textbf{J}(x)\textbf{J}(x)^T
J(x)J(x)T替换海塞矩阵的目的（且保证了雅各比矩阵中无自变量，即形式简单）
第二种近似算法是列文伯格-马夸尔特方法，它针对高斯-牛顿法没有考察近似程度的不足，进行了一定的处理和修改，核心思想是利用如下指标 ρ = f ( x + Δ x − f ( x ) ) J ( x ) T Δ x \rho=\frac{f(x+ \Delta x-f(x))}{\textbf{J}(x)^T\Delta x} ρ=J(x)TΔxf(x+Δx−f(x))去衡量近似函数与实际取值的差异大小，如果 ρ \rho ρ接近于1，那说明近似与实际接近，这时候的 H \textbf{H} H效果好，在计算 Δ x ∗ \Delta x^* Δx∗时占比大；如果 ρ \rho ρ远小于1，那说明近似比较差，需要缩小近似范围；如果 ρ \rho ρ远大于1，说明近似还不足，可以扩大近似范围
具体的做法是将方程 F ( x + Δ x ) = 1 2 ∣ ∣ f ( x ) + J T Δ x ∣ ∣ 2 2 F(x+\Delta x)=\frac{1}{2}||f(x)+\textbf{J}^T\Delta x||_2^2 F(x+Δx)=21∣∣f(x)+JTΔx∣∣22变为一个有约束的最小二乘问题，约束为 ∣ ∣ D Δ x ∣ ∣ 2 ≤ μ ||\textbf{D}\Delta x||^2\leq\mu ∣∣DΔx∣∣2≤μ，其中 μ \mu μ是一个参数，初始给定且每次迭代变化，变化规则为：当 ρ \rho ρ接近于1， μ = μ \mu=\mu μ=μ；当 ρ \rho ρ远小于1， μ = 0.5 μ \mu=0.5\mu μ=0.5μ；当 ρ \rho ρ远大于1， μ = 2 μ \mu=2\mu μ=2μ。
那么如何求解有约束的最小二乘问题每次迭代的
Δ
x
∗
\Delta x^*
Δx∗？将方程化为拉格朗日方程形式：
F
(
x
+
Δ
x
)
=
1
2
∣
∣
f
(
x
)
+
J
T
Δ
x
∣
∣
2
2
+
λ
2
(
∣
∣
D
Δ
x
∣
∣
2
−
μ
)
F(x+\Delta x)=\frac{1}{2}||f(x)+\textbf{J}^T\Delta x||_2^2+\frac{\lambda}{2}(||\textbf{D}\Delta x||^2-\mu)
F(x+Δx)=21∣∣f(x)+JTΔx∣∣22+2λ(∣∣DΔx∣∣2−μ)，再对上式求
Δ
x
\Delta x
Δx与
λ
\lambda
λ的导数，并取导数为0，可得方程组
{
(
H
+
λ
D
T
D
)
Δ
x
=
g
∣
∣
D
Δ
x
∣
∣
2
−
μ
=
0
\left\{ \begin{aligned} (\textbf{H}+\lambda \textbf{D}^T\textbf{D})\Delta x=\textbf{g}\\ ||\textbf{D}\Delta x||^2-\mu=0\\ \end{aligned} \right.
{(H+λDTD)Δx=g∣∣DΔx∣∣2−μ=0
如果该方程组有解，那么对应的 Δ x \Delta x Δx就是此次增量；而如果方程组无解，那我们就应该把 λ \lambda λ看成一个惩罚项，若发现信赖域条件不满足，或者目标函数增加，就增加 λ \lambda λ的权重，反之减小 λ \lambda λ的权重，并通过式 ( H + λ D T D ) Δ x = g (\textbf{H}+\lambda \textbf{D}^T\textbf{D})\Delta x=\textbf{g} (H+λDTD)Δx=g给出此次增量。直观上看，当 λ \lambda λ较大的时候， D T D \textbf{D}^T\textbf{D} DTD起主要作用，特别当 D \textbf{D} D取单位矩阵时（表示限定为一个球内），此时列马方法接近最速下降法， Δ x ≈ g \Delta x \approx \textbf{g} Δx≈g；反之当 λ \lambda λ较小的时候， H \textbf{H} H起主要作用，列马方法接近高斯-牛顿法。 D \textbf{D} D的取值一般还可取成非负数对角阵——实际中通常用 J T J \textbf{J}^T\textbf{J} JTJ的对角元素平方根，使得在梯度小的维度上约束范围更大一些（梯度小，对角元素小，椭球的轴更长，约束范围更大）
902
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
