非线性最小二乘(Non-linear Least Square)
1. 解决的问题
(1) 给定一个系统,其状态方程由描述。
其中:
为该系统的状态向量—即需要估计的值
是一个非线性的映射函数
- 状态向量x,可以通过非线性函数
映射得到
- z表示系统的观测值,可以通过传感器进行直接观测
(2) 给定该系统的𝑛个混有噪声的观测值,估计状态向量
,使得其经过
映射之后的预测值和观测值的误差最小;
(3) 跟线性最小二乘基本相同,不同之处在于是一个非线性函数。
2. 示意图

为机器人的位置
为传感器的观测模型(似然场模型或者重投影模型)
为传感器的观测值,激光数据或者图像特征点。
- 找到最优的
,让预测和观测的误差最小。
3. 误差函数
(1) 目标为最小化预测和观测的差,因此误差即为预测和观测的差:
(2) 假设误差服从高斯分布,因此其对应的信息矩阵为,因此该观测值误差的平方定义为:
(3) 非线性最小二乘的目标函数为:
找到一个使得
最小,
其中,
由于是非线性的,因此导致
也为非线性函数。
要解决的问题
目标函数:
直接想法:求关于变量
的导数,令其等于0,求解方程即可。
对于线性问题,该方法可以正确,但是对于非线性问题不正确。
为关于
的非线性方程,通过泰勒展开将其化为关于
的线性方程。
线性化
是关于
的非线性函数的原因是,误差函数函数
是一个非线性函数。因此直接对误差函数
进行线性化即可:
其中为映射函数对状态向量
的导数,称之为Jacobian矩阵。
因此函数的可化解为:
其中,为一对称信息矩阵,也即:
因此和
这两项且互为转置,且相等,是个数,可以进行合并。
之后,令:
可得:
由此,我们可以看出是个关于
的二次函数。其极值可直接通过其关于
的导数等于0求解得到。
求解
将对
等于0可得:
其中,由于是个对称矩阵,因此:
移项,可得:
令,然后不断迭代,直至收敛即可。
4. 总结
4.1 线性化误差函数
4.2 构建线性系统
4.3 求解线性系统
4.4 更新解,并不断迭代直至收敛