室内设计3d模型素材网站,带前台的WordPress模板,dw怎样去除网站做的页面模板,品牌型网店最小二乘问题和非线性优化 0.引言1.最小二乘问题2.迭代下降法3.最速下降法4.牛顿法5.阻尼法6.高斯牛顿(GN)法7.莱文贝格马夸特(LM)法8.鲁棒核函数 0.引言
转载自此处#xff0c;修正了一点小错误。
1.最小二乘问题
在求解 SLAM 中的最优状态估计问题时#xff0c;我们一般… 最小二乘问题和非线性优化 0.引言1.最小二乘问题2.迭代下降法3.最速下降法4.牛顿法5.阻尼法6.高斯牛顿(GN)法7.莱文贝格马夸特(LM)法8.鲁棒核函数 0.引言
转载自此处修正了一点小错误。
1.最小二乘问题
在求解 SLAM 中的最优状态估计问题时我们一般会得到两个变量一个是由传感器获得的实际观测值 z \boldsymbol{z} z一个是根据目前估计的状态量和观测模型计算出来的预测值 h ( x ) h(\boldsymbol{x}) h(x)。求解最优状态估计问题时通常我们会尝试最小化预测值和观测值计算出的残差平方使用平方是为了统一正负号的影响即求解以下最小二乘问题 x ∗ arg min x ∣ ∣ z − h ( x ) ∣ ∣ 2 \boldsymbol{x}^* \arg\min_{\boldsymbol{x}}||\boldsymbol{z} - h(\boldsymbol{x})||^2 x∗argxmin∣∣z−h(x)∣∣2 如果观测模型是线性模型则上式转化为线性最小二乘问题 x ∗ arg min x ∣ ∣ z − H x ∣ ∣ 2 \boldsymbol{x}^* \arg\min_{\boldsymbol{x}}||\boldsymbol{z} - \boldsymbol{H}\boldsymbol{x}||^2 x∗argxmin∣∣z−Hx∣∣2 对于线性最小二乘问题我们可以直接求闭式解 x ∗ − ( H T H ) − 1 H T z \boldsymbol{x}^* -(\boldsymbol{H}^T\boldsymbol{H})^{-1}\boldsymbol{H}^T\boldsymbol{z} x∗−(HTH)−1HTz 这里不进行赘述。
实际的问题中我们通常要最小化不止一个残差不同残差通常会按其重要性不确定性分配一个相应的权重系数且观测模型也通常是非线性的即求解以下问题 e i ( x ) z i − h i ( x ) i 1 , 2 , . . . , n ∣ ∣ e i ( x ) ∣ ∣ Σ i 2 e i T Σ i e i x ∗ arg min x F ( x ) arg min x ∑ i ∣ ∣ e i ( x ) ∣ ∣ Σ i 2 \begin{aligned} \boldsymbol{e}_i(\boldsymbol{x}) \boldsymbol{z}_i - h_i(\boldsymbol{x}) \qquad i 1, 2, ..., n\\ ||\boldsymbol{e}_i(\boldsymbol{x})||^2_{\Sigma_i} \boldsymbol{e}_i^T\boldsymbol{\Sigma}_i\boldsymbol{e}_i\\ \boldsymbol{x}^* \arg\min_{\boldsymbol{x}}F(\boldsymbol{x})\\ \arg\min_{\boldsymbol{x}}\sum_i||\boldsymbol{e}_i(\boldsymbol{x})||^2_{\Sigma_i} \end{aligned} ei(x)∣∣ei(x)∣∣Σi2x∗zi−hi(x)i1,2,...,neiTΣieiargxminF(x)argxmini∑∣∣ei(x)∣∣Σi2 我们想要获得一个状态量 x ∗ \boldsymbol{x}^* x∗使得损失函数 F ( x ) F(\boldsymbol{x}) F(x) 取得局部最小值。
在具体求解之前先考虑 F ( x ) F(\boldsymbol{x}) F(x) 的性质对其进行二阶泰勒展开 F ( x Δ x ) F ( x ) J Δ x 1 2 Δ x T H Δ x O ( ∣ ∣ Δ x ∣ ∣ 3 ) F(\boldsymbol{x} \Delta\boldsymbol{x}) F(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x} O(||\Delta\boldsymbol{x}||^3) F(xΔx)F(x)JΔx21ΔxTHΔxO(∣∣Δx∣∣3) 忽略高阶余项对二次函数有以下性质
如果在点 x ∗ \boldsymbol{x}^* x∗ 处导数为 0 \boldsymbol{0} 0则这个点为稳定点根据 Hessian 矩阵的正定性有不同性质
如果 H \boldsymbol{H} H 为正定矩阵则 F ( x ∗ ) F(\boldsymbol{x}^*) F(x∗) 为局部最小值如果 H \boldsymbol{H} H 为负定矩阵则 F ( x ∗ ) F(\boldsymbol{x}^*) F(x∗) 为局部最大值如果 H \boldsymbol{H} H 为不定矩阵则 F ( x ∗ ) F(\boldsymbol{x}^*) F(x∗) 为鞍点
在实际过程中一般 F ( x ) F(x) F(x) 比较复杂我们没有办法直接使其导数为 0 进而求出该点因此常用的是迭代法即找到一个下降方向使损失函数随 x \boldsymbol{x} x 的迭代逐渐减少直到 x \boldsymbol{x} x 收敛到 x ∗ \boldsymbol{x}^* x∗。下面整理以下常用的几种迭代方法。
2.迭代下降法
上面提到我们需要找到一个 x \boldsymbol{x} x 的迭代量使得 F ( x ) F(\boldsymbol{x}) F(x) 减少。这个过程分两步
找到 F ( x ) \boldsymbol{F(x)} F(x) 的下降方向构建该方向的单位向量 d \boldsymbol{d} d 确定该方向的迭代步长 α \alpha α 迭代后的自变量 x α d \boldsymbol{x} \alpha\boldsymbol{d} xαd 对应的函数值可以用一阶泰勒展开近似当步长足够小的时候 F ( x α d ) F ( x ) α J d F(\boldsymbol{x} \alpha\boldsymbol{d}) F(\boldsymbol{x}) \alpha\boldsymbol{Jd} F(xαd)F(x)αJd 因此不难发现要保持 F ( x ) F(x) F(x) 是下降的只需要保证 J d 0 \boldsymbol{Jd} 0 Jd0。以下几种方法都是以不同思路在寻找一个合适的方向进行迭代。
3.最速下降法
基于上一部分我们知道变化量为 α J d \alpha\boldsymbol{Jd} αJd其中 J d ∣ ∣ J ∣ ∣ cos θ \boldsymbol{Jd} ||\boldsymbol{J}||\cos{\theta} Jd∣∣J∣∣cosθ θ \theta θ 为梯度 J \boldsymbol{J} J 和 d \boldsymbol{d} d 的夹角。当 θ − π \theta -\pi θ−π 时 J d − ∣ ∣ J ∣ ∣ \boldsymbol{Jd} -||\boldsymbol{J}|| Jd−∣∣J∣∣ 取得最小值此时方向向量为 d − J T ∣ ∣ J ∣ ∣ \boldsymbol{d} \frac{-\boldsymbol{J}^T}{||\boldsymbol{J}||} d∣∣J∣∣−JT 因此沿梯度 J \boldsymbol{J} J 的负方向可以使 F ( x ) F(\boldsymbol{x}) F(x)但实际过程中一般只会在迭代刚开始使用这种方法当接近最优值时这种方法会出现震荡并且收敛较慢。
4.牛顿法
对 F ( x ) F(\boldsymbol{x}) F(x) 进行二阶泰勒展开有 F ( x Δ x ) F ( x ) J Δ x 1 2 Δ x T H Δ x F(\boldsymbol{x} \Delta\boldsymbol{x}) F(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x} F(xΔx)F(x)JΔx21ΔxTHΔx 我们关注的是求一个 Δ x \Delta\boldsymbol{x} Δx 使得 J Δ x 1 2 Δ x T H Δ x \boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x} JΔx21ΔxTHΔx 最小因此可以求导得 ∂ ( J Δ x 1 2 Δ x T H Δ x ) ∂ Δ x J T H Δ x 0 ⇒ Δ x − H − 1 J T \begin{aligned} \frac{\partial(\boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x})}{\partial\Delta\boldsymbol{x}} \boldsymbol{J}^T \boldsymbol{H}\Delta\boldsymbol{x} 0\\ \Rightarrow \Delta\boldsymbol{x} -\boldsymbol{H}^{-1}\boldsymbol{J}^T \end{aligned} ∂Δx∂(JΔx21ΔxTHΔx)⇒ΔxJTHΔx0−H−1JT 当 H \boldsymbol{H} H 为正定矩阵且当前 x \boldsymbol{x} x 在最优点附近时取 Δ x − H − 1 J T \Delta\boldsymbol{x} -\boldsymbol{H}^{-1}\boldsymbol{J}^T Δx−H−1JT 可以使函数获得局部最小值。但缺点是残差的 Hessian 函数通常比较难求。
5.阻尼法
在牛顿法的基础上为了控制每次迭代不要太激进我们可以在损失函数中增加一项惩罚项如下所示 arg min Δ x { F ( x ) J Δ x 1 2 Δ x T H Δ x 1 2 μ ( Δ x ) T ( Δ x ) } \arg\min_{\Delta\boldsymbol{x}}\left\{F(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{H}\Delta\boldsymbol{x} \frac{1}{2}\mu(\Delta\boldsymbol{x})^T(\Delta\boldsymbol{x})\right\} argΔxmin{F(x)JΔx21ΔxTHΔx21μ(Δx)T(Δx)} 当我们选的 Δ x \Delta\boldsymbol{x} Δx 太大时损失函数也会变大变大的幅度由 μ \mu μ 决定因此我们可以控制每次迭代量 Δ x \Delta\boldsymbol{x} Δx 的大小。同样在右侧部分对 Δ x \Delta\boldsymbol{x} Δx 求导有 J T H Δ x μ Δ x 0 ( H μ I ) Δ x − J T \begin{aligned} \boldsymbol{J}^T \boldsymbol{H}\Delta\boldsymbol{x} \mu\Delta\boldsymbol{x} 0\\ (\boldsymbol{H} \mu\boldsymbol{I})\Delta\boldsymbol{x} -\boldsymbol{J}^T \end{aligned} JTHΔxμΔx(HμI)Δx−JT0 这个思路我们后面在介绍 LM 法时也会用到。
6.高斯牛顿(GN)法
在前面的整理中实际上我们求解的是一系列残差的和求单个残差的雅可比比较简单因此在后续的几种方法中我们关注各个残差的变化。将上述非线性最小二乘问题中的各个残差写成向量形式 F ( x ) E ( x ) [ e 1 ( x ) e 2 ( x ) . . . e n ( x ) ] \boldsymbol{F}(\boldsymbol{x}) \boldsymbol{E}(\boldsymbol{x}) \begin{bmatrix} \boldsymbol{e}_1(\boldsymbol{x})\\ \boldsymbol{e}_2(\boldsymbol{x})\\ ...\\ \boldsymbol{e}_n(\boldsymbol{x})\\ \end{bmatrix} F(x)E(x) e1(x)e2(x)...en(x)
对 e ( x ) \boldsymbol{e}(\boldsymbol{x}) e(x) 进行泰勒展开有 e ( x Δ x ) e ( x ) J Δ x \boldsymbol{e}(\boldsymbol{x} \Delta\boldsymbol{x}) \boldsymbol{e}(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x} e(xΔx)e(x)JΔx 上式中 J \boldsymbol{J} J 是残差矩阵 e ( x ) \boldsymbol{e}(\boldsymbol{x}) e(x) 对状态量的雅可比矩阵。
注意到在原来的线性最小二乘问题中对每个残差还有一个权重矩阵 Σ \boldsymbol{\Sigma} Σ。这种情况下我们只需要令 e i ( x ) Σ i e i ( x ) \boldsymbol{e}_i(\boldsymbol{x}) \sqrt{\boldsymbol{\Sigma}_i}\boldsymbol{e}_i(\boldsymbol{x}) ei(x)Σi ei(x) 即可。因此下式中暂不考虑 Σ \boldsymbol{\Sigma} Σ 的影响。
在这种形式下对 e ( x ) \boldsymbol{e}(\boldsymbol{x}) e(x) 进行泰勒展开有 ∣ ∣ e ( x Δ x ) ∣ ∣ 2 e ( x Δ x ) T e ( x Δ x ) ( e ( x ) J Δ x ) T ( e ( x ) J Δ x ) e ( x ) T e ( x ) Δ x T J T e ( x ) e ( x ) T J Δ x Δ x T J T J Δ x \begin{aligned} ||e(\boldsymbol{x} \Delta\boldsymbol{x}) ||^2 \boldsymbol{e}(\boldsymbol{x} \Delta\boldsymbol{x})^T\boldsymbol{e}(\boldsymbol{x} \Delta\boldsymbol{x})\\ (\boldsymbol{e}(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x})^T(\boldsymbol{e}(\boldsymbol{x}) \boldsymbol{J}\Delta\boldsymbol{x})\\ \boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{e}(\boldsymbol{x}) \Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{e}(\boldsymbol{x}) \boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{J}\Delta\boldsymbol{x} \Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} \end{aligned} ∣∣e(xΔx)∣∣2e(xΔx)Te(xΔx)(e(x)JΔx)T(e(x)JΔx)e(x)Te(x)ΔxTJTe(x)e(x)TJΔxΔxTJTJΔx 上式中注意到 e ( x ) e(\boldsymbol{x}) e(x) 是一维的因此 Δ x T J T e ( x ) e ( x ) T J Δ x \Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{e}(\boldsymbol{x}) \boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{J}\Delta\boldsymbol{x} ΔxTJTe(x)e(x)TJΔx因此化简得 F ( x Δ x ) e ( x ) T e ( x ) 2 e ( x ) T J Δ x Δ x T J T J Δ x F ( x ) 2 e ( x ) T J Δ x Δ x T J T J Δ x \begin{aligned} F(\boldsymbol{x} \Delta\boldsymbol{x}) \boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{e}(\boldsymbol{x}) 2\boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{J}\Delta\boldsymbol{x} \Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x}\\ F(\boldsymbol{x}) 2\boldsymbol{e}(\boldsymbol{x})^T\boldsymbol{J}\Delta\boldsymbol{x} \Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} \end{aligned} F(xΔx)e(x)Te(x)2e(x)TJΔxΔxTJTJΔxF(x)2e(x)TJΔxΔxTJTJΔx 通过这种方式我们同样将其近似一个二次函数并且和我们之前展开的结果比较不难发现在这里我们实际上是用 J T e \boldsymbol{J}^T\boldsymbol{e} JTe 来近似 Jacobian 矩阵用 J T J \boldsymbol{J}^T\boldsymbol{J} JTJ 来近似 Hessian 矩阵。因此当 J \boldsymbol{J} J 满秩时我们可以保证在上式导数为 0 的地方可以确保函数取得局部最小值。同样在上式右侧部分对 Δ x \Delta\boldsymbol{x} Δx 求导并使其为 0 有 J T e ( x ) J T J Δ x 0 ⇒ J T J Δ x − J T e ( x ) ⇒ H Δ x b \begin{aligned} \boldsymbol{J}^T\boldsymbol{e}(\boldsymbol{x}) \boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} 0\\ \Rightarrow \boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} -\boldsymbol{J}^T\boldsymbol{e}(\boldsymbol{x})\\ \Rightarrow \boldsymbol{H}\Delta\boldsymbol{x} \boldsymbol{b} \end{aligned} JTe(x)JTJΔx0⇒JTJΔx⇒HΔx−JTe(x)b 上式中我们令 H J T J , b − J T e \boldsymbol{H} \boldsymbol{J}^T\boldsymbol{J}, \boldsymbol{b} -\boldsymbol{J}^T\boldsymbol{e} HJTJ,b−JTe。这样我们获得了高斯牛顿法的求解过程
计算残差矩阵关于状态值的雅可比矩阵 J \boldsymbol{J} J利用雅可比矩阵和残差构建信息矩阵和信息向量 H , b \boldsymbol{H}, \boldsymbol{b} H,b计算当次迭代量 Δ x H − 1 b \Delta\boldsymbol{x} \boldsymbol{H}^{-1}\boldsymbol{b} ΔxH−1b如果迭代量足够小则结束迭代否则进入下一次迭代
7.莱文贝格马夸特(LM)法
LM 法是在高斯牛顿法的基础上按照阻尼法的思路加入阻尼因子即求解以下方程 ( H μ I ) Δ x b (\boldsymbol{H} \mu\boldsymbol{I})\Delta\boldsymbol{x} \boldsymbol{b} (HμI)Δxb 上式中阻尼因子的作用有 添加进 H \boldsymbol{H} H 保证 H \boldsymbol{H} H 是正定的 当 μ \mu μ 很大时 Δ x − ( H μ I ) − 1 b ≈ − 1 μ b − 1 μ J T E ( x ) \Delta\boldsymbol{x} -(\boldsymbol{H}\mu\boldsymbol{I})^{-1}\boldsymbol{b}\approx-\frac{1}{\mu}\boldsymbol{b}-\frac{1}{\mu}\boldsymbol{J}^T\boldsymbol{E}(\boldsymbol{x}) Δx−(HμI)−1b≈−μ1b−μ1JTE(x)接近最速下降法 当 μ \mu μ 很小时则接近高斯牛顿法 因此合理的设置阻尼因子能够达到动态对迭代速度进行调节。阻尼因子的设置分为两部分 初始值的选取 随迭代量变化的更新策略
先来看初始值选取方法阻尼因子的大小应该根据 J T J \boldsymbol{J}^T\boldsymbol{J} JTJ 的大小来选取对 J T J \boldsymbol{J}^T\boldsymbol{J} JTJ 进行特征值分解有 J T J V Λ V T \boldsymbol{J}^T\boldsymbol{J} \boldsymbol{V}\boldsymbol{\Lambda}\boldsymbol{V}^T JTJVΛVT Λ diag ( λ 1 , λ 2 , . . . , λ n ) , V [ v 1 , . . . , v n ] \boldsymbol{\Lambda} \text{diag}(\lambda_1, \lambda_2,..., \lambda_n), \boldsymbol{V} [\boldsymbol{v}_1, ..., \boldsymbol{v}_n] Λdiag(λ1,λ2,...,λn),V[v1,...,vn]因此迭代公式化简为 ( V Λ V T μ I ) Δ x b Δ x ( V Λ V T μ I ) − 1 b − ∑ i v i T b λ i μ v i \begin{aligned} (\boldsymbol{V\Lambda}\boldsymbol{V}^T \mu\boldsymbol{I})\Delta\boldsymbol{x} \boldsymbol{b}\\ \Delta\boldsymbol{x} (\boldsymbol{V\Lambda}\boldsymbol{V}^T \mu\boldsymbol{I})^{-1}\boldsymbol{b}\\ -\sum_i\frac{\boldsymbol{v}_i^T\boldsymbol{b}}{\lambda_i \mu}\boldsymbol{v}_i \end{aligned} (VΛVTμI)ΔxΔxb(VΛVTμI)−1b−i∑λiμviTbvi 因此将 μ \mu μ 选为 λ i \lambda_i λi 接近即可一个简单的思路是设 μ 0 τ max ( J T J ) i i \mu_0 \tau\max{(\boldsymbol{J}^T\boldsymbol{J})_{ii}} μ0τmax(JTJ)ii实际中一般设 τ ≈ [ 1 0 − 8 , 1 ] \tau \approx [10^{-8}, 1] τ≈[10−8,1]
接下来看 μ \mu μ 的更新策略先定性分析应该怎么更新阻尼因子
当 Δ x \Delta\boldsymbol{x} Δx 令 F ( x ) F(\boldsymbol{x}) F(x) 增加时应该提高 μ \mu μ 来降低 Δ x \Delta\boldsymbol{x} Δx 即通过减少步长来降低本次迭代带来的影响当 Δ x \Delta\boldsymbol{x} Δx 令 F ( x ) F(\boldsymbol{x}) F(x) 减少时应该降低 μ \mu μ 来提高 Δ x \Delta\boldsymbol{x} Δx 即通过增加步长来提高这次迭代的影响
下面进行定量分析令 L ( Δ x ) F ( x ) 2 E ( x ) T J Δ x 1 2 Δ x T J T J Δ x L(\Delta\boldsymbol{x}) F(\boldsymbol{x}) 2\boldsymbol{E}(\boldsymbol{x})^T\boldsymbol{J}\Delta\boldsymbol{x} \frac{1}{2}\Delta\boldsymbol{x}^T\boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} L(Δx)F(x)2E(x)TJΔx21ΔxTJTJΔx考虑以下比例因子 ρ F ( x ) − F ( x Δ x ) L ( 0 ) − F ( Δ x ) \rho \frac{F(\boldsymbol{x}) - F(\boldsymbol{x} \Delta\boldsymbol{x})}{L(\boldsymbol{0}) - F(\Delta\boldsymbol{x})} ρL(0)−F(Δx)F(x)−F(xΔx) Marquardt 提出一个策略
当 ρ 0 \rho 0 ρ0表示当前 Δ x \Delta\boldsymbol{x} Δx 使 F ( x ) F(\boldsymbol{x}) F(x) 增大表示离最优值还很远应该提高 μ \mu μ 使其接近最速下降法进行较大幅度的更新当 ρ 0 \rho 0 ρ0 且比较大表示当前 Δ x \Delta\boldsymbol{x} Δx 使 F ( x ) F(\boldsymbol{x}) F(x) 减小应该降低 μ \mu μ 使其接近高斯牛顿法降低速度更新至最优点如果 ρ 0 \rho 0 ρ0 但比较小表示已经到最优点附近则增大阻尼 μ \mu μ缩小迭代步长
Marquardt 的具体策略如下
if rho 0.25: mu mu * 2
else if rho 0.75: mu mu /3一个使用 Marquardt 策略进行更新的过程如下
可以发现效果并不算很好随着迭代次数的增加 μ \mu μ 开始进行震荡表示迭代量周期性的使 F ( x ) F(\boldsymbol{x}) F(x) 增加又下降。
因此Nielsen 提出了另一个策略也是 G2O 和 Ceres 中使用的策略
if rho 0:mu mu * max(1/3, 1 - (2 * rho - 1)^3)v 2
else:mu mu * vv 2 * v使用这种策略进行优化的例子如下
可以看到 μ \mu μ 随着迭代的进行可以比较平滑的持续下降直至达到收敛。
8.鲁棒核函数
在进行最小二乘问题中我们会遇到一些异常观测值使得观测残差特别大如果不对这些异常点做处理会影响在优化过程中优化器会尝试最小化异常的残差项最后影响状态估计的精度鲁棒核函数就是用来降低这些异常观测值造成的影响。
将鲁棒核函数直接作用在每个残差项上将最小二乘问题变成如下形式 F ( x ) ∑ i ρ ( ∣ ∣ e i ( x ) ∣ ∣ 2 ) F(\boldsymbol{x}) \sum_i\rho(||e_i(\boldsymbol{x})||^2) F(x)i∑ρ(∣∣ei(x)∣∣2) 使用鲁棒核函数时求解非线性最小二乘的过程 在这个形式下对带有鲁棒核函数的残差进行二阶泰勒展开 ρ ( s Δ s ) ρ ( x ) ρ ′ ( x ) Δ s 1 2 ρ ′ ′ ( x ) Δ 2 s \rho(s \Delta s) \rho(x) \rho(x)\Delta s \frac{1}{2}\rho(x)\Delta^2s ρ(sΔs)ρ(x)ρ′(x)Δs21ρ′′(x)Δ2s 上式中变化量 Δ s \Delta s Δs 的计算方式如下 Δ s k ∣ ∣ e i ( x Δ x ) ∣ ∣ 2 − ∣ ∣ e i ( x ) ∣ ∣ 2 ∣ ∣ e i ( x ) J i Δ x ∣ ∣ 2 − ∣ ∣ e i ( x ) ∣ ∣ 2 2 e i ( x ) T J i Δ x Δ x T J i T J i Δ x \begin{aligned} \Delta s_k ||e_i(\boldsymbol{x}\Delta\boldsymbol{x})||^2 - ||e_i(\boldsymbol{x})||^2\\ ||e_i(\boldsymbol{x})\boldsymbol{J}_i\Delta\boldsymbol{x}||^2 - ||e_i(\boldsymbol{x})||^2\\ 2e_i(\boldsymbol{x})^T\boldsymbol{J}_i\Delta\boldsymbol{x}\Delta\boldsymbol{x}^T\boldsymbol{J}_i^T\boldsymbol{J}_i\Delta\boldsymbol{x} \end{aligned} Δsk∣∣ei(xΔx)∣∣2−∣∣ei(x)∣∣2∣∣ei(x)JiΔx∣∣2−∣∣ei(x)∣∣22ei(x)TJiΔxΔxTJiTJiΔx 将 Δ s \Delta s Δs 代入 ρ ( s Δ s ) \rho(s \Delta s) ρ(sΔs) 可得 ρ ( s Δ s ) ρ ( s ) ρ ′ ( s ) ( 2 e i ( x ) T J i Δ x Δ x T J i T J i Δ x ) 1 2 ρ ′ ′ ( s ) ( 2 e i ( x ) T J i Δ x Δ x T J i T J i Δ x ) 2 ≈ ρ ( s ) 2 ρ ′ ( s ) e i ( x ) T J i Δ x ρ ′ ( s ) Δ x T J i T J i Δ x 2 ρ ′ ′ ( s ) Δ x T J i T e i ( x ) e i ( x ) T J i Δ x \begin{aligned} \rho(s \Delta s) \rho(s) \rho(s)(2e_i(\boldsymbol{x})^T\boldsymbol{J}_i\Delta\boldsymbol{x}\Delta\boldsymbol{x}^T\boldsymbol{J}_i^T\boldsymbol{J}_i\Delta\boldsymbol{x}) \\ \frac{1}{2}\rho(s)(2e_i(\boldsymbol{x})^T\boldsymbol{J}_i\Delta\boldsymbol{x}\Delta\boldsymbol{x}^T\boldsymbol{J}_i^T\boldsymbol{J}_i\Delta\boldsymbol{x})^2\\ \approx \rho(s) 2\rho(s)e_i(\boldsymbol{x})^T\boldsymbol{J}_i\Delta\boldsymbol{x}\rho(s)\Delta\boldsymbol{x}^T\boldsymbol{J}_i^T\boldsymbol{J}_i\Delta\boldsymbol{x} \\ 2\rho(s)\Delta\boldsymbol{x}^T\boldsymbol{J}_i^Te_i(\boldsymbol{x})e_i(\boldsymbol{x})^T\boldsymbol{J}_i\Delta\boldsymbol{x} \end{aligned} ρ(sΔs)≈ρ(s)ρ′(s)(2ei(x)TJiΔxΔxTJiTJiΔx)21ρ′′(s)(2ei(x)TJiΔxΔxTJiTJiΔx)2ρ(s)2ρ′(s)ei(x)TJiΔxρ′(s)ΔxTJiTJiΔx2ρ′′(s)ΔxTJiTei(x)ei(x)TJiΔx 按照之前的思路对上式求导并使其为 0可得 ∑ i J i T ( ρ ′ ( s ) 2 ρ ′ ′ ( s ) e i ( x ) e i ( x ) T ) J Δ x − ∑ i ρ ′ ( s ) J i T e i ( x ) \sum_i\boldsymbol{J}_i^T(\rho(s) 2\rho(s)e_i(\boldsymbol{\boldsymbol{x}})e_i(\boldsymbol{x})^T)\boldsymbol{J}\Delta\boldsymbol{x} -\sum_i\rho(s)\boldsymbol{J}_i^Te_i(\boldsymbol{x}) i∑JiT(ρ′(s)2ρ′′(s)ei(x)ei(x)T)JΔx−i∑ρ′(s)JiTei(x) 对比之前的矩阵 J T J Δ x − J i T e ( x ) \boldsymbol{J}^T\boldsymbol{J}\Delta\boldsymbol{x} -\boldsymbol{J}_i^T\boldsymbol{e}(\boldsymbol{x}) JTJΔx−JiTe(x) 可得当我们使用了鲁棒核函数之后只需要将各项残差的核函数的一阶二阶导数值计算出再按照以上形式对信息矩阵和信息向量进行更新即可。
常用的鲁棒核函数 柯西鲁棒核函数 ρ ( s ) c 2 log ( 1 s c 2 ) ρ ′ ( s ) 1 1 s c 2 ρ ′ ′ ( s ) − 1 c 2 ( ρ ′ ( s ) ) 2 \begin{aligned} \rho(s) c^2\log{(1\frac{s}{c^2})}\\ \rho(s) \frac{1}{1\frac{s}{c^2}}\\ \rho(s) -\frac{1}{c^2}(\rho(s))^2 \end{aligned} ρ(s)ρ′(s)ρ′′(s)c2log(1c2s)1c2s1−c21(ρ′(s))2 其中 c c c 为控制参数。当残差是正态分布Huber c 选为 1.345Cauchy c 选为 2.3849。不同鲁棒核函数的效果如下图所示
