车载多传感器融合定位方案Pipeline：IMU，CNSS，LIDAR

为什么用IMU, CNSS, LIDAR的组合

IMU可以提供连续的位置信息，以补充GNSS可能因为地形等原因缺失的数据信息
GNSS可以提供绝对的位置信息，减轻IMU累积的数据漂移问题
LIDAR可以根据准确的周边信息，确定自己在地图中的位置
三者可相互补充共同确定车辆的状态和所处位置

整体流程图

整个方案的大体思路为：将IMU数据作为输入带入运动学方程计算nominal state，当其他观测数据available时，将其带入计算车辆error state,然后两者相加得出最终的更正状态，用于后续迭代。当没有观测数据时，就将预测数据作为结果进行迭代。

采用这种方式的好处是：将最终的true state的确定分成了两个，nominal state只和状态方程和输入有关，这样一来我们就能很方便的在方程上增加想要的约束。
另一个好处在于能很好地处理传感器数据暂时丢失的问题。有观测数据的时候就拿来修正状态，没有的时候就用预测状态，应用起来比较flexible。当然只用IMU时，数据漂移现象还是没法避免，个人感觉在这种情况下，可能还需要camera辅助。

1. 同步传感器坐标系

通常情况下，IMU和GNSS数据用的是车身坐标系，而LIDAR坐标系与传感器on board位置相关，因此要将LIDAR数据转换到车身坐标系，完成传感器数据坐标系的统一

C_li = np.array([
     [ 0.9975 , -0.04742,  0.05235],
     [ 0.04992,  0.99763, -0.04742],
     [-0.04998,  0.04992,  0.9975 ]
])

t_i_li = np.array([0.5, 0.1, 0.5])

# Transform from the LIDAR frame to the vehicle (IMU) frame.
lidar.data = (C_li @ lidar.data.T).T + t_i_li

2. 设置预估传感器方差和噪音常量

这是滤波器最重要的方面之一，如果出现标定错误或者数据丢失时，通常需要调整传感器方差进行补偿。

var_imu_f = 0.10
var_imu_w = 0.001
var_gnss  = 0.01
var_lidar = 100

g = np.array([0, 0, -9.81])  # gravity 1x3
l_jac = np.zeros([9, 6])
l_jac[3:, :] = np.eye(6)  # motion model noise jacobian
h_jac = np.zeros([3, 9])
h_jac[:, :3] = np