本文最后更新于2020年4月23日,已超过 7 个月没更新!

非线性最小二乘(Non-linear Least Square)

1. 解决的问题

(1) 给定一个系统,其状态方程由f(x)=z描述。
其中:

  • x为该系统的状态向量—即需要估计的值
  • f(x)是一个非线性的映射函数
  • 状态向量x,可以通过非线性函数f(x)映射得到z
  • z表示系统的观测值,可以通过传感器进行直接观测

(2) 给定该系统的𝑛个混有噪声的观测值(z_1,...,z_n),估计状态向量x,使得其经过f(x)映射之后的预测值和观测值的误差最小;

(3) 跟线性最小二乘基本相同,不同之处在于f(x)是一个非线性函数。

2. 示意图

  • X为机器人的位置
  • f(x)为传感器的观测模型(似然场模型或者重投影模型)
  • Z为传感器的观测值,激光数据或者图像特征点。
  • 找到最优的x,让预测和观测的误差最小。

3. 误差函数

(1) 目标为最小化预测和观测的差,因此误差即为预测和观测的差:

    \[e_i(x) = f_i(x) - z_i\]

(2) 假设误差服从高斯分布,因此其对应的信息矩阵为\Omega_i,因此该观测值误差的平方定义为:

    \[E_i(x) = e_i(x)^T \Omega_i e_i(x)\]

(3) 非线性最小二乘的目标函数为:
找到一个x使得F(x)最小,

    \[\min_x F(x)\]

其中,

    \[F(x) = \sum E_i(x) = \sum (e_i(x)^T \Omega_i e_i(x))\]

由于e_i(x)是非线性的,因此导致F(x)也为非线性函数。

要解决的问题
目标函数:

    \[\min_x F(x)\]

直接想法:求F(x)关于变量x的导数,令其等于0,求解方程即可。

对于线性问题,该方法可以正确,但是对于非线性问题不正确。

F(x)为关于x的非线性方程,通过泰勒展开将其化为关于x的线性方程。

线性化
F(x)是关于x的非线性函数的原因是,误差函数函数e_i(x)是一个非线性函数。因此直接对误差函数e_i(x)进行线性化即可:

    \[e_i(x + \Delta{x}) = e_i(x) + J_i(x) \Delta{x}\]

其中J为映射函数对状态向量x的导数,称之为Jacobian矩阵。

    \[J_i(x) = (\frac{\partial{f_i(x)}}{\partial{x_1}} , \frac{\partial{f_i(x)}}{\partial{x_2}} , \cdot \cdot \cdot , \frac{\partial{f_i(x)}}{\partial{x_n}})\]

因此函数F(x)的可化解为:

    \[\begin{split} F(x+\Delta{x}) & = \sum (e_i^T (x+\Delta{x}) \Omega_i e_i(x+\Delta{x}))\\ & = \sum (e_i(x) + J_i \Delta{x})^T \Omega_i (e_i(x) + J_i \Delta{x}))\\ & = \sum (e_i^T \Omega_i e_i + e_i^T \Omega_i J_i \Delta{x} + \Delta{x}^T J_i^T \Omega_i e_i + \Delta{x}^T J_i^T \Omega_i J_i \Delta{x})\\ & = \sum (e_i^T \Omega_i e_i + 2 e_i^T \Omega_i J_i \Delta{x} + \Delta{x}^T J_i^T \Omega_i J_i \Delta{x}) \end{split}\]

其中,\Omega_i为一对称信息矩阵,也即:

    \[\Omega_i = \Omega_i^T\]

因此e_i^T \Omega_i J_i \Delta{x}\Delta{x}^T J_i^T \Omega_i e_i这两项且互为转置,且相等,是个数,可以进行合并。

之后,令:

    \[c_i = e_i^T \Omega_i e_i\]

    \[b_i^T = e_i^T \Omega_i J_i\]

    \[H_i = J_i^T \Omega_i J_i\]

可得:

    \[\begin{split} F(x+\Delta{x}) & = \sum (c_i + 2b_i^T \Delta{x} + \Delta{x}^T H_i \Delta{x})\\ & = \sum{c_i} + 2\sum{b_i^T \Delta{x}} + \Delta{x}^T \sum{H_i \Delta{x}}\\ & = c + 2b^T + \Delta{x}^T H \Delta{x} \end{split}\]

由此,我们可以看出F(x+\Delta{x})是个关于\Delta{x}的二次函数。其极值可直接通过其关于\Delta{x}的导数等于0求解得到。

求解
F(x+\Delta{x})\Delta{x}等于0可得:

    \[\frac{\partial{F(x+\Delta{x})}}{\partial\Delta{x}} = 2b + 2H\Delta{x} = 0\]

其中,由于H是个对称矩阵,因此:

    \[\frac{\partial{\Delta{x}^T H\Delta{x}}}{\partial\Delta{x}} = (H^T + H) \cdot \Delta{x} = 2H \Delta{x}\]

移项,可得:

    \[H\Delta{x} = -b\]

    \[\Delta{x}^* = -H^{-1}b\]

x=x+\Delta{x},然后不断迭代,直至收敛即可。

4. 总结

4.1 线性化误差函数

    \[e_i(x+\Delta{x}) = e_i(x) +J_i \Delta{x}\]

4.2 构建线性系统

    \[b^T = \sum{(e^T_i \Omega_i J_i)} \qquart H=\sum{J_i^T \Omega_i J_i}\]

4.3 求解线性系统

    \[\Delta{X}^* = -H^{-1} b\]

4.4 更新解,并不断迭代直至收敛

    \[x = x + \Delta{x}^*\]


Try and fail, but don't fail to try.