LidarSLAM（四）：robot_localization

使用的目的robot_loacaization

相似场景匹配错误调整时发生的“跳变”

机器人轨迹不“平滑”

协方差的计算

X i= [1.1, 1.9, 3]

Yi = [5.0, 10.4, 14.6]

E(X) = (1.1 + 1.9 + 3) / 3 = 2

E(Y) = (5.0 + 10.4 + 14.6) / 3 = 10

E(XY) = (1.1 × 5.0 + 1.9 × 10.4 + 3 × 14.6) / 3 = 23.02

Cov(X,Y) = E(XY) - E(X)E(Y) = 23.02 - 2 × 10 = 3.02

此外：还可以计算：D(X) = E(X^2) - E^2(X) = (1.1^2 + 1.9^2 + 3^2) / 3 - 4 = 4.60 - 4 = 0.6  σx = 0.77

                             D(Y) = E(Y^2) -  E^2(Y) = (5^2 + 10.4^2 + 14.6^2) / 3 - 100 = 15.44  σy = 3.93

X,Y的相关系数：

r(X,Y) = Cov(X,Y) / (σxσy) = 3.02 / (0.77×3.93)  =  0.9979

表明这组数据X,Y之间相关性很好!

 

robot_localization的使用

1选择全局融合还是局部融合

局部

map_frame: map             

odom_frame: odom           

base_link_frame: base_link     

world_frame: odom           

全局

map_frame: map            

odom_frame: odom          

base_link_frame: base_link   

world_frame: map

没有使用GPS，所以选用局部融合

2 使用UKF还是EKF？

UKF is slower but more accurate for non-linear transformations

EKF is faster though!

But more importantly, according to [1]. EKF is just as effective as UKF for sensor fusion, so just use EKF when fusing nav data for ROS

[1] https://www.sciencedirect.com/science/article/pii/S0921889015001517?via%3Dihub

3确定需要融合的传感器

本次试验中选择了四种：轮子的编码器、三维激光经LOAM算法生成的里程计，IMU以及hdl

4需要确定每种传感器提供了哪种信息

imu：三个角速度，三个角加速度（三个转角必须要在内）

轮速计： xyz的移动和vvw的转动  轮速计是差分模式融合，realizative 修改为true

LOAM输出：xyz的移动和vvw的转动

hdl：xyz的移动和vvw的转动

注意信息的完整，比如占有编码器和imu进行融合时odom的xyz就要打开，和LOAM一起时，不需要打开，因为loam有xyz

注意检查y方向的偏移

确定协方差矩阵

因此，如果要将协方差矩阵传递给卡尔曼滤波器，则最重要的值实际上是方差值（从左上角到右下角的对角线）。

     这些差异决定了传感器对于特定轴的“可信度”！
     方差更高➡噪声/不确定性更高➡可信度更低➡卡尔曼滤波器将减少传感器的读数！
     调整每个传感器的测量协方差矩阵非常重要！ 因此，正在调整您的初始协方差估计以及过程噪声协方差矩阵。

此外：如果协方差随着时间的推移逐渐收敛（或消失，更好），您就会知道它们已正确调整。

如果它们爆炸了，那只会破坏数据的可靠性...