VINS-Mono工程笔记（五）：IMU预积分
发布时间
阅读量：
阅读量
1.processimu()函数分析
/** * 处理IMU数据
* linear_acceleration 线加速度
* angular_velocity 角速度
* */
void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)
{
//1.判断是不是第一个imu消息，如果是第一个imu消息，则将当前传入的线加速度和角速度作为初始的加速度和角速度
if (!first_imu)
{
first_imu = true;
acc_0 = linear_acceleration;//记录线加速度值
gyr_0 = angular_velocity;//记录角速度值
}
/** * 2.创建预积分对象
* 首先，pre_integrations是一个数组，里面存放了(WINDOW_SIZE + 1)个指针，指针指向的类型是IntegrationBase的对象
*/
if (!pre_integrations[frame_count])
{
//创建pre_integrations[frame_count]中的对象
pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};
}
//frame_count==0表示此时滑动窗口中还没有图像帧数据，所以先不进行预积分
if (frame_count != 0)
{
//3.进行预计分
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
//if(solver_flag != NON_LINEAR)
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);
dt_buf[frame_count].push_back(dt);
//当前的线加速度和角速度存放到先加速度buffer和角速度buffer当中
linear_acceleration_buf[frame_count].push_back(linear_acceleration);
angular_velocity_buf[frame_count].push_back(angular_velocity);
int j = frame_count;
/** * 4.更新Rs、Ps、Vs三个向量数组。
* Rs为旋转向量数组，Ps为位置向量数组，Vs为速度向量数组，数组的大小为WINDOW_SIZE + 1
* 那么，这三个向量数组中每个元素都对应的是每一个window
*/
//计算上一时刻的加速度，前边乘一个旋转矩阵Rs[j]的作用是进行坐标系转换
Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;
//根据上一时刻陀螺仪的角速度和当前时刻的角速度求出平均角速度
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);
//位移（位置）更新，位置是在之前的基础上加上当前的位移量，使用的是位移公式：s = v*t + 1/2*a*t^2
Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;
//速度更新，使用的是速度公式：v = a * t a是加速度，t是时间
Vs[j] += dt * un_acc;
}
//更新acc_0和gyr_0的值，本次的线加速度和角速度作为下一个IMU消息的前一个状态值
acc_0 = linear_acceleration;
gyr_0 = angular_velocity;
}识别第一个imu数据的标志的同时,记录last_imu数据.因为使用的是中值积分法,需要依赖于一个之前的imu数据.
在滑动窗口中的每一帧生成IntegrationBase对象，并将其加入到pre_integrations数组中
当 frame_count 等于零时，则表示滑动窗口内尚未采集到任何图像帧数据。
所以无需执行预积分操作。
仅更新线加速度与角速度的初始值参数。
而当 frame_count 不等于零时，
表明滑动窗口内已获取到相应的图像帧数据。
即可对对应图像帧采集到的 IMU 设备执行预积分操作。
通过 IntegrationBase 类型对象的 push_back 函数实现。
pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);4）重新赋值Rs、Ps以及Vs这三个数组。其中每个数组的长度等于滑动窗口宽度加一。通过基于图像帧ID的方式计算得到，在滑动窗口中的每一个图像帧都对应着一个旋转矩阵、位置坐标以及速度信息。这些参数在后续步骤中将被用于窗口滑动操作以及图像位姿初始化的过程。
2.imu预积分代码
/** * dt 时间间隔
* acc 陀螺仪的线加速度
* gyr 陀螺仪的角速度
*/
void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
dt_buf.push_back(dt);
acc_buf.push_back(acc);
gyr_buf.push_back(gyr);
//进入预积分阶段
propagate(dt, acc, gyr);
}push_back()内部存储了imu数据，并执行propagate进行前向积分。
/** * 预计分计算
* _dt 时间间隔
* _acc_1 线加速度
* _gyr_1 角速度
*/
void propagate(double _dt, const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1)
{
dt = _dt;
acc_1 = _acc_1;
gyr_1 = _gyr_1;
Vector3d result_delta_p;
Quaterniond result_delta_q;
Vector3d result_delta_v;
Vector3d result_linearized_ba;
Vector3d result_linearized_bg;
//中点积分方法
midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
linearized_ba, linearized_bg,
result_delta_p, result_delta_q, result_delta_v,
result_linearized_ba, result_linearized_bg, 1);
//checkJacobian(_dt, acc_0, gyr_0, acc_1, gyr_1, delta_p, delta_q, delta_v,
// linearized_ba, linearized_bg);
//更新PQV
delta_p = result_delta_p;
delta_q = result_delta_q;
delta_v = result_delta_v;
//更新偏置
linearized_ba = result_linearized_ba;
linearized_bg = result_linearized_bg;
delta_q.normalize();
//时间进行累加
sum_dt += dt;
//预积分完后，更新当前的线加速度和角速度为上一时刻的线加速度和角速度
acc_0 = acc_1;
gyr_0 = gyr_1;
}基于初始状态量(p, v, q, ba, bg)为零的条件，在midPointIntegration()函数下完成单个imu的中值积分运算。将所有imu在当前帧图像内各自计算其积分结果。其中该积分过程仅基于imu本地坐标系完成，并未考虑重力加速度以及初始状态的影响。在integratebase函数体内完成单个imu的积分计算后，系统会更新当前帧的状态量参数。
在midPointIntegration()函数内部开发了一种基于离散采样的积分运算机制，并负责传递运动学方程的具体数值解
/** * 该函数是以中值点的方式进行预积分求解PVQ的，需要注意的是这里使用的是离散形式的预积分公式
* 参数中_0代表上次测量值，_1代表当前测量值，delta_p，delta_q，delta_v代表相对预积分初始参考系的位移，旋转四元数，以及速度
* 从k帧预积分到k+1帧，则参考系是k帧的imu坐标系
*/
void midPointIntegration(double _dt,
const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,
const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,
const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,
const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,
Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,
Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)
{
//ROS_INFO("midpoint integration");
//delta_q为相对预计分参考系的旋转四元数，线加速度的测量值减去偏差然后和旋转四元数相乘表示将线加速度从世界坐标系下转到了body(IMU)坐标系下
Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);
//计算平均角速度
Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
//对平均角速度和时间的乘积构成的旋转值组成的四元数左乘旋转四元数，获得当前时刻body中的旋转向量（四元数表示）
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
//用计算出来的旋转向量左乘当前的加速度，表示将线加速度从世界坐标系下转到了body坐标系下
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
//计算两个时刻的平均加速度
Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);
//当前的位移：当前位移=前一次的位移+(速度×时间)+1/2×加速度的×时间的平方
//匀加速度运动的位移公式：s_1 = s_0 + v_0 * t + 1/2 * a * t^2
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
//速度计算公式：v_1 = v_0 + a*t
result_delta_v = delta_v + un_acc * _dt;
// 预积分的过程中Bias并未发生改变，所以还保存在result当中
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
//是否更新IMU预计分测量关于IMU Bias的雅克比矩阵
if(update_jacobian)
{
//计算平均角速度
Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;
//计算_acc_0这个观测线加速度对应的实际加速度
Vector3d a_0_x = _acc_0 - linearized_ba;
//计算_acc_1这个观测线加速度对应的实际加速度
Vector3d a_1_x = _acc_1 - linearized_ba;
Matrix3d R_w_x, R_a_0_x, R_a_1_x;
/** * | 0 -W_z W_y |
* [W]_x = | W_z 0 -W_x |
* | -W_y W_x 0 |
*/
R_w_x<<0, -w_x(2), w_x(1),
w_x(2), 0, -w_x(0),
-w_x(1), w_x(0), 0;
R_a_0_x<<0, -a_0_x(2), a_0_x(1),
a_0_x(2), 0, -a_0_x(0),
-a_0_x(1), a_0_x(0), 0;
R_a_1_x<<0, -a_1_x(2), a_1_x(1),
a_1_x(2), 0, -a_1_x(0),
-a_1_x(1), a_1_x(0), 0;
//F是一个15行15列的数据类型为double，数据全部为0的矩阵，Matrix创建的矩阵默认按列存储
MatrixXd F = MatrixXd::Zero(15, 15);
/** * matrix.block(i,j, p, q) : 表示返回从矩阵(i, j)开始，每行取p个元素，每列取q个元素所组成的临时新矩阵对象，原矩阵的元素不变；
* matrix.block<p,q>(i, j) :<p, q>可理解为一个p行q列的子矩阵，该定义表示从原矩阵中第(i, j)开始，获取一个p行q列的子矩阵，
* 返回该子矩阵组成的临时矩阵对象，原矩阵的元素不变；
*/
//从F矩阵的(0,0)位置的元素开始，将前3行3列的元素赋值为单位矩阵
/** * 下面F和V矩阵为什么这样构造，是需要进行相关推导的。这里的F、V矩阵的构造理解可以参考论文附录中给出的公式
*/
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();
//cout<<"A"<<endl<<A<<endl;
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;
//step_jacobian = F;
//step_V = V;
/** * 求矩阵的转置、共轭矩阵、伴随矩阵：可以通过成员函数transpose()、conjugate()、adjoint()来完成。注意：这些函数返回操作后的结果，
* 而不会对原矩阵的元素进行直接操作，如果要让原矩阵进行转换，则需要使用响应的InPlace函数，如transpoceInPlace()等
*/
//雅克比jacobian的迭代公式：J_(k+1)=F*J_k，J_0=I
jacobian = F * jacobian;
/** * covariance为协方差，协方差的迭代公式：P_(k+1) = F*P_k*F^T + V*Q*V^T，P_0 = 0
* P_k就是协方差，Q为noise，其初值为18*18的单位矩阵
*/
covariance = F * covariance * F.transpose() + V * noise * V.transpose();
}
}值得强调的是，在VINS论文中所采用的是欧拉积分法作为理论基础，在代码实现过程中却采用了中值积分法进行具体计算。其中噪声信号主要叠加在原先定义的角加速度与线加速度的基础上，并基于前一帧的角加速度与线加速度引入了随机游走型噪声信号（共计18维）。
最新的雅可比矩阵与状态斜方差被用来计算积分类别加权残差项以及预积分偏差值的修正量
jacobian = F * jacobian;
/** * covariance为协方差，协方差的迭代公式：P_(k+1) = F*P_k*F^T + V*Q*V^T，P_0 = 0
* P_k就是协方差，Q为noise，其初值为18*18的单位矩阵
*/
covariance = F * covariance * F.transpose() + V * noise * V.transpose();3.imu运动学方程推导。
全部评论 (0)
还没有任何评论哟~