一、最小二乘法

回顾最小二乘问题:

min⁡xF(x)=12∑i=1N∥f(x)∥22\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)∂xJ(x)=(∂F(x)∂x1,∂F(x)∂x2,...,∂F(x)∂xn)\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)=JT(x)=(∂F(x)∂x1,∂F(x)∂x2,...,∂F(x)∂xn)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
如果无法直接求解最小二乘问题，我们就需要利用迭代的方法来实现求解最优值。

我们给定一个初值x0x_0x0以及$λ $步长，则参数的更新方式为：
x(k+1)=x(k)−λJT(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(θ)=12∑i=1N∥z−hθ(x,θ)∥22=12(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θ⋅Δθ+12⋅Δθ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当作参数，给定初值x0x_0x0，则最优解：

x=x0+Δxx = x_0+\Delta x x=x0+Δx
我们把ΔxΔxΔx作为优化的参数：
min⁡ΔxF(x+Δx)=12∑i=1N∥f(x+Δx)∥22\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)+Jx⋅Δx+12⋅ΔxT⋅Hx⋅ΔxF(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：
Jx+ΔxTHx⟹JxT+HxΔx=0J_{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∂xTA⋅x∂x=2xTA∂uT⋅v∂x=uT⋅∂v∂x+vT⋅∂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
得到增量方程:
HxΔx=−JxTH_x\Delta x=-J^T_x HxΔx=−JxT
这种方法又被称为牛顿法 ,但存在一个缺点就是H的计算太过复杂，不利于直接利用。所以对于最小二乘法，常常有比较实用的方法：高斯牛顿法和列文伯格-马夸尔特法。

高斯牛顿法

对于优化函数

min⁡ΔxF(x+Δx)=12∑i=1N∥f(x+Δx)∥22\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)+Jx⋅Δx+12⋅ΔxT⋅Hx⋅Δx⟹F(x+Δx)=12∑i=1N∥f(x)+Jx⋅Δx∥22=12(f(x)+Jx⋅Δx)T(f(x)+Jx⋅Δ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)+Jx⋅Δx)T⋅Jx\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)+Jx⋅Δx)T⋅Jx=0⟹JxT⋅(f(x)+Jx⋅Δx)=0⟹JT(x)⋅f(x)+JT(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
所以增量方程为:
JT(x)⋅J(x)⋅Δx=−JT(x)⋅f(x)⟹HΔx=b(H=JT(x)⋅J(x),b=−JT(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(xk)J(x_k)J(xk)和f(xk)f(x_k)f(xk);
3.求解ΔxkΔx_kΔxk,可以用LDLTLDL^TLDLT分解法求解
4.Δx满足要求，退出迭代，否则令x(k+1)=xk+Δxkx_{(k+1)}=x_k+Δx_kx(k+1)=xk+Δxk

列文伯格-马夸尔特方法

推导略；
目标函数：
L(Δxk,λ)=12∑i=1N∥f(xk)+JT(xk)Δxk)∥22+λ2(∥D⋅Δxk∥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矩阵为JTJJ^TJJTJ的对角元素平方根；
相当于求解方程:
(H+λI)⋅Δxk=g(\bm H+\lambda \bm I)\cdot \Delta x_k=g(H+λI)⋅Δxk=g
这种方法可以避免HHH矩阵为奇异矩阵（奇异矩阵不可逆），当λ趋近于0时，相当于是高斯牛顿法；当λ很大时，相当于是梯度下降法。

SLAM--非线性优化相关推荐

视觉SLAM⑥---非线性优化
目录 6.0 主要目标 6.1 SLAM问题的数学表达 6.2 状态估计问题 6.2.1 批量状态估计与最大后验估计 6.1.2 最小二乘的引出 6.1.3 例子:批量状态估计 6.2 非线性最小二乘 ...
vins中imu融合_基于非线性优化算法—当视觉SLAM遇到VINS会碰撞出怎样的火花？
今天来给大家分享一个视觉SLAM中比较综合且复杂的系统-VINS.VINS旨在通过融合两个传感器测量数据获得移动机器人的位姿和特征点在空间中的位置,在现代控制理论学科中属于最优估计问题.在移动智能机器 ...
视觉SLAM十四讲学习笔记-第六讲学习笔记总结（1）---非线性优化原理
第六讲学习笔记如下: 视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题_ ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的实践-高斯牛顿法和曲线拟合
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的非线性最小二乘问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
视觉SLAM十四讲学习笔记-第六讲-非线性优化的状态估计问题
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
SLAM专题（6）-- 非线性优化
0. 摘要: SLAM中经常遇到非线性优化问题:多个误差项平方和组成的最小二乘问题,两个最常见的梯度下降方法-非线性优化方案:高斯牛顿法.裂纹伯格-马夸尔特方法. 两个基于c++编写的优化库:来自谷歌 ...
高博SLAM基础课第五讲——PnP非线性优化
这一题很重要 #include <Eigen/Core> #include <Eigen/Dense>using namespace Eigen;#include <ve ...
视觉SLAM⑨后端Ⅰ(KF、EKF、非线性优化)
目录 9.0 本章内容 9.1 概述 9.1.1 状态估计的概率解释 9.1.2 线性系统和KF 9.1.3 非线性系统和EKF 9.1.4 EKF的讨论 9.2 BA与图优化 9.2.1 投影模型和 ...
视觉SLAM ch6 非线性优化
一.首先理解运动方程和观测方程 (1)运动方程: x表示相机的位姿,每个时刻位姿记为x1, x2...... xk,构成了运动的轨迹.uk 是传感器的输入:wk 是噪声,假设噪声服从高斯分布. 运动方 ...

SLAM--非线性优化

目录

一、最小二乘法

二、求解增量方程

高斯牛顿法

列文伯格-马夸尔特方法

SLAM--非线性优化相关推荐

最新文章

热门文章