vins中imu融合_VINS 中的 IMU 预积分推导和代码解读

原标题：VINS 中的 IMU 预积分推导和代码解读

VIO 中,如果在世界坐标系中对 IMU 进行积分,积分项中包含体坐标系相对于世界坐标系的瞬时旋转矩阵。然而,在优化位姿时,关键帧时刻体坐标系相对于世界坐标系的旋转矩阵会发生变化,那么需要对 IMU 重新进行积分。预积分就是为了避免这种重复积分。IMU 预积分将参考坐标系改为前一帧的体坐标系,从而积出了两帧之间的相对运动。

预积分

将第 k 帧和第 k+1 帧之间的所有 IMU 进行积分,可得第 k+1 帧的位置、速度和旋转(PVQ)，作为视觉估计的初始值，示意图如下：

(点击图片放大看公式)

连续形式

离散形式

//采用的是中值积分的传播方式

Vector3d un _gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];

Rs[j] *= Utility::deltaQ(un_gyr *dt).toRotationMatrix;

Vector3d un _acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;

Vector3d un _acc = 0.5 * (un_acc _0 + un_acc_1);

Ps[j] += dt * Vs[j] + 0.5 *dt * dt *un_acc;

Vs[j] += dt * un_acc;

(左右滑动试试)

F:

MatrixXd F = MatrixXd::Zero(15, 15);

F.block <3,3>(0, 0) = Matrix3d::Identity;

F.block <3,3>(0, 3) = -0.25 * delta_q.toRotationMatrix * R_a_0_x * _dt * _dt +

-0.25 * result_delta_q.toRotationMatrix * R_a_1_x *

(Matrix3d::Identity - R_w_x * _dt) * _dt * _dt;

F.block <3,3>(0, 6) = MatrixXd::Identity(3,3) * _dt;

F.block <3,3>(0, 9) = -0.25 * (delta_q.toRotationMatrix +

result_delta_q.toRotationMatrix) * _dt * _dt;

F.block <3,3>(0, 12) = -0.25 * result_delta_q.toRotationMatrix * R_a_1_x * _dt * _dt * -

_dt;

F.block <3,3>(3, 3) = Matrix3d::Identity - R_w_x * _dt;

F.block <3,3>(3, 12) = -1.0 * MatrixXd::Identity(3,3) * _dt;

F.block <3,3>(6, 3) = -0.5 * delta_q.toRotationMatrix * R_a_0_x * _dt +

-0.5 * result_delta_q.toRotationMatrix * R_a_1_x *

(Matrix3d::Identity - R_w_x * _dt) * _dt;

F.block <3,3>(6, 6) = Matrix3d::Identity;

F.block <3,3>(6, 9) = -0.5 * (delta_q.toRotationMatrix +

result_delta_q.toRotationMatrix) * _dt;

F.block <3,3>(6, 12) = -0.5 * result_delta_q.toRotationMatrix * R_a_1_x * _dt * -_dt;

F.block <3,3>(9, 9) = Matrix3d::Identity;

F.block <3,3>(12, 12) = Matrix3d::Identity;

(左右滑动试试)

V:

MatrixXd V = MatrixXd::Zero(15,18);

V.block <3,3>(0, 0) = 0.25 * delta_q.toRotationMatrix * _dt * _dt;

V.block <3,3>(0, 3) = 0.25 * -result_delta_q.toRotationMatrix * R_a_1_x * _dt * _dt *

0.5 * _dt;

V.block <3,3>(0, 6) = 0.25 * result_delta_q.toRotationMatrix * _dt * _dt;

V.block <3,3>(0, 9) = V.block <3,3>(0, 3);

V.block <3,3>(3, 3) = 0.5 * MatrixXd::Identity(3,3) * _dt;

V.block <3,3>(3, 9) = 0.5 * MatrixXd::Identity(3,3) * _dt;

V.block <3,3>(6, 0) = 0.5 * delta_q.toRotationMatrix * _dt;

V.block <3,3>(6, 3) = 0.5 * -result_delta_q.toRotationMatrix * R_a_1_x * _dt * 0.5 *

_dt;

V.block <3,3>(6, 6) = 0.5 * result_delta_q.toRotationMatrix * _dt;

V.block <3,3>(6, 9) = V.block <3,3>(6, 3);

V.block <3,3>(9, 12) = MatrixXd::Identity(3,3) * _dt;

V.block <3,3>(12, 15) = MatrixXd::Identity(3,3) * _dt;

(左右滑动试试)

离散形式的 PVQ 增量误差的 Jacobian 和协方差

对应代码在integration_base.h文件的midPointIntegration：

jacobian= F * jacobian;

covariance= F * covariance * F.transpose + V * noise * V.transpose;

从零开始学习三维视觉核心技术SLAM，扫描查看介绍，3天内无条件退款

早就是优势，学习切忌单打独斗，这里有教程资料、练习作业、答疑解惑等，优质学习圈帮你少走弯路，快速入门！返回搜狐，查看更多

责任编辑：