predict()
从两帧IMU数据中获得当前位姿的预测思路非常简单,无非是求出当前时刻𝑡
与下一时刻𝑡+1
加速度的均值, 把它作为Δ𝑡
时间内的平均加速度,有了这个平均加速度及当前时刻的初始速度和初始位置,就可以近似的求出𝑡+1
时刻的速度和位置。
但是由于IMU的数据存在着坐标系、bias和重力加速度的问题需要额外的一些处理。
首先对于加速度,因为imu的加速度数据是在Body坐标系下表示的,所以要利用对应时刻的姿态将其转换到世界坐标系下,转换之前要减去bias,转化之后要减去重力加速度(世界坐标系下的重力加速度恒等于9.8):
具体可以参考:IMU数据积分获得当前位姿
//从IMU测量值imu_msg和上一个PVQ递推得到下一个tmp_Q,tmp_P,tmp_V,中值积分
void predict(const sensor_msgs::ImuConstPtr &imu_msg)
{
double t = imu_msg->header.stamp.toSec();
//init_imu=1表示第一个IMU数据
if (init_imu)
{
latest_time = t;
init_imu = 0;
return;
}
//两帧IMU数据的时间差
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};
Eigen::Vector3d un_acc_0 = tmp_Q * (acc_0 - tmp_Ba) - estimator.g;
Eigen::Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - tmp_Bg;
tmp_Q = tmp_Q * Utility::deltaQ(un_gyr * dt);
Eigen::Vector3d un_acc_1 = tmp_Q * (linear_acceleration - tmp_Ba) - estimator.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;
}
问题:tmp_Ba忽略了,tmp_Bg为多少?