深蓝学院-多传感器融合定位-第6章作业

1. 及格题目

及格题目: 根据课程给定的数据，完成基于中值法的解算；

首先，看原始代码的activity.cpp文件，其中已经有写好的Helper Routine, 基本按着这个步骤和Todo里面的顺序，填一下function就行了。

bool Activity::UpdatePose(void) {
     
    if (!initialized_) {
     
        // use the latest measurement for initialization:
        OdomData &odom_data = odom_data_buff_.back();
        IMUData imu_data = imu_data_buff_.back();

        pose_ = odom_data.pose;
        vel_ = odom_data.vel;

        initialized_ = true;

        odom_data_buff_.clear();
        imu_data_buff_.clear();

        // keep the latest IMU measurement for mid-value integration:
        imu_data_buff_.push_back(imu_data);
    } else {
     
        //
        // TODO: implement your estimation here
        //
        // get deltas:
        Eigen::Vector3d deltas = Eigen::Vector3d::Zero();
        size_t index_curr_ = 1;
        size_t index_prev_ = 0;
        if(Activity::GetAngularDelta(index_curr_, index_prev_, deltas) == false){
     
            std::cout << "GetAngularDelta(): index error" << std::endl;
            return false;
        }

        // update orientation:
        Eigen::Matrix3d R_curr_ = Eigen::Matrix3d::Identity();
        Eigen::Matrix3d R_prev_ = Eigen::Matrix3d::Identity();
        Activity::UpdateOrientation(deltas, R_curr_, R_prev_);

        // get velocity delta:
        double delta_t_ = 0;
        Eigen::Vector3d velocity_delta_ = Eigen::Vector3d::Zero();
        Activity::GetVelocityDelta(index_curr_, index_prev_, R_curr_, R_prev_, delta_t_, velocity_delta_);

        // update position:
        Activity::UpdatePosition(delta_t_, velocity_delta_);

        // move forward -- 
        // NOTE: this is NOT fixed. you should update your buffer according to the method of your choice:
        imu_data_buff_.pop_front();
    }
    
    return true;
}

2. 良好题目

良好题目: 根据课程给定的数据，完成基于中值法、欧拉法的解算，并对精度做对比分析；

只用修改几个小地方,非常简单。

// angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev); // 中值法
angular_delta = delta_t*angular_vel_prev; // 欧拉法

// velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev); // 中值法
velocity_delta = delta_t*linear_acc_curr; // 欧拉法

中值积分效果如下图，

欧拉积分效果如下图，

很明显，中值积分的精度远高于欧拉积分。

3. 优秀题目

优秀题目: 利用IMU仿真程序，自己生成不同运动状况(静止、匀速、加减速、快速转弯等)的仿真数
据，对比两种解算方法精度差异与运动状况的关系，并给出原因分析

（生成rosbag以后，怎么用一二题的imu_integration代码来进行积分，不太明白。）