fix some formula error

[LeatherWang]

Modifications

• adjust the coordinate system of ins/gps measurement
• add gyro/accel bias compensation into prediction equation
• fix error observation equation and jacobian

Results

[shockjiang]

@LeatherWang 请问这个mr merge之后，对于输入数据的坐标系是否有要求。例如： accel-0.csv: accel_x (m/s^2),accel_y (m/s^2),accel_z (m/s^2)， 是NED还是ENU？ gps-0.csv: gps_lat (deg),gps_lon (deg),gps_alt (m),gps_vN (m/s),gps_vE (m/s),gps_vD (m/s)，是否依然可以？注意，此处速度为NED gyro-0.csv：gyro_x (deg/s),gyro_y (deg/s),gyro_z (deg/s)：相对于IMU body frame的坐标系，or ENU?

[shockjiang]

@LeatherWang 在理解这个branch的时候遇到了一些困难，不知是否方便请教一下？

[NicolasHHH]

@LeatherWang 请问这个mr merge之后，对于输入数据的坐标系是否有要求。例如： accel-0.csv: accel_x (m/s^2),accel_y (m/s^2),accel_z (m/s^2)， 是NED还是ENU？ gps-0.csv: gps_lat (deg),gps_lon (deg),gps_alt (m),gps_vN (m/s),gps_vE (m/s),gps_vD (m/s)，是否依然可以？注意，此处速度为NED gyro-0.csv：gyro_x (deg/s),gyro_y (deg/s),gyro_z (deg/s)：相对于IMU body frame的坐标系，or ENU?

根据改动，accel-0 应该是 ENU， 因为imu_flow.cpp 中的TransformCoordinate都被删了。 gyro-0与accel-0一致。 gps-0保持NED, 在tool.h 中对其进行了转换 （注意Eigen::AngleAxisd复合运算时，旋转轴始终是初始的xyz，因此去掉了.inverse()）

[zm0612]

Thank you for your contribution to this project. I have further refactored the entire algorithm according to your ideas, welcome to review. @LeatherWang