记录——————————

 
        //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;