1. 添加重力约束

重力约束是在在车体系的方向是（0，0，1），添加时1）如果已知车辆在水平方向运动，添加平面约束；2）如果已知车辆静止（判断IMU加速度计和陀螺仪的norm都很小）则按照IMU的加速度计当观测，协方差用加速度计的模-重力常亮的模

double gravity_diff = fabs(imu_data.acc.norm() - GetIMUConfig().gravity_norm); 
double err = std::acos(GetIMUConfig().gravity_norm /(GetIMUConfig().gravity_norm +gravity_diff));
err = (err < 0.1 / 180.0 * 3.141592653)? (0.1 / 180.0 * 3.141592653): err;
Eigen::Matrix3d cov =Eigen::Matrix3d::Identity() *(err * err + imu_data.gyro.norm() * imu_data.gyro.norm());