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方向的偏移
确定协方差矩阵
因此，如果要将协方差矩阵传递给卡尔曼滤波器，则最重要的值实际上是方差值（从左上角到右下角的对角线）。
这些差异决定了传感器对于特定轴的“可信度”！
方差更高➡噪声/不确定性更高➡可信度更低➡卡尔曼滤波器将减少传感器的读数！
调整每个传感器的测量协方差矩阵非常重要！ 因此，正在调整您的初始协方差估计以及过程噪声协方差矩阵。
此外：如果协方差随着时间的推移逐渐收敛（或消失，更好），您就会知道它们已正确调整。
如果它们爆炸了，那只会破坏数据的可靠性...