深蓝学院高翔《自动驾驶与机器人中的SLAM技术》第三章作业

Zcoasting

已于 2023-11-03 12:28:14 修改

阅读量245

点赞数 2

分类专栏：深蓝学院高翔《自动驾驶与机器人中的SLAM技术》作业文章标签：自动驾驶机器人

于 2023-10-30 10:30:42 首次发布

本文链接：https://blog.csdn.net/Zcoasting/article/details/134113746

版权

深蓝学院高翔《自动驾驶与机器人中的SLAM技术》作业专栏收录该内容

6 篇文章 4 订阅

订阅专栏

第三章

第二章作业详解
 第四章作业详解

第一题

在这里插入图片描述

第二题

右乘模式下的dx更新
左乘模式下的dx更新

PS: 噪声不参与计算

    // 右乘模式下的dx更新
    auto update_dx_r = [&](Vec18T& dx) {
        dx.template block<3, 1>(0, 0) += dt * dx.template block<3, 1>(3, 0);    // &p
        dx_.template block<3, 1>(3, 0) += dt * (-R_.matrix() * SO3::hat(imu.acce_ - ba_) * dx.template block<3, 1>(6, 0) 
                                        - R_.matrix() * dx.template block<3, 1>(12, 0) 
                                        + dx.template block<3, 1>(15, 0));  // &v
        dx_.template block<3, 1>(6, 0) = (SO3::exp(-(imu.gyro_ - bg_) * dt).matrix()) * dx.template block<3, 1>(6, 0) 
                                        - dt * dx.template block<3, 1>(9, 0);  // &theta
    };

    // 左乘模式下的dx更新
    auto update_dx_l = [&](Vec18T& dx) {
        dx.template block<3, 1>(0, 0) += dt * dx.template block<3, 1>(3, 0);    // &p
        dx_.template block<3, 1>(3, 0) += dt * (-SO3::hat(R_.matrix() * (imu.acce_ - ba_)) * dx.template block<3, 1>(6, 0) 
                                        - R_.matrix() * dx.template block<3, 1>(12, 0) 
                                        + dx.template block<3, 1>(15, 0));  // &v
        dx_.template block<3, 1>(6, 0) =  - dt * R_.matrix() * dx.template block<3, 1>(9, 0);  // &theta
    };

记录运行时间

    Timer timer;
    timer.start();
    bool with_less = true;
    if (!with_less) {
        dx_ = F * dx_;  
    }
    else {
        // update_dx_r(dx_);
        update_dx_l(dx_);
    }
    timer.stop();
    std::cout << "运行时间：" << timer.elapsed() << " 微秒" << std::endl;

修改前：
修改后：

可以很明显的观察到拆分计算后相对于矩阵运算运行时间加快。

第三题

左乘推导过程中，误差变量的运动方程中只有速度与旋转和右乘不同

在这里插入图片描述

根据上图公式修改 Predict() 中 F 矩阵的推导

    Mat18T F = Mat18T::Identity();                                                 // 主对角线
    F.template block<3, 3>(0, 3) = Mat3T::Identity() * dt;                         // p 对 v
    // F.template block<3, 3>(3, 6) = -R_.matrix() * SO3::hat(imu.acce_ - ba_) * dt;  // v对theta 右乘
    F.template block<3, 3>(3, 6) = -SO3::hat(R_.matrix() * (imu.acce_ - ba_)) * dt;  // v对theta 左乘                         
    F.template block<3, 3>(3, 12) = -R_.matrix() * dt;                             // v 对 ba
    F.template block<3, 3>(3, 15) = Mat3T::Identity() * dt;                        // v 对 g
    // F.template block<3, 3>(6, 6) = SO3::exp(-(imu.gyro_ - bg_) * dt).matrix();     // theta 对 theta 右乘
    // F.template block<3, 3>(6, 9) = -Mat3T::Identity() * dt;                        // theta 对 bg 右乘
    F.template block<3, 3>(6, 9) = -R_.matrix() * dt;                        // theta 对 bg 左乘

根据左乘公式修改 UpdateAndReset() 中对名义状态变量根据误差 dx_ 的更新

    /// 更新名义状态变量，重置error state
    void UpdateAndReset() {
        p_ += dx_.template block<3, 1>(0, 0);
        v_ += dx_.template block<3, 1>(3, 0);
        // R_ = R_ * SO3::exp(dx_.template block<3, 1>(6, 0));    // ！！！！改为左乘
        R_ = SO3::exp(dx_.template block<3, 1>(6, 0)) * R_;
        
        if (options_.update_bias_gyro_) {
            bg_ += dx_.template block<3, 1>(9, 0);
        }

        if (options_.update_bias_acce_) {
            ba_ += dx_.template block<3, 1>(12, 0);
        }

        g_ += dx_.template block<3, 1>(15, 0);

        // 以上为均值部分的误差更新，更新后可以简单的实现为dx_=0
        // 下面为协方差的更新，需要考虑重置后的切空间投影

        ProjectCov();
        dx_.setZero();
    }

切空间投影同样需要修改，推导公式如下图

在这里插入图片描述

    /// 对P阵进行投影，参考式(3.63)
    void ProjectCov() {
        Mat18T J = Mat18T::Identity();
        // 右乘
        // J.template block<3, 3>(6, 6) = Mat3T::Identity() - 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
        // 左乘
        J.template block<3, 3>(6, 6) = Mat3T::Identity() + 0.5 * SO3::hat(dx_.template block<3, 1>(6, 0));
        cov_ = J * cov_ * J.transpose();
    }

观测过程同样需要修改成为左乘方式，根据下图公式修改 ObserveSE3()

在这里插入图片描述

    // 更新x和cov
    Vec6d innov = Vec6d::Zero();
    innov.template head<3>() = (pose.translation() - p_);          // 平移部分
    // innov.template tail<3>() = (R_.inverse() * pose.so3()).log();  // 旋转部分(3.67)  右乘
    innov.template tail<3>() = (pose.so3() * R_.inverse()).log();   // 旋转部分 左乘

第二题与第三题修改过后的运行结果，如下图

使用odom观测修正速度

在这里插入图片描述