slam传感器数据处理Ⅱ——里程计标定

用最小二乘法标定

1、线性最小二乘的基本原理

通用性好，效果一般。(pid),

1. 线性方程组Ax=b

   为 × 的矩阵。
   x为 × 1的向量

行表示约束，列表示自由度（未知数的维度）；

当m=n时，适定方程组，方程组有唯一解

当m

当m>n时，超定方程组，方程组有通常无解 （最多的）

2. 绝大多数情况为m>n，超定方程组

   无解，可以寻找最靠近真实解的解
   无解但是有最小二乘解
   *通解: ∗ = ()−1

如果一个系统AX=B 是病态的（AX=B+&b）会导致最终结果偏差非常大。

3. 最小二乘的求解—线性空间的角度

   • 表示的列向量空间
   • 无解意味着向量不在中
   • 最近的解即为：向量在中的投影
     设∗为向量b在空间中的投影，显然 − ∗ 垂直于空间。
     （ − ∗） 跟矩阵的每一个列向量都垂直
     

解：误差最小的方向

在非线性中 对误差求偏导 最终来拟合。

##### 2、最小二乘的直线拟合

1. 例子

   • 直线拟合—y=5x+2
     - 采样数据：
     x=(1,2,3,4,5,6,7,8,9,10)
     y = (6.9918 , 14.2987, 16.2019, 22.4263, 25.6191,33.2563, 35.7755, 42.0298, 47.9954, 53.9545)

• 构建方程组：
• 代入数据可得：

线性最小二乘的高纬代数通解

最小二乘会被个别点带偏，对噪声 处理，用 Ransac … 也可用迭代最小二乘（跟卡尔曼滤波有深厚关系卡尔曼过程噪声为0时 结果一致）。

3、最小二乘在里程计标定中的应用

1. 直接线性方法

   direct Liner method

   • 通用性强
   • 实现简单
   • 精度不高

2. *基于模型的方法

   Modle-hased

   • 精度高
   • 特异性高
   • 实现复杂

3. 实现直接线性方法

   • 用激光雷达的scan-match数据作为真值∗ （ui*表示pi pj之间相对位置）PL-ICP)
   • 里程计测量得到的数据为 （oi ,oj之间相对位置关系）
   • 假设成线性关系∗ = ∗ （呈线性关系）

其中：

​ 11 12 13
X = 21 22 23
​ 31 32 33

对于每一组数据，可得：

∗ 11 + ∗ 12+ ∗ 13 = ∗
∗ 21 + ∗ 22+ ∗ 23 = ∗
∗ 31 + ∗ 32+ ∗ 33 = ∗

测试环节 用到CSM包 sudo apt-get ros-ingigo-kinect CSM