imu单次测量离散积分代码

这里给出imu积分代码， 所算的tmp_Q， tmp_P， tmp_V在 /world 坐标系下

Eigen::Vector3d tmp_P;
Eigen::Quaterniond tmp_Q;
Eigen::Vector3d tmp_V;
Eigen::Vector3d tmp_Ba;//bias可以优化计算，也可以用工程化方法得出
Eigen::Vector3d tmp_Bg;

Eigen::Vector3d g; // /world
Eigen::Vector3d acc_0;//上一时刻的测量值
Eigen::Vector3d gyr_0;//上一时刻的测量值


//input：imu_msg ros采集

//output： tmp_Q， tmp_P， tmp_V

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
    double t = imu_msg->header.stamp.toSec();
    double dt = t - latest_time;
    latest_time = t;

    double dx = imu_msg->linear_acceleration.x;
    double dy = imu_msg->linear_acceleration.y;
    double dz = imu_msg->linear_acceleration.z;
    Eigen::Vector3d linear_acceleration{dx, dy, dz};

    double rx = imu_msg->angular_velocity.x;
    double ry = imu_msg->angular_velocity.y;
    double rz = imu_msg->angular_velocity.z;
    Eigen::Vector