//ROS_INFO("midpoint integration");
//注意这里不涉及世界坐标系,全在body坐标系下。公式见预积分离散形式(vio从零开始写vi0中公式33)
Vector3d un_acc_ = delta_q * (_acc_0 - linearized_ba);
//对角速度处理
Vector3d un_gyr_1 = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;//改进前的角速度w
Vector3d temp_sum=_gyr_0 + _gyr_1;
Vector3d un_gyr_2=_gyr_0.norm()*_gyr_0/temp_sum.norm() + _gyr_1.norm()*_gyr_1/temp_sum.norm-linearized_bg;//改进前的角速度w
Vector3d un_gyr=(un_gyr_1+un_gyr_2)/2;//改进后的角速度w
//对Q处理
result_delta_q = delta_q * Quaterniond(1, un_gyr(0) * _dt / 2, un_gyr(1) * _dt / 2, un_gyr(2) * _dt / 2);
Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);
//对加速度进行处理
Vector3d un_acc_1 = 0.5 * (un_acc_0 + un_acc_1);//
Vector3d temp_sum2=un_acc_0 + un_acc_1;
Vector3d un_acc_2 =un_acc_0.norm()* un_acc_0/temp_sum2.norm() + un_acc_1.norm()*un_acc_1/temp_sum2;
Vector3d un_acc = (un_acc_1+un_acc_2)/2;//改进后的加速度
result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;
result_delta_v = delta_v + un_acc * _dt;
result_linearized_ba = linearized_ba;
result_linearized_bg = linearized_bg;
记录——————————
于 2021-01-11 22:30:26 首次发布