在这里对从零手写VIO的第二次作业进行总结：
作业题目：

1仿真代码解析

仿真代码地址：https://github.com/HeYijia/vio_data_simulation
该github仓库中除了提供了普通版本的仿真代码外，还提供了ROS版本的仿真代码，用于生成img.bag，以供后面使用Allan方差工具进行标定。

1.1编译vio_data_simulation-master

首先是编译这个普通版本的仿真代码，就是按编译的常规套路来：

git clone https://github.com/HeYijia/vio_data_simulation
cd vio_data_simulation
mkdir build
cd build
camke ..
make
cd ../bin
./data_gen

这样就生成我们所需的：

imu_pose_noise.txt ，imu_pose.txt ，imu_int_pose_noise.txt， imu_int_pose.txt

等数据。

使用python-tool下的Python脚本，可以绘制处点，IMU轨迹等

cd ../python_tool
python draw_trajctory.py

1.2仿真思路

指定轨迹方程,求一阶导得到速度, 角速度,求二阶导得到加速度。对加速度角速度添加高斯白噪声和bias 的随机游走噪声，得到仿真数据。

1.3运动模型

在该仿真代码中，在imu.cpp中定义了IMU的运动模型：

MotionData IMU::MotionModel(double t)
{MotionData data;// paramfloat ellipse_x = 15;float ellipse_y = 20;float z = 1;           // z轴做sin运动float K1 = 10;          // z轴的正弦频率是x，y的k1倍float K = M_PI/ 10;    // 20 * K = 2pi 　　由于我们采取的是时间是20s, 系数K控制yaw正好旋转一圈，运动一周// translation world系下 因为IMU的position就是body系在world系下的坐标// twb:  body frame in world frameEigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5,  z * sin( K1 * K * t ) + 5);Eigen::Vector3d dp(- K * ellipse_x * sin(K*t),  K * ellipse_y * cos(K*t), z*K1*K * cos(K1 * K * t));              // position导数　in world framedouble K2 = K*K;Eigen::Vector3d ddp( -K2 * ellipse_x * cos(K*t),  -K2 * ellipse_y * sin(K*t), -z*K1*K1*K2 * sin(K1 * K * t));     // position二阶导数// Rotation double k_roll = 0.1;double k_pitch = 0.2;// body系下的角度Eigen::Vector3d eulerAngles(k_roll * cos(t) , k_pitch * sin(t) , K*t );   // roll ~ [-0.2, 0.2], pitch ~ [-0.3, 0.3], yaw ~ [0,2pi]// 对欧拉角进行求导，得到world系下的欧拉角速度Eigen::Vector3d eulerAnglesRates(-k_roll * sin(t) , k_pitch * cos(t) , K);      // euler angles 的导数//    Eigen::Vector3d eulerAngles(0.0,0.0, K*t );   // roll ~ 0, pitch ~ 0, yaw ~ [0,2pi]
//    Eigen::Vector3d eulerAnglesRates(0.,0. , K);      // euler angles 的导数Eigen::Matrix3d Rwb = euler2Rotation(eulerAngles);         // body frame to world frameEigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates;   //  euler rates trans to body gyroEigen::Vector3d gn (0,0,-9.81);                                   //  gravity in navigation frame(ENU)   ENU (0,0,-9.81)  NED(0,0,9,81)Eigen::Vector3d imu_acc = Rwb.transpose() * ( ddp -  gn );  //  Rbw * Rwn * gn = gsdata.imu_gyro = imu_gyro;   // body系下的角速度data.imu_acc = imu_acc;     // body系下的加速度data.Rwb = Rwb;             //IMU的位姿数据使用Twb进行存储data.twb = position;data.imu_velocity = dp;     // IMU的速度 world系data.timestamp = t;         // 时间戳return data;}

可以看到，指定的IMU轨迹方程Position比较简单，大致能想出它的样子，大概是一个“王冠”的样子它在xy面投影为圆心（-5，,5）的圆。同时IMU也在以自身坐标系下做旋转运动，以欧拉角表示其旋转，该旋转也是一个随时间变化的函数。

对Position进行一阶导和二阶导，分别得到IMU在世界坐标系下的速度和加速度。对Rotation进行一阶导，得到IMU在body系下的角速度。利用这些值，我们最终得到了IMU的MotionData：

	data.imu_gyro = imu_gyro;   // body系下的角速度data.imu_acc = imu_acc;     // body系下的加速度data.Rwb = Rwb;             //IMU的位姿数据使用Twb进行存储data.twb = position;data.imu_velocity = dp;     // IMU的速度 world系data.timestamp = t;         // 时间戳

1.4添加噪声

设定,加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015. 加速度 bias 的随机游走噪声设定为 5e−4 ,陀螺仪的 bias 随机游走噪声设定为 5e−5 。

void IMU::addIMUnoise(MotionData& data)
{std::random_device rd;std::default_random_engine generator_(rd());std::normal_distribution<double> noise(0.0, 1.0);// gyroEigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));// 生成一个高斯白噪声Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();  // 感觉这里可有可无// w = w + ng + bg, 其中ng = σd*n, n~N(0,1),σd = σ/sqrt(Δt)  ,bg会在下面单独更新data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_;// accEigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();// // a = a + na + ba, 其中na = σd*n, n~N(0,1),σd = σ/sqrt(Δt)  ,ba会在下面单独更新data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;// gyro_bias updateEigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));// bg = σd*n, n~N(0,1), σd = σ*sqrt(Δt)gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;data.imu_gyro_bias = gyro_bias_;// acc_bias updateEigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));// ba = σd*n, n~N(0,1), σd = σ*sqrt(Δt)acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;data.imu_acc_bias = acc_bias_;}

在这里我们只考虑了高斯白噪声和 bias 随机游走，需要注意两者的计算：

w = w + ng + bg, 其中ng = σdn, n~N(0,1),σd = σ/sqrt(Δt) , bg = σdn, n~N(0,1), σd = σsqrt(Δt)
a = a + na + ba, 其中na = σdn, n~N(0,1),σd = σ/sqrt(Δt), ba = σdn, n~N(0,1), σd = σsqrt(Δt)

因为IMU获取的数据都是离散数据，注意高斯白噪声方差从连续时间到离散时间需要乘以一个1/sqrt(Δt)，随机游走的噪声方差从连续时间到离散之间需要乘以一个sqrt(Δt)，Δt为传感器的采样时间。

至此我们获得了IMU的仿真数据，主要是IMU在body系下的位姿数据：
imu_pose_noise.txt
imu_pose.txt

1.5运动模型的离散积分

在实际应用中，因为IMU的频率远远大于相机频率，所以我们往往需要对IMU离散数据进行积分，使IMU数据与相机数据对齐。
这次就利用上面得到的IMU的离散仿真数据，使用欧拉积分和中值积分，比较两种积分的的效果。（通过比较积分后的轨迹imu_int_pose.txt是否和积分前的轨迹imu_pose.txt重合）

1.5.1欧拉积分

代码：

for (int i = 1; i < imudata.size(); ++i) {MotionData imupose = imudata[i];//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]Eigen::Quaterniond dq;Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt / 2.0;dq.w() = 1;dq.x() = dtheta_half.x();dq.y() = dtheta_half.y();dq.z() = dtheta_half.z();/// imu 动力学模型 欧拉积分Eigen::Vector3d acc_w = Qwb * (imupose.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gwQwb = Qwb * dq;Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;Vw = Vw + acc_w * dt;//　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次save_points << imupose.timestamp << " "<< Qwb.w() << " "<< Qwb.x() << " "<< Qwb.y() << " "<< Qwb.z() << " "<< Pwb(0) << " "<< Pwb(1) << " "<< Pwb(2) << " "<< Qwb.w() << " "<< Qwb.x() << " "<< Qwb.y() << " "<< Qwb.z() << " "<< Pwb(0) << " "<< Pwb(1) << " "<< Pwb(2) << " "<< std::endl;}

效果：

1.5.2中值积分

代码：

for (int i = 1; i < imudata.size(); ++i){MotionData imupose_pre = imudata[i-1];MotionData imupose_now = imudata[i];MotionData imupose_mean = imudata[i];imupose_mean.imu_gyro = (imupose_pre.imu_gyro + imupose_now.imu_gyro) / 2.0;Eigen::Quaterniond dq;Eigen::Vector3d dtheta_half = (imupose_mean.imu_gyro) * dt / 2.0;dq.w() = 1;dq.x() = dtheta_half.x();dq.y() = dtheta_half.y();dq.z() = dtheta_half.z();//　imu 动力学模型　参考svo预积分论文Eigen::Vector3d acc_w = Qwb * (imupose_pre.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gwQwb = Qwb * dq; //获得Qwb_k+1//Qwb+1Eigen::Vector3d acc_w1 = Qwb * (imupose_now.imu_acc) + gw;  // aw = Rwb * ( acc_body - acc_bias ) + gwacc_w = (acc_w + acc_w1) / 2.0;//a updateVw = Vw + acc_w * dt;//Pwb updatePwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;//　按着imu postion, imu quaternion , cam postion, cam quaternion 的格式存储，由于没有cam，所以imu存了两次save_points << imupose_mean.timestamp << " "<< Qwb.w() << " "<< Qwb.x() << " "<< Qwb.y() << " "<< Qwb.z() << " "<< Pwb(0) << " "<< Pwb(1) << " "<< Pwb(2) << " "<< Qwb.w() << " "<< Qwb.x() << " "<< Qwb.y() << " "<< Qwb.z() << " "<< Pwb(0) << " "<< Pwb(1) << " "<< Pwb(2) << " "<< std::endl;}

效果：

很明显，中值积分的效果要好于欧拉积分，当然，这是显而易见的，根据原理我们就能看出：

2ROS版的IMU仿真代码

2.1 下载编译

下载编译：

mkdir vio_sim_ws/src
cd src
git clone https://github.com/HeYijia/vio_data_simulation
git checkout ros_version
catkin_make

2.2 生成imu.bag

首先，打开catkin_ws_vio_data_simulation/src/vio_data_simulation-ros_version/src/gener_alldata.cpp，设置imu.bag的存储路径。

bag.open("/your-path/imu.bag", rosbag::bagmode::Write);

然后，启动节点，生成imu.bag。

source devel/setup.bash
rosrun vio_data_simulation vio_data_simulation_node

至此，我们得到了一个IMU仿真数据的ROS bag包。接下来利用Allan方差工具对其进行标定。

3使用Allan方差工具标定

主要是使用imu_utils和kalibr_allan进行标定。

3.1 使用imu_utils标定

3.1.1 安装im_utils

首先，安装依赖：

sudo apt-get install libdw-dev

先编译code_utils，然后再编译imu_utils

mkdir -p imu-calibration/src
cd imu-calibration/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
cd imu-calibration/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make

中间出现的问题：
错误1：找不到Eigen
我们一般通过以下命令安装Eigen：

sudo apt-get install libeigen3-dev

这样Eigen就默认安装在/usr/include/eigen3，需要在/home/jlg/imu-calibration/src/code_utils/CMakeLists.txt中注释掉find_package(Eigen3 REQUIRED)，然后添加：

include_directories(/usr/include/eigen3)

错误2：找不到backward.hpp

atal error: backward.hpp: No such file or directory

把文件code_utils/src/sumpixel_test.cpp中的#include "backward.hpp"改成#include "code_utils/backward.hpp"即可。

错误3：std::ofstream未定义

/home/***/imu-calibration/src/imu_utils/src/imu_an.cpp:69:19: error:
aggregate ‘std::ofstream out_t’ has incomplete type and cannot be
defined

打开文件imu_utils/src/imu_an.cpp，添加：

#include <fstream>

这里参考了：https://blog.csdn.net/learning_tortosie/article/details/102415313
事实上，可能是运气好，我没有碰到这些问题，但是还是记录一下，以防万一。

3.1.2 标定

1. 采集IMU数据
   这里的IMU数据就使用我们上面生成的imu.bag。当然作者也提供了几个包，可以先测试一下。
   
   可以看到，我们之前生成的img.bag，topic为imu, 大概大概四个小时的时长。
2. 写launch文件
   可以参考launch文件夹下作者写的几个launch文件，根据自己imu的topic和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= "mems"/><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>

比如我这里就修改了imu_topic所对应的value：/imu，imu_name所对应的value：mems，另外要记得把launch文件名改成imu_name，我这里就是mems.launch。

3. 生成Allan方差数据
   启动刚才的launch文件

cd imu-calibration
source./devel/setup.bash
roslaunch imu_utils mems.launch

新打开一个terminal窗口，以200倍速播放imu.bag

rosbag play -r 200 imu.bag

要注意，这里 img.bag要带上你存放的路径：yourpath/imu.bag

过一段时间（15s左右？），就会在data文件夹下生成一些数据：

以及一个mems_imu_param.yaml：

%YAML:1.0
---
type: IMU
name: mems
Gyr:unit: " rad/s"avg-axis:gyr_n: 2.1221203233760574e-01gyr_w: 8.9467924142534435e-04x-axis:gyr_n: 2.1046384179740413e-01gyr_w: 8.0473827758366584e-04y-axis:gyr_n: 2.1360563530391058e-01gyr_w: 9.2214122848867288e-04z-axis:gyr_n: 2.1256661991150250e-01gyr_w: 9.5715821820369432e-04
Acc:unit: " m/s^2"avg-axis:acc_n: 2.6764053623067113e-01acc_w: 3.6174135533268148e-03x-axis:acc_n: 2.6811296473220231e-01acc_w: 3.6652237104732588e-03y-axis:acc_n: 2.6789918945541225e-01acc_w: 3.7987720199202969e-03z-axis:acc_n: 2.6690945450439874e-01acc_w: 3.3882449295868896e-03

4. 绘制Allan方差图
   修改 draw_allan.m中文件路径
   
   将这几个路径都改为刚刚生成的数据的路径，然后运行drawn_allan.m
   
   需要注意的是，纵坐标的单位是deg/h。
   对该图的理解，可以参考：
   https://www.cl.cam.ac.uk/techreports/UCAM-CL-TR-696.pdf
   https://www.vectornav.com/support/library/gyroscope
   对图进行分析即可得到加速度和角速度的高斯白噪声和游走Bias误差
   t =1 的时候索对应的纵坐标值就是高斯白噪声
   t=3的时候对应的纵坐标就是游走Bias误差。