Least Squares

Least Squares

LS

一般最小二乘：

• 计算超定系统的解的方法
• 方程多于未知数
• 方程中的平均误差之和最小
• 解决大型问题的标准方法
• 使用线性化近似值
• Gauss-Newton 是非线性问题的迭代方法

问题：

• 给定一个由一组n 个观察函数描述的系统 { f i ( x } = 1 : n {\{f_i(x\}=1:n} {fi​(x}=1:n
  • $x$ 状态向量（状态）
  • $z_i$ 是状态$x$的一个测量（实际测量）
  • $\widehat{z_i}=f_i(x)$是一个函数，$x$ 到一个预测$\widehat{z_i}$的映射，（预测）
• 给了$n$ 个噪声测量 $z_{1:n}$，关于状态$x$
• 目的：估计最优状态$x$通过测量 $z_{1:n}$

误差函数：

• 误差 $e_i(x)$ 是预测和实际测量的差异：$e_i(x)=z_i-f_i(x)$
• 我们假设误差是均值为0的正态分布
• 高斯误差的信息矩阵 ${\Omega }_i$，均方误差仅依赖状态和尺度：$e_i(x)=e_i(x{)}^T{\Omega }_i{e}_i(x)$

目的：

• 找一个状态 $x^{\ast }$ 使得所有的测量误差最小：$x^{\ast }=\underset{x}{\mathrm{argmin}}{\sum }_i{e}_i(x{)}^T{\Omega }_i{e}_i(x)$

假设：

• 一个“好的”初始预测是可用的
• 误差函数在（希望是全局坐标系下）最小值附近是“平滑的”
• 然后，我们可以通过迭代局部线性化来解决问题

通过迭代局部线性化求解

• 围绕当前解决解或初始值预测线性化误差项
• 计算平方误差函数的一阶导数
• 将其设置为零并求解线性系统
• 获得新状态（希望更接近最小值）
• 迭代

线性误差函数：

• 通过泰勒展开近似初始估计 $\hat{x}$ 的误差函数：$e_i(\hat{x}+\Delta x)\simeq \underset{e_i}{\underbrace{e_i(\hat{x})}}+J_i(\hat{x})\Delta x$

均方误差：

• 通过之前的线性化，我们可以固定 $x$ 并执行最小化。
• 我们在平方误差项中替换泰勒展开：
  • $e_i(\hat{x}+\Delta x)=e_i(\hat{x}+\Delta x{)}^T{\Omega }_i{e}_i(\hat{x}+\Delta x)\simeq (e_i+J_i\Delta x{)}^T{\Omega }_i(e_i+J_i\Delta x)$
  • $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$
• 所有的加数都是标量，因此转置无效：
  • $e_i(\hat{x}+\Delta x)=e_i^T{\Omega }_i{e}_i+2e_i^T{\Omega }_i{J}_i\Delta x+\Delta x^T{J}_i^T{\Omega }_i{J}_i\Delta x=c_i+2b_i^T\Delta x+\Delta x^T{H}_i\Delta x$
• 全局误差是对应于各个测量值的平方误差项的总和，形成一个新的表达式，它近似于当前解附近的全局误差
  • $F(\hat{x}+\Delta x)\simeq {\sum }_i(c_i)+2({\sum }_i{b}_i^T)\Delta x+\Delta x^T({\sum }_i{H}_i)\Delta x=c+2b^T\Delta x+\Delta x^{T}H\Delta x$
  • $b^T={\sum }_i{e}_i^T{\Omega }_i{J}_i$ 和 $H={\sum }_i{J}_i^T{\Omega }_i{J}_i$
• 我们需要迭代 $F(\hat{x}+\Delta x)$，变量 $\Delta x$
• 导出二次形式
  • 假设一个二次式：$f(x)=x^{T}Hx+b^{T}x$
  • 求偏导：
  • • $\frac{\partial f}{\partial x}=(H+H^T)x+b$
• 则其对变量的偏导有：
  • $\frac{\partial F(\hat{x}+\Delta x)}{\partial \Delta x}\simeq 2H\Delta x+2b$
• 另其偏导等于0，则 $0=2b+2H\Delta x$
• 因此解增量为 $\Delta x^{\ast }$ ：$\Delta x^{\ast }=-H^{-1}b$

Gauss-Newton

高斯牛顿：

• 线性化 $x$ 计算每个测量：$e_i(\hat{x}+\Delta x)\simeq \underset{e_i}{\underbrace{e_i(\hat{x})}}+J_i(\hat{x})\Delta x$

• 均方误差：

  • $e_i(\hat{x}+\Delta x)=e_i(\hat{x}+\Delta x{)}^T{\Omega }_i{e}_i(\hat{x}+\Delta x)\simeq (e_i+J_i\Delta x{)}^T{\Omega }_i(e_i+J_i\Delta x)$
  • $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$

• 为线性系统计算每一项：

  • $b^T={\sum }_i{e}_i^T{\Omega }_i{J}_i$
  • $H={\sum }_i{J}_i^T{\Omega }_i{J}_i$

• 解线性系统： $\Delta x^{\ast }=-H^{-1}b$

高斯牛顿总结：

• 目的：使得均方误差最小
• 开始初始化高斯噪声
• 求偏导线性化误差
• 这导致二次形式
• 通过将其导数设置为零来获得线性系统
• 求解线性系统导致状态更新
• 迭代

Eg Odo Calib

• 这儿有一个函数 $f_i(x)$，给一些偏差参数 $x$ ，返回一个无偏（校准）的里程计 $u_i^{\prime }$，如下：

  • $u_i^{\prime }=f_i(x)=\left(\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right)u_i$

• 为了得到修正函数 $f(x)$，我们需要找到参数 $x$。

• 设其状态向量为：$x={\left(\begin{array}{ccccccccc}{x}_{11}& x_{12}& x_{13}& x_{14}& x_{15}& x_{16}& x_{17}& x_{18}& x_{19}\end{array}\right)}^T$

• 误差函数为：$e_i(x)=u_i^{\ast }-\left(\begin{array}{ccc}{x}_{11}& x_{12}& x_{13}\\ x_{21}& x_{22}& x_{23}\\ x_{31}& x_{32}& x_{33}\end{array}\right)u_i$

• 其偏导为：$J_i=\frac{\partial e_i(x)}{\partial x}=-\left(\begin{array}{ccccccccc}{u}_{i,x}& u_{i,y}& u_{i,\theta }& & & & & & \\ & & & u_{i,x}& u_{i,y}& u_{i,\theta }& & & \\ & & & & & & u_{i,x}& u_{i,y}& u_{i,\theta }\end{array}\right)$

• 线性系统： $H\Delta x=-b$

• 为啥求导是那个呢？

  • $\left[\begin{array}{ccccccccc}{u}_{i,x}& u_{i,y}& u_{i,\theta }& & & & & & \\ & & & u_{i,x}& u_{i,y}& u_{i,\theta }& & & \\ & & & & & & u_{i,x}& u_{i,y}& u_{i,\theta }\end{array}\right]\ast \left[\begin{array}{c}{x}_{11}\\ ⋮\\ x_{33}\end{array}\right]=\left[\begin{array}{c}{x}_{ix}^{\ast }\\ x_{iy}^{\ast }\\ x_{i\theta }^{\ast }\end{array}\right]$
  • $A_i\vec{X}=b_i$ 可直接求出：$\vec{X}=(A^{t}A{)}^{-1}{A}^{T}b$

求解线性系统的 Cholesky 分解

• A是对称正定矩阵
• 求解：$Ax=b$
• Cholesky 导致 $A=L{L}^T$，其中 $L$ 是一个下三角矩阵
  • 第一步解：$Ly=b$
  • 然后：$L^{T}x=y$