深蓝学院第二章作业

第一题：设置IMU仿真代码中的不同参数，并用Allan方差进行标定

需要说明的是：Allan方差标定的必须是处于静止状态的IMU数据，并且通常需要获得3小时以上的IMU数据输出（Allan方差分析的是IMU的时间特性），若连接实物测试则耗费时间较长。此处为仿真生成数据，因此时间很短。
第一题与第二题有在两处有显著的不同：

• 其一：对于第一题静止状态下的IMU，仿真时长设置为4*3600秒，而第二题运动状态下的IMU数据仅仿真20s。
• 其二：为了确保获得静止状态下的IMU，只需将姿态、角速度、位置、速度和加速度均设置为0即可，对于包含噪声的数据处理方式是相同的。

1 使用 imu_utils 完成 allan 标定

步骤1：ros下编译

由于imu_utils的编译需要用到code_utils功能包，因此需要在imu_utils的package.xml文件中加入以下代码：

<build_depend>code_utils</build_depend>

网上普遍使用先编译code_utils，再编译imu_utils的方法，个人感觉在编译更大型项目时颇为不妥。

除此之外，可能会遇到其他一些缺少库的问题，对应安装上即可，问题不是很大。

我是单独创建了一个工作空间用于编译本章内容的代码，其中包括三个功能包：code_utils、imu_utils和vio_data_simulation。

步骤2：执行vio_data_simulation_node，生成imu.bag

在编译完成后会生成很多个节点，而vio_data_simulation下有两个：vio_data_simulation_node和vio_data_simulation_node_txt，vio_data_simulation_node生成.bag，vio_data_simulation_node_txt作用与非ros下编译的gen_data类似。

在终端中执行以下代码：

rosrun vio_data_simulation vio_data_simulation_node

则会在主目录下生成imu.bag文件，主目录指"/home/user_name/"

步骤3：播放imu.bag，并用imu_utils接收

为了使imu.bag和imu_utils中的launch文件同时运行，对imu_utils中的my.launch进行如下更改：（为节省时间，采用500倍速率回放）

<launch>

    <node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
        <param name="imu_topic" type="string" value= "/imu"/>
        <param name="imu_name" type="string" value= "mytest"/>
        <param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/>
        <param name="max_time_min" type="int" value= "120"/>
        <param name="max_cluster" type="int" value= "100"/>
    </node>

    <node pkg="rosbag" type="play" name="playe" output="screen" args="-r 500 /home/tiandao/vio_sim_ws/imu.bag"/>

</launch>

在终端中执行：

roslaunch imu_utils my.launch

能够在…/imu_utils/data文件夹下生成17个文件，前16个txt文件为误差，最后一个yaml文件为标定结果，可以作为IMU标定的参数。我们看一下yaml中的内容。

%YAML:1.0
---
type: IMU
name: mytest
Gyr:
   unit: " rad/s"
   avg-axis:
      gyr_n: 2.0866512204425436e-01
      gyr_w: 9.3802691125544730e-04
   x-axis:
      gyr_n: 2.0900719595284514e-01
      gyr_w: 8.4776684794549064e-04
   y-axis:
      gyr_n: 2.0753009869661257e-01
      gyr_w: 9.6996805171902704e-04
   z-axis:
      gyr_n: 2.0945807148330531e-01
      gyr_w: 9.9634583410182454e-04
Acc:
   unit: " m/s^2"
   avg-axis:
      acc_n: 2.6757254048053242e-01
      acc_w: 3.1735792946508980e-03
   x-axis:
      acc_n: 2.6960190515117144e-01
      acc_w: 2.7945709382155384e-03
   y-axis:
      acc_n: 2.6814423616754696e-01
      acc_w: 3.3684261723526455e-03
   z-axis:
      acc_n: 2.6497148012287886e-01
      acc_w: 3.3577407733845106e-03

将标定获得数与预设值进行对比，预设值在param.h中

// noise
double gyro_noise_sigma = 0.015;    // rad/s * 1/sqrt(hz)
double acc_noise_sigma = 0.019;      //　m/(s^2) * 1/sqrt(hz)
// bias
double gyro_bias_sigma = 0.00005;
double acc_bias_sigma = 0.0005;

由于imu_utils中并未涉及随机游走的标定，因此下面仅对imu噪声进行对比分析。

$accnoise=0.2676\ast \frac{1}{\sqrt{200}}\approx 0.019$
$gyronoise=0.2088\ast \frac{1}{\sqrt{200}}\approx 0.015$

可以看出与预期值保持一致。

步骤4：用matlab脚本画Allan曲线

用scripts文件夹下的matlab函数绘制曲线。
陀螺仪Allan方差图如下图所示：

加速度计Allan方差图如下图所示：

2 使用kalibr_allan完成allan标定

步骤1：编译bagconver功能包

由于本人电脑中的matlab装在了自定义目录，所以CMakeLists中需要指定matlab根目录。

set(Matlab_ROOT_DIR /home/tiandao/Polyspace/R2020a)

之后进行编译即可。

步骤2：生成imu.bag并转为imu.mat

这里使用上面已经生成好的imu.bag。
执行：

rosrun bagconvert bagconvert /home/ximing/imu.bag imu

从而获得imu.mat文件。

步骤3：对mat文件进行Allan方差分析

用kalibr_allan中matlab中的函数进行Allan方差分析，能够得到以下两张图。
陀螺仪Allan方差图：

加速度计Allan方差图：

对比能够发现与预期结果极为接近。

第二题：将仿真代码中的欧拉积分替换为中值积分

对于数值积分中的欧拉积分、中值积分以及龙格库塔积分等，实质上在于斜率选取不同从而导致精度方面的差异。其中，欧拉积分将此时刻的测量值作为斜率，进行积分；而中值积分则采用此时刻与下一时刻测量值之平均作为斜率。从这个意义上看，中值积分的精度是明显比欧拉积分要高的。

下面的代码中展示了欧拉积分与中值积分的不同：

		Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half =  (imupose.imu_gyro + imupose_last.imu_gyro) * dt /4.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();
        
        /// imu 动力学模型 欧拉积分
//        Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gw
//        Qwb = Qwb * dq;
//        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
//        Vw = Vw + acc_w * dt;
        
        /// 中值积分
        Eigen::Vector3d acc_w = (Qwb * dq * imupose.imu_acc + Qwb * imupose_last.imu_acc)/2.0 + gw;
        Qwb = Qwb * dq;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;

用python进行测试，绘制轨迹图像。

• 欧拉积分
  

• 中值积分
  
  此外，运行draw_points.py能够输出一个持续20s的视频，该文件只能使用python2运行。下图为某一时刻的截图：
  

第三题：阅读论文

前两题均为给定连续轨迹曲线时，对轨迹求导获得相关数据；但很多时候没有连续的轨迹曲线，只有离散的位姿点，这时需要通过B-Spline方法对离散点进行插值，从而获得连续的轨迹曲线，进而求导获得相关数据。本论文解决的就是后面的问题。

论文推导内容如下：