最小二乘优化问题

这里给大家介绍一下最小二乘优化问题，然后举例里程计校准odometry calibration加深印象。

问题描述

  假设现有系统由$n$个观测函数描述 $\{f_i(x){\}}_{i=1..n}$，其中$x$是状态向量，$z_i$是状态$x$的测量，函数$z_i^{\prime }=f_i(x)$是预测量，它将状态变量$x$映射到预测测量$z_i^{\prime }$。
  此外，我们已知关于状态$x$的每个测量$z_{1:n}$都是有噪声的。
  我们现在要求状态$x$的最佳估计，使得观测函数能最好的表达测量$z_{1:n}$
  通俗解释，$x$就是要求的参数，$z$就是已知的测量值，我们希望通过最小二乘法使得观测函数能够最好的拟合测量值。状态量和观测量可以是多维变量，状态变量之间不独立。
  比如$x$可以是空间中3d特征的位置，$z_i$是这些3d特征在2d图像平面上投影的坐标，通过最小二乘法，给定图像上的2d坐标，最优估计空间中3d特征的位置。【其实这就是Direct Linear Transform算法，这里不扩展】

问题求解

  定义$e_i$为预测量与实际观测量(不是ground truth)的差分$$e_i(x)=z_i-f_i(x)$$  我们假设差分函数满足均值为0的正态高斯分布，${\Omega }_i$是gaussian error的信息矩阵，这里的公式中${\Omega }_i$仅对应一个约束。这样观测函数的平方误差$e_i(x)$仅依赖于状态量，且它是标量(不是李代数)。$$e_i(x)={\mathbf{e}}_{\mathbf{i}}(x{)}^T{\Omega }_i{\mathbf{e}}_{\mathbf{i}}(x)$$我们的目标是寻找状态变量$x^{\ast }$使得所有观测量的误差最小，如下图所示：

  对于线性优化问题，直接求误差函数的一阶导数空域就可以得到问题的唯一解。但是对于非线性优化问题，我们采用一阶泰勒展开构造近似的线性函数，就是在当前状态变量(current initial guess)附近构造局部线性区域，求解该线性问题，得到迭代步长，希望估计的状态变量使得误差函数距离最小值更近一点。关于误差函数的一阶泰勒展开：

  那么均方误差的泰勒展开如下，由于所有变量都是标量，因此可以无视转置符号。

  定义新的变量，简化公式表达：

  全局误差是所有测量函数均方误差的和，我们用下式近似表达在当前状态解$x$下的全局误差

  其中，$b^T={\sum }_i{e}_i^T{\Omega }_i{J}_i$，$H={\sum }_i{J}_i^T\Omega J_i$，$H$为information matrix或precision matrix，它表达了观测的不确定性(观测的可信度)，它是协方差的逆矩阵。信息矩阵本身是对称的稀疏矩阵，非零部分代表变量之间存在约束，因此非零元素的个数是约束数量的两倍。他的物理含义，当你的观测越确信，观测误差越小(我觉得协方差越小)，那么信息矩阵的值越大。
  “In its physical meaning, information matrix represents how reliable this measurement is. Therefore, the more precisely the measurement is made or the more you trust in this measurement, the larger values in the information matrix you can set. ”
  对近似的全局误差函数(此处认为是线性函数了)在当前状态解$x$上求一阶导数得到，$$\frac{\partial F(x+\Delta x)}{\partial \Delta x}\approx 2b+2H\Delta x$$ 因此迭代步长的解$\Delta x^{\ast }$为$\Delta x^{\ast }=-H^{-1}b$
最小二乘法不断迭代如下步骤：

• 围绕当前猜测的状态变量$x$来线性化系统，并且计算每个观测值的差分$e_i(x+\Delta x)\approx e_i(x)+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}\Delta x$
• 更新之前的状态估计，$x\leftarrow x+\Delta x^{\ast }$

应用举例：Odometry Calibration

  假设我们有机器人在环境中移动且可以获取里程测量值$u_i$。但是由于里程计受系统误差systematic error影响，使得测量值不准确。我们想通过最小二乘法去除系统误差的影响(这个过程叫做calibration)。要想校准，对于每个观测变量$u_i$，必须有对应的ground truth $u_i^{\ast }$，这个基准可以来自scan matching的近似计算或其他slam 过程。
  定义函数$f_i(x)$来纠偏带噪声的里程计读数$u_i$，给定一些偏置参数$x$，输入$u_i$，返回无偏$u_i‘$，如下所示，我们的目标是求参数$x$。

按照之前最小二乘法，状态向量为$x=⟨x_{11}{x}_{12}{x}_{13}{x}_{21}{x}_{22}{x}_{23}{x}_{31}{x}_{32}{x}_{33}{⟩}^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,j}& u_{i,\theta }& & & & & & \\ & & & u_{i,x}& u_{i,y}& u_{i,\theta }& & & \\ & & & & & & u_{i,x}& u_{i,y}& u_{i,\theta }\end{array}\right)$。从结果看，偏导函数并不依赖于偏置参数。
  代码是用octave写的，接近matlab，已上传github地址。
  数据文件z.dat格式：每行是单个里程计的测量值包括，$u_x^{\prime }{u}_y^{\prime }{u}_t^{\prime }{u}_x{u}_y{u}_t$，前三个是里程计ground truth，后三个里程计测量值，它们代表机器人前后两帧的运动。具体代码见github。

more off;
#load the calibration matrix
disp('loading the matrix');
Z=load('data/calib.dat');

#compute the ground truth trajectory
TrueTrajectory=compute_odometry_trajectory(Z(:,1:3));
disp('ground truth');
plot(TrueTrajectory(:,1),TrueTrajectory(:,2));
pause(1);

#compute the uncalibrated odometry
OdomTrajectory=compute_odometry_trajectory(Z(:,4:6));
disp('odometry');
plot(OdomTrajectory(:,1),OdomTrajectory(:,2));
pause(1);

disp('computing calibration parameters');
#compute the calibration parameters
X=ls_calibrate_odometry(Z);
disp(X);
pause(1);

disp('computing calibrated odometry');
COdom=apply_odometry_correction(X,Z(:,4:6));
CalTrajectory=compute_odometry_trajectory(COdom);
plot(CalTrajectory(:,1),CalTrajectory(:,2));

结果：

  此处的里程计校正是标量问题(欧式空间)，变量本身没有约束。如果是求相机位姿之类的问题，由于旋转矩阵$R$(非欧式空间)，是正交矩阵并且行列式值为1，因此变成了有约束的优化问题。我们需要将这些李群转换成李代数，通过转换求解空间得到无约束优化问题，可以继续用牛顿高斯、LM算法在李代数上求导等等。