SLAM--非线性优化
一、最小二乘法
回顾最小二乘问题:
min x F ( x ) = 1 2 ∑ i = 1 N ∥ f ( x ) ∥ 2 2 \mathop {\min }\limits_x F(x) = \frac {1}{2}\sum\limits_{i = 1}^N {\left\| f(x)\right\|_2^2} xminF(x)=21i=1∑N∥f(x)∥22
观察这个函数可以发现,它同二次函数一样,存在着极小值,要使得
F
(
x
)
F(x)
F(x)达到最小值,只需要令:
∂
F
(
x
)
∂
x
=
0
\frac {\partial \bm F(x)}{\partial x}=0
∂x∂F(x)=0
我们其成为(Jacobian)雅可比矩阵:
J
(
x
)
=
∂
F
(
x
)
∂
x
J
(
x
)
=
(
∂
F
(
x
)
∂
x
1
,
∂
F
(
x
)
∂
x
2
,
.
.
.
,
∂
F
(
x
)
∂
x
n
)
\begin{aligned} \bm J(x) &=\frac {\partial F(x)}{\partial x} \\ \bm J(x) &= (\frac {\partial \bm F(x)}{\partial x_1} ,\frac {\partial \bm F(x)}{\partial x_2} ,...,\frac {\partial \bm F(x)}{\partial x_n} ) \end{aligned}
J(x)J(x)=∂x∂F(x)=(∂x1∂F(x),∂x2∂F(x),...,∂xn∂F(x))
我们将雅可比矩阵的转置称为梯度:
▽
F
(
x
)
=
J
T
(
x
)
=
(
∂
F
(
x
)
∂
x
1
,
∂
F
(
x
)
∂
x
2
,
.
.
.
,
∂
F
(
x
)
∂
x
n
)
T
\bigtriangledown F(x)=J^T(x) = (\frac {\partial F(x)}{\partial x_1} ,\frac {\partial F(x)}{\partial x_2} ,...,\frac {\partial F(x)}{\partial x_n} )^T
▽F(x)=JT(x)=(∂x1∂F(x),∂x2∂F(x),...,∂xn∂F(x))T
如果无法直接求解最小二乘问题,我们就需要利用迭代的方法来实现求解最优值。
我们给定一个初值
x
0
x_0
x0以及$λ $步长,则参数的更新方式为:
x
(
k
+
1
)
=
x
(
k
)
−
λ
J
T
(
x
(
k
)
)
\bm {x^{(k+1)} = x^{(k)}- λ J^T(x^{(k)})}
x(k+1)=x(k)−λJT(x(k))
这种方式我们称为梯度下降法。
具体的,存在一种映射变化h,观测值输入为x,输出为y(可以理解为数据集),为了找到一种最优的映射来估计z状态,使得:
min
θ
R
(
θ
)
=
1
2
∑
i
=
1
N
∥
z
−
h
θ
(
x
,
θ
)
∥
2
2
=
1
2
(
z
−
h
θ
(
x
,
θ
)
)
T
(
z
−
h
θ
(
x
,
θ
)
)
\begin{aligned} \mathop {\min }\limits_{\theta} R(\theta ) &= \frac {1}{2}\sum\limits_{i = 1}^N {\left\| z-h_{\theta}(x,\theta)\right\|_2^2} \\ &= \frac {1}{2}(z-h_{\theta}(x,\theta))^T(z-h_{\theta}(x,\theta)) \end{aligned}
θminR(θ)=21i=1∑N∥z−hθ(x,θ)∥22=21(z−hθ(x,θ))T(z−hθ(x,θ))
对其泰勒展开:
R
(
θ
+
Δ
θ
)
≈
R
(
θ
)
+
J
θ
⋅
Δ
θ
+
1
2
⋅
Δ
θ
T
⋅
H
θ
⋅
Δ
θ
,
(
H
θ
为
海
塞
矩
阵
)
R(\theta +\Delta \theta) \approx R(\theta)+J_{\theta} \cdot \Delta \theta+\frac 1 2\cdot \Delta \theta ^T \cdot H_{\theta} \cdot \Delta \theta,(H_\theta为海塞矩阵)
R(θ+Δθ)≈R(θ)+Jθ⋅Δθ+21⋅ΔθT⋅Hθ⋅Δθ,(Hθ为海塞矩阵)
其中雅克比矩阵:
J
θ
(
θ
)
=
∂
R
(
θ
)
∂
θ
J
θ
(
θ
)
=
(
h
θ
(
x
,
θ
)
−
z
)
T
∂
h
θ
(
x
,
θ
)
∂
θ
\begin{aligned} J_\theta(\theta) &= \frac {\partial R(\theta)}{\partial \theta}\\ \bm {J_\theta(\theta)} &= \bm {(h_{\theta}(x,\theta)-z)^T \frac {\partial h_{\theta}(x,\theta)}{\partial \theta}}\ \end{aligned}
Jθ(θ)Jθ(θ)=∂θ∂R(θ)=(hθ(x,θ)−z)T∂θ∂hθ(x,θ)
⟹
J
θ
T
(
θ
)
=
(
∂
h
θ
(
x
,
θ
)
∂
θ
)
T
⋅
(
h
θ
(
x
,
θ
)
−
z
)
\Longrightarrow \bm {J_\theta ^T(\theta) = (\frac {\partial h_{\theta}(x,\theta)}{\partial \theta})^T\cdot (h_{\theta}(x,\theta)-z)}
⟹JθT(θ)=(∂θ∂hθ(x,θ))T⋅(hθ(x,θ)−z)
二、求解增量方程
我们也可以利用增量Δx来求最优解,把
Δ
x
Δx
Δx当作参数,给定初值
x
0
x_0
x0,则最优解:
x
=
x
0
+
Δ
x
x = x_0+\Delta x
x=x0+Δx
我们把
Δ
x
Δx
Δx作为优化的参数:
min
Δ
x
F
(
x
+
Δ
x
)
=
1
2
∑
i
=
1
N
∥
f
(
x
+
Δ
x
)
∥
2
2
\mathop {\min }\limits_{\Delta x} F(x+\Delta x) = \frac {1}{2}\sum\limits_{i = 1}^N {\left\| f(x+\Delta x)\right\|_2^2}
ΔxminF(x+Δx)=21i=1∑N∥f(x+Δx)∥22
对于
F
(
x
+
Δ
x
)
F(x+Δx)
F(x+Δx)的泰勒展开式:
F
(
x
+
Δ
x
)
≈
F
(
x
)
+
J
x
⋅
Δ
x
+
1
2
⋅
Δ
x
T
⋅
H
x
⋅
Δ
x
F(x +\Delta x) \approx F(x)+J_{x} \cdot \Delta x+\frac 1 2\cdot \Delta x ^T \cdot H_{x} \cdot \Delta x
F(x+Δx)≈F(x)+Jx⋅Δx+21⋅ΔxT⋅Hx⋅Δx
令
F
(
x
)
F(x)
F(x)为无穷小量,对
Δ
x
Δx
Δx求导,并令导数为0:
J
x
+
Δ
x
T
H
x
⟹
J
x
T
+
H
x
Δ
x
=
0
J_{x} +\Delta x^T H_x\\ \Longrightarrow J^T_{x} +H_x\Delta x=0
Jx+ΔxTHx⟹JxT+HxΔx=0
矩阵的求导用到 两个重要的知识点:
若a和A不是关于x的函数矩阵,则有:
∂
a
⋅
x
∂
x
=
a
∂
x
T
A
⋅
x
∂
x
=
2
x
T
A
∂
u
T
⋅
v
∂
x
=
u
T
⋅
∂
v
∂
x
+
v
T
⋅
∂
u
∂
x
\frac {\partial a \cdot \bm x}{\partial \bm x} = a\\ \\ \frac {\partial \bm x^T A \cdot \bm x}{\partial \bm x}=2\bm x^T A\\ \frac {\partial \bm u^T \cdot \bm v}{\partial \bm x} = \bm u^T \cdot \frac {\partial \bm v}{\partial \bm x}+ \bm v^T \cdot \frac {\partial \bm u}{\partial \bm x}
∂x∂a⋅x=a∂x∂xTA⋅x=2xTA∂x∂uT⋅v=uT⋅∂x∂v+vT⋅∂x∂u
得到增量方程:
H
x
Δ
x
=
−
J
x
T
H_x\Delta x=-J^T_x
HxΔx=−JxT
这种方法又被称为 牛顿法 ,但存在一个缺点就是H的计算太过复杂,不利于直接利用。所以对于最小二乘法,常常有比较实用的方法:高斯牛顿法和列文伯格-马夸尔特法。
高斯牛顿法
对于优化函数
min
Δ
x
F
(
x
+
Δ
x
)
=
1
2
∑
i
=
1
N
∥
f
(
x
+
Δ
x
)
∥
2
2
\mathop {\min }\limits_{\Delta x} F(x+\Delta x) = \frac {1}{2}\sum\limits_{i = 1}^N {\left\| f(x+\Delta x)\right\|_2^2}
ΔxminF(x+Δx)=21i=1∑N∥f(x+Δx)∥22
对f(x)进行泰勒展开:
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
x
⋅
Δ
x
+
1
2
⋅
Δ
x
T
⋅
H
x
⋅
Δ
x
⟹
F
(
x
+
Δ
x
)
=
1
2
∑
i
=
1
N
∥
f
(
x
)
+
J
x
⋅
Δ
x
∥
2
2
=
1
2
(
f
(
x
)
+
J
x
⋅
Δ
x
)
T
(
f
(
x
)
+
J
x
⋅
Δ
x
)
\begin{aligned} f(x +\Delta x)& \approx f(x)+J_{x} \cdot \Delta x+\frac 1 2\cdot \Delta x ^T \cdot H_{x} \cdot \Delta x\\ \Longrightarrow F(x+\Delta x) &= \frac {1}{2}\sum\limits_{i = 1}^N {\left\| f(x)+J_{x} \cdot \Delta x \right\|_2^2} \\&= \frac 1 2 (f(x)+J_x\cdot \Delta x)^T(f(x)+J_x\cdot \Delta x) \end{aligned}
f(x+Δx)⟹F(x+Δx)≈f(x)+Jx⋅Δx+21⋅ΔxT⋅Hx⋅Δx=21i=1∑N∥f(x)+Jx⋅Δx∥22=21(f(x)+Jx⋅Δx)T(f(x)+Jx⋅Δx)求导:
∂
F
(
x
+
Δ
x
)
∂
Δ
x
=
(
f
(
x
)
+
J
x
⋅
Δ
x
)
T
⋅
J
x
\frac {\partial F(x+\Delta x)}{\partial \Delta x}=(f(x)+J_x\cdot \Delta x)^T \cdot J_x
∂Δx∂F(x+Δx)=(f(x)+Jx⋅Δx)T⋅Jx
令其等于0:
(
f
(
x
)
+
J
x
⋅
Δ
x
)
T
⋅
J
x
=
0
⟹
J
x
T
⋅
(
f
(
x
)
+
J
x
⋅
Δ
x
)
=
0
⟹
J
T
(
x
)
⋅
f
(
x
)
+
J
T
(
x
)
⋅
J
(
x
)
⋅
Δ
x
=
0
\begin{aligned} (f(x)+J_x\cdot \Delta x)^T \cdot J_x &= 0\\ \Longrightarrow J_x ^T \cdot (f(x)+J_x\cdot \Delta x) &=0\\ \Longrightarrow J^T(x) \cdot f(x)+ J^T(x)\cdot J(x)\cdot &\Delta x =0 \end{aligned}
(f(x)+Jx⋅Δx)T⋅Jx⟹JxT⋅(f(x)+Jx⋅Δx)⟹JT(x)⋅f(x)+JT(x)⋅J(x)⋅=0=0Δx=0
所以增量方程为:
J
T
(
x
)
⋅
J
(
x
)
⋅
Δ
x
=
−
J
T
(
x
)
⋅
f
(
x
)
⟹
H
Δ
x
=
b
(
H
=
J
T
(
x
)
⋅
J
(
x
)
,
b
=
−
J
T
(
x
)
⋅
f
(
x
)
)
\begin{aligned} J^T(x)\cdot J(x)\cdot \Delta x &=-J^T(x) \cdot f(x)\\ \Longrightarrow H\Delta x&=b\\(H= J^T(x)\cdot J(x) ,b=-J^T(x) \cdot f(x)) \end{aligned}
JT(x)⋅J(x)⋅Δx⟹HΔx(H=JT(x)⋅J(x),b=−JT(x)⋅f(x))=−JT(x)⋅f(x)=b
迭代步骤:
1.给定初值x0;
2.求出第k次迭代的
J
(
x
k
)
J(x_k)
J(xk)和
f
(
x
k
)
f(x_k)
f(xk);
3.求解
Δ
x
k
Δx_k
Δxk,可以用
L
D
L
T
LDL^T
LDLT分解法求解
4.Δx满足要求,退出迭代,否则令
x
(
k
+
1
)
=
x
k
+
Δ
x
k
x_{(k+1)}=x_k+Δx_k
x(k+1)=xk+Δxk
列文伯格-马夸尔特方法
推导略;
目标函数:
L
(
Δ
x
k
,
λ
)
=
1
2
∑
i
=
1
N
∥
f
(
x
k
)
+
J
T
(
x
k
)
Δ
x
k
)
∥
2
2
+
λ
2
(
∥
D
⋅
Δ
x
k
∥
2
−
μ
)
L(\Delta x_k,\lambda) = \frac {1}{2}\sum\limits_{i = 1}^N {\left\| f(x_k)+J^T(x_k)\Delta x_k)\right\|_2^2} +\frac \lambda 2 (\left\| D\cdot \Delta x_k\right\|^2-\mu )
L(Δxk,λ)=21i=1∑N∥∥f(xk)+JT(xk)Δxk)∥∥22+2λ(∥D⋅Δxk∥2−μ)
一般的,D矩阵为
J
T
J
J^TJ
JTJ的对角元素平方根;
相当于求解方程:
(
H
+
λ
I
)
⋅
Δ
x
k
=
g
(\bm H+\lambda \bm I)\cdot \Delta x_k=g
(H+λI)⋅Δxk=g
这种方法可以避免
H
H
H矩阵为奇异矩阵(奇异矩阵不可逆),当λ趋近于0时,相当于是高斯牛顿法;当λ很大时,相当于是梯度下降法。
更多推荐
所有评论(0)