非线性优化

文章目录

非线性优化
- @[toc]
- 状态估计问题
- - 问题描述
  - 批量状态估计
  - 最大后验估计
- 非线性最小二乘
- - 引出
  - 求解
  - - 一阶和二阶梯度法
    - 高斯牛顿法
    - 列文伯格-马夸尔特方法

状态估计问题

问题描述

SLAM模型：
xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j\boldsymbol{x}_{k}=f(\boldsymbol{x}_{k-1},\boldsymbol{u}_{k})+\boldsymbol{w}_{k}\\ \boldsymbol{z}_{k,j}=h(\boldsymbol{y}_{j},\boldsymbol{x}_{k})+\boldsymbol{v}_{k,j} xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
运动方程与输入的具体形式有关，观测方程由针孔模型给定。

数据受噪声影响，有：
wk∼N(0,Rk),vk∼N(0,Qk,j)\boldsymbol{w}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol{R}_k),\boldsymbol{v}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
通过带噪声的数据z\boldsymbol zz和u\boldsymbol uu推断位姿x\boldsymbol xx和y\boldsymbol yy（以及它们的概率分布），这构成了一个状态估计问题。

批量状态估计

处理状态估计问题的方法有两种：

持有一个当前时刻的估计状态，然后用新的数据来更新。称为**增量/渐进（incremental）**的方法，或者叫做滤波器，如卡尔曼滤波器
把所有的数据收集到一起再处理，称为批量（batch）方法，即SfM（Structure from Motion），缺点是不实时

仅对当前时刻附近的一些轨迹进行优化，称为滑动窗口估计法

最大后验估计

定义所有时刻的机器人位姿和路标点坐标：
x=x1,⋯,xNy=y1,⋯,yM\boldsymbol x={\boldsymbol x_1,\cdots,\boldsymbol x_{N}}\\ \boldsymbol y={\boldsymbol y_1,\cdots,\boldsymbol y_{M}} x=x1,⋯,xNy=y1,⋯,yM
已知输入数据和观测数据，求x,y\boldsymbol x,\boldsymbol yx,y的条件概率分布：
P(x,y∣z,u)P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u) P(x,y∣z,u)
利用贝叶斯法则：
P(x,y∣z,u)⏟后验=P(z,u∣x,y)P(x,y)P(z,u)∝P(z,u∣x,y)⏟似然P(x,y)⏟先验\underbrace{P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u)}_{\text{后验}}=\frac{P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)P(\boldsymbol x,\boldsymbol y)}{P(\boldsymbol z,\boldsymbol u)}\propto\underbrace{P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)}_{似然}\underbrace{P(\boldsymbol x,\boldsymbol y)}_{先验} 后验P(x,y∣z,u)=P(z,u)P(z,u∣x,y)P(x,y)∝似然P(z,u∣x,y)先验P(x,y)
直接求后验分布是困难的，但是可以求一个状态最优估计，使得在该状态下后验概率最大化：
(x,y)∗MAP=arg⁡max⁡P(x,y∣z,u)=arg⁡max⁡P(z,u∣x,y)P(x,y){(\boldsymbol x,\boldsymbol y)^{*}}_{\text{MAP}}=\arg\max P(\boldsymbol x,\boldsymbol y\vert\boldsymbol z,\boldsymbol u)=\arg\max P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)P(\boldsymbol x,\boldsymbol y) (x,y)∗MAP=argmaxP(x,y∣z,u)=argmaxP(z,u∣x,y)P(x,y)
求解最大后验概率等价于最大化似然和先验的乘积。先验未知时，可以求解最大似然估计（Maximize Likelihood Estimation, MLE）：
(x,y)∗MLE=arg⁡max⁡P(z,u∣x,y){(\boldsymbol x,\boldsymbol y)^{*}}_{\text{MLE}}=\arg\max P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y) (x,y)∗MLE=argmaxP(z,u∣x,y)
可以理解为：在什么状态下，最可能产生现在观测到的数据

非线性最小二乘

引出

考虑任意高维高斯分布x∼N(μ,Σ)\boldsymbol x\sim\mathcal{N}(\boldsymbol\mu,\boldsymbol\Sigma)x∼N(μ,Σ)，有：
P(x)=1(2π)Ndet⁡(Σ)exp⁡(−12(x−μ)TΣ−1(x−μ))P(\boldsymbol x)=\frac{1}{\sqrt{(2\pi)^N\det(\boldsymbol\Sigma)}}\exp(-\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)}) P(x)=(2π)Ndet(Σ)1exp(−21(x−μ)TΣ−1(x−μ))
取其负对数，有：
−ln⁡(P(x))=12ln⁡((2π)Ndet⁡(Σ))+12(x−μ)TΣ−1(x−μ)-\ln(P(x))=\frac{1}{2}\ln((2\pi)^N\det(\boldsymbol\Sigma))+\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)} −ln(P(x))=21ln((2π)Ndet(Σ))+21(x−μ)TΣ−1(x−μ)
对原函数求最大化相当于对负对数求最小化（负对数第一项与自变量无关）。
max⁡P(x)=min⁡−ln⁡(P(x))=min⁡12(x−μ)TΣ−1(x−μ)\max\quad P(x)=\min\quad-\ln(P(x))=\min\quad\frac{1}{2}{(\boldsymbol x-\boldsymbol\mu)}^T\boldsymbol\Sigma^{-1}{(\boldsymbol x-\boldsymbol\mu)} maxP(x)=min−ln(P(x))=min21(x−μ)TΣ−1(x−μ)
假设噪声项服从高斯分布：
wk∼N(0,Rk),vk∼N(0,Qk,j)\boldsymbol{w}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol{R}_k),\boldsymbol{v}_k\sim\mathcal{N}(\boldsymbol0,\boldsymbol Q_{k,j}) wk∼N(0,Rk),vk∼N(0,Qk,j)
观测和输入（运动传感器读数）的条件概率均服从高斯分布：
P(uk∣xk−1,xk)=N(f(xk−1,uk),Rk)P(zk,j∣xkyj)=N(h(yj,xk),Qk,j)P(\boldsymbol u_k\vert\boldsymbol x_{k-1},\boldsymbol x_k)=\mathcal{N}(f(\boldsymbol x_{k-1},\boldsymbol u_k),\boldsymbol R_k)\\ P(\boldsymbol z_{k,j}\vert\boldsymbol x_k\boldsymbol y_j)=\mathcal{N}(h(\boldsymbol y_j,\boldsymbol x_k),\boldsymbol Q_{k,j}) P(uk∣xk−1,xk)=N(f(xk−1,uk),Rk)P(zk,j∣xkyj)=N(h(yj,xk),Qk,j)
对联合分布（似然）进行因式分解：
P(z,u∣x,y)=∏kP(uk∣xk−1,xk)∏k,jP(zk,j∣xkyj)P(\boldsymbol z,\boldsymbol u\vert\boldsymbol x,\boldsymbol y)=\prod_kP(\boldsymbol u_k\vert\boldsymbol x_{k-1},\boldsymbol x_k)\prod_{k,j}P(\boldsymbol z_{k,j}\vert\boldsymbol x_k\boldsymbol y_j) P(z,u∣x,y)=k∏P(uk∣xk−1,xk)k,j∏P(zk,j∣xkyj)
定义输入和观测与模型之间的误差：
eu,k=xk−f(xk−1,uk)ez,j,k=zk,j−h(yj,xk)e_{\boldsymbol u,k}=\boldsymbol x_k-f(\boldsymbol x_{k-1},\boldsymbol u_k)\\ e_{\boldsymbol z,j,k}=\boldsymbol z_{k,j}-h(\boldsymbol y_j,\boldsymbol x_k) eu,k=xk−f(xk−1,uk)ez,j,k=zk,j−h(yj,xk)
最大似然估计等价于最小化噪声项（误差）的二次型：
min⁡J(x,y)=∑keu,kTRk−1eu,k+∑k∑jez,j,kTQk,j−1ez,j,k\min J(\boldsymbol x,\boldsymbol y)=\sum_ke_{\boldsymbol u,k}^T\boldsymbol R_k^{-1}e_{\boldsymbol u,k}+\sum_k\sum_je_{\boldsymbol z,j,k}^T\boldsymbol Q_{k,j}^{-1}e_{\boldsymbol z,j,k} minJ(x,y)=k∑eu,kTRk−1eu,k+k∑j∑ez,j,kTQk,j−1ez,j,k
误差项的二次型称为马哈拉诺比斯距离（Mahalanobis distance），又称为马氏距离。也可以看作是Rk−1,Qk,j−1\boldsymbol R_k^{-1},\boldsymbol Q_{k,j}^{-1}Rk−1,Qk,j−1加权后的欧氏距离，Rk−1,Qk,j−1\boldsymbol R_k^{-1},\boldsymbol Q_{k,j}^{-1}Rk−1,Qk,j−1称为信息矩阵（高斯分布协方差矩阵的逆）

这样就得到了一个最小二乘问题，它的解等价于状态的最大似然估计。

求解

考虑简单的最小二乘问题：
KaTeX parse error: Got function '\boldsymbol' with no arguments as subscript at position 7: \min_\̲b̲o̲l̲d̲s̲y̲m̲b̲o̲l̲ ̲xF(\boldsymbol …
fff形式简单时，另目标函数导数为0有解析解：
dFdx=0\frac{dF}{d\boldsymbol x}=0 dxdF=0
形式复杂时用迭代方法，从一个初始值出发，不断更新当前的优化变量，使得目标函数下降：

给定初始值x0\boldsymbol x_0x0
对于第kkk次迭代，寻找一个增量Δxk\Delta \boldsymbol x_kΔxk，使得∥f(xk+Δxk)∥22\Vert f(\boldsymbol x_k+\Delta \boldsymbol x_k)\Vert^2_2∥f(xk+Δxk)∥22达到极小值
若Δxk\Delta \boldsymbol x_kΔxk足够小则停止
否则，令xk+1=xk+Δxk\boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_kxk+1=xk+Δxk，返回第2步

一阶和二阶梯度法

将目标函数在xk\boldsymbol x_kxk附近进行泰勒展开：
F(xk+Δxk)≈F(xk)+J(xk)TΔxk+12ΔxkTH(xk)ΔxkF(\boldsymbol x_k+\Delta\boldsymbol x_k)\approx F(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta \boldsymbol x_k+\frac{1}{2}\Delta\boldsymbol x_k^T\boldsymbol H(\boldsymbol x_k)\Delta \boldsymbol x_k F(xk+Δxk)≈F(xk)+J(xk)TΔxk+21ΔxkTH(xk)Δxk
其中，J(xk)\boldsymbol J(\boldsymbol x_k)J(xk)是F(x)F(\boldsymbol x)F(x)关于x\boldsymbol xx的一阶导数雅可比（Jacobian）矩阵，H(xk)\boldsymbol H(\boldsymbol x_k)H(xk)是二阶导数**海塞（Hessian）**矩阵。

保留一阶梯度，称为一阶梯度法：
Δx∗=−J(xk)\Delta\boldsymbol x^*=-\boldsymbol J(\boldsymbol x_k) Δx∗=−J(xk)
保留二阶梯度信息，即二阶梯度法，增量方程为：
Δx∗=arg⁡min⁡(F(x)+J(x)TΔx+12ΔxTHΔx)\Delta\boldsymbol x^*=\arg\min(F(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x+\frac{1}{2}\Delta\boldsymbol x^T\boldsymbol H\Delta\boldsymbol x) Δx∗=argmin(F(x)+J(x)TΔx+21ΔxTHΔx)
另右侧关于Δx\Delta\boldsymbol xΔx的导数为零，有：
HΔx=−J\boldsymbol H\Delta\boldsymbol x=-\boldsymbol J HΔx=−J
求此方程，得到增量。又称为牛顿法

高斯牛顿法

高斯牛顿法的思想是将f(x)f(\boldsymbol x)f(x)（不是F(x)F(\boldsymbol x)F(x)）进行一阶泰勒展开：
f(x+Δx)≈f(x)+J(x)TΔxf(\boldsymbol x+\Delta\boldsymbol x)\approx f(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x f(x+Δx)≈f(x)+J(x)TΔx
这里J(x)T\boldsymbol J(\boldsymbol x)^TJ(x)T是f(x)关于xf(\boldsymbol x)关于\boldsymbol xf(x)关于x的导数。目标是寻找增量Δx\Delta\boldsymbol xΔx使得∥f(x+Δx)∥2\Vert f(\boldsymbol x+\Delta\boldsymbol x)\Vert^2∥f(x+Δx)∥2最小，即如下最小二乘问题：
Δx∗=arg⁡min⁡Δx12∥f(x)+J(x)TΔx∥2\Delta\boldsymbol x^*=\arg\min_{\Delta\boldsymbol x}\frac{1}{2}\Vert f(\boldsymbol x)+\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x\Vert^2 Δx∗=argΔxmin21∥f(x)+J(x)TΔx∥2
求导并使其为零，最终得到如下方程组：
J(x)JT(x)⏟H(x)Δx=−J(x)f(x)⏟g(x)\underbrace{\boldsymbol J(\boldsymbol x)\boldsymbol J^T(\boldsymbol x)}_{\boldsymbol H(\boldsymbol x)}\Delta\boldsymbol x=\underbrace{-\boldsymbol J(\boldsymbol x)f(\boldsymbol x)}_{\boldsymbol g(\boldsymbol x)} H(x)J(x)JT(x)Δx=g(x)−J(x)f(x)
这个方程是关于变量Δx\Delta\boldsymbol xΔx的线性方程组，称为增量方程，也可以称为高斯牛顿方程（Gauss-Newton equation）或正规方程（Normal equation）。上式可变为：
HΔx=g\boldsymbol H\Delta\boldsymbol x=\boldsymbol g HΔx=g

高斯牛顿法用JJT\boldsymbol J\boldsymbol J^TJJT作为牛顿法中二阶Hessian矩阵的近似，从而省略了计算H\boldsymbol HH的过程。求解增量方程是整个优化问题的核心所在。

高斯牛顿法的算法步骤：

给定初始值x0\boldsymbol x_0x0
对于第kkk次迭代，求出当前的雅可比矩阵J(xk)\boldsymbol J(\boldsymbol x_k)J(xk)和误差f(xk)f(\boldsymbol x_k)f(xk)
求解增量方程：HΔxk=g\boldsymbol H\Delta\boldsymbol x_k=\boldsymbol gHΔxk=g
若Δxk\Delta\boldsymbol x_kΔxk足够小，则停止；否则，令xk+1=xk+Δxk\boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_kxk+1=xk+Δxk，返回第2步

列文伯格-马夸尔特方法

在高斯牛顿法中，可能出现JJT\boldsymbol J\boldsymbol J^TJJT为奇异矩阵或者病态的情况，此时增量的稳定性较差，导致算法不收敛，原函数在这个点的局部近似不像一个二次函数。

高斯牛顿法中的近似二阶泰勒展开只能在展开点附近有较好的近似效果。因此，想到给Δx\Delta\boldsymbol xΔx添加一个范围，称为信赖区域（Trust Region），这个范围定义了在什么情况下二阶近似是有效的，这类方法也称为信赖区域方法。

根据近似模型跟实际模型之间的差异来确定信赖区域的范围：如果差异小，近似效果好，扩大近似的范围；反之缩小近似范围。

定义指标来刻画近似的好坏程度：
ρ=f(x+Δx)−f(x)J(x)TΔx\rho=\frac{f(\boldsymbol x+\Delta\boldsymbol x)-f(\boldsymbol x)}{\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x} ρ=J(x)TΔxf(x+Δx)−f(x)
分子是实际函数下降值，分子是模型下降值。如果接近于1，则近似效果好。

改良版非线性优化框架：

给定初始值x0\boldsymbol x_0x0，以及初始优化半径μ\muμ
对于第kkk次迭代，在高斯牛顿法的基础上加上信赖区域，求解：
min⁡Δxk12∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ\min_{\Delta\boldsymbol x_k}\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2,\quad\text{s.t.}\quad\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2\leq\mu Δxkmin21∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ

其中，μ\muμ是信赖区域的半径，D\boldsymbol DD是系数矩阵。列文伯格把D\boldsymbol DD取成单位阵I\boldsymbol II，相当于直接把Δxk\Delta\boldsymbol x_kΔxk约束在一个球中；马夸尔特将D\boldsymbol DD取成非负数对角阵，使得梯度小的维度上约束范围更大一些。
按下式计算ρ\rhoρ：
ρ=f(x+Δx)−f(x)J(x)TΔx\rho=\frac{f(\boldsymbol x+\Delta\boldsymbol x)-f(\boldsymbol x)}{\boldsymbol J(\boldsymbol x)^T\Delta\boldsymbol x} ρ=J(x)TΔxf(x+Δx)−f(x)
若ρ>34\rho>\frac{3}{4}ρ>43，则设置μ=2μ\mu=2\muμ=2μ
若ρ<14\rho<\frac{1}{4}ρ<41，则设置μ=0.5μ\mu=0.5\muμ=0.5μ
若ρ\rhoρ大于某阈值，则认为近似可行，令xk+1=xk+Δxk\boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_kxk+1=xk+Δxk
若Δxk\Delta\boldsymbol x_kΔxk足够小，则停止；否则，令xk+1=xk+Δxk\boldsymbol x_{k+1}=\boldsymbol x_k+\Delta\boldsymbol x_kxk+1=xk+Δxk，返回第2步

在列文伯格-马夸尔特优化中，需要求解如下带不等式约束的优化子问题：
min⁡Δxk12∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ\min_{\Delta\boldsymbol x_k}\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2,\quad\text{s.t.}\quad\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2\leq\mu Δxkmin21∥f(xk)+J(xk)TΔxk∥2,s.t.∥DΔxk∥2≤μ
用拉格朗日法求解，构成拉格朗日函数：
L(Δxk,λ)=12∥f(xk)+J(xk)TΔxk∥2+λ2(∥DΔxk∥2−μ)\mathcal{L}(\Delta\boldsymbol x_k,\lambda)=\frac{1}{2}\Vert f(\boldsymbol x_k)+\boldsymbol J(\boldsymbol x_k)^T\Delta\boldsymbol x_k\Vert^2+\frac{\lambda}{2}(\Vert\boldsymbol D\Delta\boldsymbol x_k\Vert^2-\mu) L(Δxk,λ)=21∥f(xk)+J(xk)TΔxk∥2+2λ(∥DΔxk∥2−μ)
这里λ\lambdaλ为拉格朗日乘子。令拉格朗日函数关于Δx\Delta\boldsymbol xΔx的导数为零，其核心仍然是计算增量的线性方程：
(H+λDTD)Δxk=g(\boldsymbol H+\lambda\boldsymbol D^T\boldsymbol D)\Delta\boldsymbol x_k=\boldsymbol g (H+λDTD)Δxk=g
考虑其简化形式（D=I\boldsymbol D=\boldsymbol ID=I），则相当于求解：
(H+λI)Δxk=g(\boldsymbol H+\lambda\boldsymbol I)\Delta\boldsymbol x_k=\boldsymbol g (H+λI)Δxk=g
当λ\lambdaλ较小时，列文伯格-马夸尔特方法更接近于高斯牛顿法；λ\lambdaλ较大时，更接近于一阶梯度下降法（最速下降）

[SLAM基础学习简记]非线性优化相关推荐

ROS系统SLAM基础学习：gazebo仿真机器人自主导航
ROS系统SLAM基础学习:gazebo仿真机器人自主导航 move_base节点配置 amcl节点配置导航仿真导航SLAM仿真自主探索SLAM仿真自主导航:避障遇到的问题及解决方法和总结 ...
超硬核直播课 | 自主旋翼无人机主流算法、视觉SLAM基础
视觉SLAM是通过两帧或多帧图像来估计自身的位姿变化,再通过累积位姿变化计算当前位置.视觉SLAM不仅解决了移动机器人"我在哪里"的关键问题,而且在传感器自身定位.路径规划及场景理 ...
十.激光SLAM框架学习之LeGO-LOAM框架---算法原理和改进、项目工程代码
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
六.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---4.laserMapping.cpp--后端建图和帧位姿精估计（优化）
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
五.激光SLAM框架学习之A-LOAM框架---项目工程代码介绍---3.laserOdometry.cpp--前端雷达里程计和位姿粗估计
专栏系列文章如下: 一:Tixiao Shan最新力作LVI-SAM(Lio-SAM+Vins-Mono),基于视觉-激光-惯导里程计的SLAM框架,环境搭建和跑通过程_goldqiu的博客-CSDN ...
达梦DM数据库—线上培训学习简记
达梦DM数据库-线上培训学习简记 (20200812) 有幸参加国内领先的达梦数据库(自主研发)的线上学习,达梦的老师都非常的NICE, 遇到问题会非常耐心的线上解答.现将这次的学习中的一些知识点做了 ...
SLAM基础 —— 视觉与IMU融合（VIO基础理论）
SLAM基础 -- 视觉与IMU融合(VIO基础理论) 1. 基于Bundle Adjustment的VIO融合 1.1 为什么需要融合 1.2 视觉与IMU融合两类方案: 1.3 视觉SLAM中的B ...
单目slam基础特点匹配光流匹配　单应变换恢复变换矩阵　本质矩阵恢复变换矩阵　深度滤波
非滤波单目slam基础非滤波方法的单目视觉SLAM系统综述论文直接法特征点法混合法区别与联系按照 2D−2D 数据关联方式的不同 ,视觉定位方法可以分为直接法.非直接法和混合法1. 直接法 ...
激光SLAM入门学习笔记
激光SLAM入门学习笔记激光SLAM入门学习笔记一.推荐阅读书籍二.推荐公众号.知乎.博客 1.公众号 2.知乎 3.博客三.推荐阅读论文&代码(参考泡泡机器人) 2D激光SLAM 3 ...
【SLAM基础入门】贝叶斯滤波、卡尔曼滤波、粒子滤波笔记（1）
贝叶斯滤波.卡尔曼滤波.粒子滤波 (https://www.bilibili.com/video/BV1HT4y1577g?spm_id_from=333.999.header_right.histo ...

[SLAM基础学习简记]非线性优化