非线性最小二乘法原理

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

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

1. 解决的问题

(1) 给定一个系统，其状态方程由描述。
其中：

为该系统的状态向量—即需要估计的值

是一个非线性的映射函数

状态向量x，可以通过非线性函数 $f(x)$ 映射得到

z表示系统的观测值，可以通过传感器进行直接观测

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

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

2. 示意图

为机器人的位置

为传感器的观测模型(似然场模型或者重投影模型)

为传感器的观测值，激光数据或者图像特征点。

找到最优的，让预测和观测的误差最小。

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$ 使得最小，

$\min_x F(x)$

其中，

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

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

要解决的问题
目标函数：

$\min_x F(x)$

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

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

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

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

$e_i(x + \Delta{x}) = e_i(x) + J_i(x) \Delta{x}$

其中 $J$ 为映射函数对状态向量的导数，称之为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}})$

因此函数的可化解为：

$\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}$