VIO 第二讲 IMU传感器&IMU预积分与视觉信息融合 {课后作业}

解答：
一、生成allan方差标定曲线
1、安装 ROS
系统版本是 ubuntu 16.04。具体安装网上一大堆！
2、建立catkin工作区

mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
cd ~/catkin_ws/
catkin_make

此时在 catkin_ws 目录下自动生成了 devel 和 build 文件夹。
3. 编译 imu_utils 和 code_utils

首先编译code_utils文件，否则会出现错误。其次编译imu_utils文件。

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/code_utils
cd ~/catkin_ws
catkin_make

cd ~/catkin_ws/src
git clone https://github.com/gaowenliang/imu_utils
cd ~/catkin_ws
catkin_make

4.生成 IMU 仿真数据
课程提供的 vio_data_simulation-ros_version 代码可以用来生产需要的 bag 文件。
main 函数下第三行 rosbag::bag 初始化改为:

rosbag::Bag bag("/home/xxxxx/imu.bag",rosbag::BagMode::Write);
//路径修改成自己的路径

写入语句中修改一下生成包的 topic:

bag.write("/imu", time_now, imu_data);

运行主函数即可生成 bag 文件,生成的包放到了自己设定目录下。设置完成后，运行

roscore
cd ~/catkin_ws/devel/lib/vio_data_simulation
rosrun vio_data_simulation vio_data_simulation_node

5. Allan 方差标定
在imu_utils 文件夹的launch文件夹下，新建一个launch文件，里面内容：（注意<“imu_topic” “imu_name” 的值）

<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= "imutest"/>
        <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>
</launch>

打开三个终端

• 第一个运行： roscore
• 第二个运行:播放上次采集的bag包：rosbag play -r 200 imu.bag
• 第三个终端： source devel/setup.bash 文件后运行:roslaunch roslaunch imu_utils imutest.launch

然后生成的数据在imu_utils/data/下.

接下来,我们去画出来这些方差图,在scripts下有很多matlab的脚本文件.

因为我们这里只使用了一个IMU的数据,所以修改matlab文件中的代码,如果你想比较多个,也修改一下即可.没有matlab的话，可以写一个python脚本：

import numpy as np
import matplotlib.pyplot as plt
import pandas as pd
dt=pd.read_csv('/home/sxf/Slam_ws/VIO/CH2/src/imu_utils/data/data_imutest_sim_gyr_t.txt')
dt=dt.values
data_x = pd.read_csv('/home/sxf/Slam_ws/VIO/CH2/src/imu_utils/data/data_imutest_sim_gyr_x.txt');
data_x =data_x.values
data_y= pd.read_csv('/home/sxf/Slam_ws/VIO/CH2/src/imu_utils/data/data_imutest_sim_gyr_y.txt');
data_y=data_y.values
data_z = pd.read_csv('/home/sxf/Slam_ws/VIO/CH2/src/imu_utils/data/data_imutest_sim_gyr_z.txt');
data_z= data_z.values
data_draw=np.hstack((data_x,data_y,data_z))
y1=np.log10(data_x)
y2=np.log10(data_y)
y3=np.log10(data_z)
x=np.log10(dt)
ln1, = plt.plot(x, y1, color = 'red', linewidth = 2.0, linestyle = '-')
ln2, = plt.plot(x, y2, color = 'green', linewidth = 2.0, linestyle =':')
ln3, = plt.plot(x, y3, color = 'black', linewidth = 2.0, linestyle = '--')
plt.legend(handles=[ln1, ln2,ln3], labels=['gyr-x', 'gyr-y','gyr-z'],
loc='lower right')
plt.show()

6、运行结果
原始噪声值
绘制加速度acc计艾伦方差曲线:

绘制陀螺仪gyr艾伦方差曲线如下:

二、中值积分

MotionData imupose = imudata[i-1];
MotionData imupose_next = imudata[i];
Eigen::Quaterniond dq;
Eigen::Vector3d dtheta_half = (imupose.imu_gyro +imupose_next.imu_gyro) * dt/4.0;
dq.w() = 1;dq.x() = dtheta_half.x();
dq.y() = dtheta_half.y();
dq.z() = dtheta_half.z();
// imu 动力学模型 欧拉积分
/// 中值积分
Eigen::Vector3d acc_wm = Qwb * (imupose.imu_acc) + gw;
Qwb = Qwb * dq;
Qwb.normalize();
acc_wm = (acc_wm + Qwb * (imupose_next.imu_acc)+gw)/2;
Vw = Vw + acc_wm * dt;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_wm;

在欧拉和中值积分中没有做归一化和有做归一化结果变化比较大。以下是结果分析
欧拉没有归一化

欧拉积分有归一化

中值积分没有归一化

中值积分有归一化

综上：即使是小量更新,四元数不做归一化累积起来也有一定误差,而添加归一化后的中值积分基本上完全拟合原函数。