VINS-Mono代码学习记录-predict()

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为多少?

  • 3
    点赞
  • 6
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值