这里给出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::Vector3d angular_velocity{rx, ry, rz};
//! 这个地方的tmp_Q是local-->world
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba - tmp_Q.inverse() * g);
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba - tmp_Q.inverse() * g);
Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
tmp_P = tmp_P + dt * tmp_V + 0.5 * dt * dt * un_acc;
tmp_V = tmp_V + dt * un_acc;
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}
template <typename Derived>
static Eigen::Quaternion<typename Derived::Scalar> deltaQ(const Eigen::MatrixBase<Derived> &theta)
{
typedef typename Derived::Scalar Scalar_t;
Eigen::Quaternion<Scalar_t> dq;
Eigen::Matrix<Scalar_t, 3, 1> half_theta = theta;
half_theta /= static_cast<Scalar_t>(2.0);
dq.w() = static_cast<Scalar_t>(1.0);
dq.x() = half_theta.x();
dq.y() = half_theta.y();
dq.z() = half_theta.z();
return dq;
}