深蓝学院-多传感器融合定位-第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_