Function predict

src/dvins/vins_estimator/src/estimator_node.cpp:82–121

};

void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
  double t = imu_msg->header.stamp.toSec();
  if (init_imu) {
    latest_time = t;
    init_imu = 0;
    P_rt.setZero();
    V_rt.setZero();
    Q_rt.setIdentity();
    Ba_rt.setZero();
    Bg_rt.setZero();
    acc_0 = Eigen::Vector3d(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
    gyr_0 = Eigen::Vector3d(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
    return;
  }
  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 acc_1{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 gyr_1{rx, ry, rz};

  Eigen::Vector3d un_acc_0 = Q_rt * (acc_0 - Ba_rt) - estimator.g;
  Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + gyr_1) - Bg_rt;
  Q_rt = Q_rt * Utility::deltaQ(un_gyr * dt);
  Eigen::Vector3d un_acc_1 = Q_rt * (acc_1 - Ba_rt) - estimator.g;
  Eigen::Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
  P_rt = P_rt + dt * V_rt + 0.5 * dt * dt * un_acc;
  V_rt = V_rt + dt * un_acc;
  W_rt = gyr_1 - Bg_rt;

  acc_0 = acc_1;
  gyr_0 = gyr_1;
}

void update()
{