《视觉SLAM进阶：从零开始手写VIO》第二讲作业-IMU仿真、MU imu_utils标定
《视觉SLAM进阶：从零开始手写VIO》第二讲作业-IMU仿真、MU imu_utils标定
作业题目：
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
因为有脚本文件build.sh，或者
chmod +x build.sh
./build.sh
cd ../bin
./data_gen
运行完可执行文件data_gen后，会在bin目录下生成以下txt文件
imu_pose_noise.txt ，imu_pose.txt ，imu_int_pose_noise.txt， imu_int_pose.txt
1.1.1 欧拉积分法
（1）公式
（2）代码
for (int i = 1; i < imudata.size(); ++i) {
MotionData imupose = imudata[i];//imu的位姿
//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]； 即qwb′ = qwb ⊗ [1 1/2 * ω∆t ]
Eigen::Quaterniond dq;
// 欧拉积分
Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0; // qwb′ = qwb ⊗ [1 1/2 * ω∆t ] 的 1/2 * ω∆t 部分，ω是角速度的真实值imudata[i].imu_gyro
// 中值积分
// Eigen::Vector3d dtheta_half = ( imudata[i - 1].imu_gyro + imupose.imu_gyro)/ 2.0 * dt / 2.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;
（3）轨迹对比图
使用python-tool下的Python脚本，可以绘制出点，IMU轨迹等
cd ../python_tool
python3 draw_trajctory.py
但是我运行发现报错：python3: can’t open file ‘draw_trajctory.py’: [Errno 2] No such file or directory
原因是：下载的代码是draw_trajcory.py少了字幕t，需要改为draw_trajctory.py。
运行结果如图：
1.1.2 中值积分法
（1）公式
（2）代码
这里有几点需要特别说明：
a）源程序中的循环计数变量i是从1开始的，而不是从0开始的；
b）与理论中的公式相比，源程序中给出的数据是已经去掉bias之后的量；
c）必须先更新姿态矩阵，因为求中值法中求acc时，需要用到前后两个时刻的姿态四元数；
d）由陀螺数据构造出的四元数不要忘了归一化；
e）最好是在源程序的基础上做最轻微的改动。
for (int i = 1; i < imudata.size(); ++i) {
MotionData imupose = imudata[i];//imu的位姿
//delta_q = [1 , 1/2 * thetax , 1/2 * theta_y, 1/2 * theta_z]； 即qwb′ = qwb ⊗ [1 1/2 * ω∆t ]
Eigen::Quaterniond dq;
// 欧拉积分
//Eigen::Vector3d dtheta_half = imupose.imu_gyro * dt /2.0; // qwb′ = qwb ⊗ [1 1/2 * ω∆t ] 的 1/2 * ω∆t 部分，ω是角速度的真实值imudata[i].imu_gyro
// 中值积分
Eigen::Vector3d dtheta_half = ( imudata[i - 1].imu_gyro + imupose.imu_gyro)/ 2.0 * dt / 2.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;
// /// imu 动力学模型 中值积分
Eigen::Vector3d acc_w = (Qwb*(imudata[i - 1].imu_acc) + gw + Qwb*dq*(imupose.imu_acc) + gw) / 2.0;
Qwb = Qwb * dq;
Vw = Vw + acc_w * dt;
Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
（3）轨迹对比图
./build.sh
cd ../bin
./data_gen
cd ../python_tool
python3 draw_trajctory.py
对比两个仿真结果图，欧拉法最后两个曲线明显分离，而中值法最后两个曲线完全重合在一起。说明中值法的精度高于欧拉法。
简化运行命令：
命令为了方便以后的运行，修改build.sh文件
修改build.sh文件
#!/bin/bash
echo "Configuring and building ..."
if [ ! -d "build" ]; then
mkdir build
fi
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j4
增加以下代码：
cd ../bin
./data_gen
cd ../python_tool
python3 draw_trajctory.py
以后只需要运行./build.sh就可以看见轨迹对比图，或者运行 bash build.sh也可以，且方便cout的调试。
运行draw_points.py代码，可得下图：
（将build.sh的python3 draw_trajctory.py改为python3draw_points.py）
./build.sh
有报错
Traceback (most recent call last):
File "draw_points.py", line 30, in <module>
x.append( numbers_float[0] )
TypeError: 'map' object is not subscriptable
**解决办法：**使用list包装出现问题的对象，list（），共有4处要修改。
numbers_float = map(float, odom) # 转化为浮点数
numbers_float = list(numbers_float)
可以看出IMU是绕房子做Z轴的正弦运动
1.2仿真思路
指定轨迹方程,求一阶导得到速度, 角速度,求二阶导得到加速度。对加速度、角速度添加高斯白噪声和bias 的随机游走噪声，得到仿真数据。
1.3运动模型
在该仿真代码中，在imu.cpp中定义了IMU的运动模型：
// 指定轨迹方程，求一阶导得到速度,角速度，求二阶导得到加速度
// 并将求得的 速度 角速度 加速度 时间戳 旋转矩阵 平移矩阵 赋值给结构体MotionData对象data的成员变量
MotionData IMU::MotionModel(double t)
{
MotionData data;//结构体对象data
// param
float 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 平移
// twb: body frame in world frame
// 指定轨迹方程，求一阶导得到速度,角速度，求二阶导得到加速度
Eigen::Vector3d position( ellipse_x * cos( K * t) + 5, ellipse_y * sin( K * t) + 5, z * sin( K1 * K * t ) + 5); //t=0 起点 (20,5,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 frame
double 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;
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]
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 frame
Eigen::Vector3d imu_gyro = eulerRates2bodyRates(eulerAngles) * eulerAnglesRates; // euler rates trans to body gyro //角速度
Eigen::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 = gs //加速度
// struct MotionData的成员赋值
data.imu_gyro = imu_gyro;//陀螺仪 欧拉角速度 IMU的角速度真实值
data.imu_acc = imu_acc;//加速度计 加速度 IMU的加速度真实值
data.Rwb = Rwb; //旋转矩阵
data.twb = position; //平移矩阵
data.imu_velocity = dp; //速度
data.timestamp = t;//时间戳
return data;
}
1.4添加噪声
设定,加速度的高斯白噪声设定为 0.019, 陀螺仪的高斯白噪声为 0.015. 加速度 bias 的随机游走噪声设定为 5e−4 ,陀螺仪的 bias 随机游走噪声设定为 5e−5 。
param.h定义了参数
class Param{
public:
Param();
// time
int imu_frequency = 200;
int cam_frequency = 30;
double imu_timestep = 1./imu_frequency; // imu的采样时间 0.005s
double cam_timestep = 1./cam_frequency; // 相机的采样时间 1/30s
double t_start = 0.;//起始时间
double t_end = 20; // 20 s 结束时间
// noise
double gyro_bias_sigma = 1.0e-5;//陀螺仪的 bias 随机游走噪声
double acc_bias_sigma = 0.0001;//加速度 bias 的随机游走噪声
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声
double pixel_noise = 1; // 1 pixel noise
// cam f
double fx = 460;
double fy = 460;
double cx = 255;
double cy = 255;
double image_w = 640;
double image_h = 640;
// 外参数
Eigen::Matrix3d R_bc; // cam to body
Eigen::Vector3d t_bc; // cam to body
};
//向imu数据data里添加离散时间的高斯白噪声noise和离散时间的bias随机游走，并更新 加速度和角速度
void IMU::addIMUnoise(MotionData& data)
{
std::random_device rd;//非确定性(真)随机数生成器
std::default_random_engine generator_(rd());//创建随机数引擎对象 传入一个随机数种子
std::normal_distribution<double> noise(0.0, 1.0);//随机数的正态连续分布。
Eigen::Vector3d noise_gyro(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值 方差为 1 的白噪声
// cout <<"noise_gyro:" << endl;
// cout<
// noise_gyro:
// -1.35678
// -0.33964
// -0.199193
// noise_gyro:
// 0.264621
// -0.522613
// 0.0166623
Eigen::Matrix3d gyro_sqrt_cov = param_.gyro_noise_sigma * Eigen::Matrix3d::Identity();// 陀螺仪的高斯白噪声协方差
// gyro_sqrt_cov:
// 0.015 0 0
// 0 0.015 0
// 0 0 0.015
// cout <<"gyro_sqrt_cov:" << endl;
// cout<
// 忽略 scale 的影响，只考虑白噪声和 bias 随机游走
// 角速度 w = w + 离散时间的高斯白噪声noise + 离散时间的bias随机游走
// 高斯白噪声的连续时间到离散时间之间差一个 1/√∆t ,√∆t是采样时间
// 离散时间的高斯白噪声noise: nd[k] = sigema/√∆t * w[k] = gyro_sqrt_cov / sqrt( param_.imu_timestep ) * noise_gyro
data.imu_gyro = data.imu_gyro + gyro_sqrt_cov * noise_gyro / sqrt( param_.imu_timestep ) + gyro_bias_; //3个轴的角速度一起算
// IMU的角速度测量值 imu_gyro
// cout <<"data.imu_gyro:" << endl;
// cout<
// data.imu_gyro:
// -0.380758
// 0.118272
// 0.0782825
// data.imu_gyro:
// -0.275656
// 0.115311
// 0.618848
Eigen::Vector3d noise_acc(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值 方差为 1 的白噪声
Eigen::Matrix3d acc_sqrt_cov = param_.acc_noise_sigma * Eigen::Matrix3d::Identity();// 加速度计的高斯白噪声协方差
// 忽略 scale 的影响，只考虑白噪声和 bias 随机游走
// 加速度 a = a + 离散时间的高斯白噪声noise + 离散时间的bias随机游走
// 离散时间的高斯白噪声noise: nd[k] = sigema/√∆t * w[k] = acc_sqrt_cov / sqrt( param_.imu_timestep ) * noise_acc
// IMU的加速度测量值 imu_acc
data.imu_acc = data.imu_acc + acc_sqrt_cov * noise_acc / sqrt( param_.imu_timestep ) + acc_bias_;
// gyro_bias update 陀螺仪 离散时间的bias随机游走 更新
Eigen::Vector3d noise_gyro_bias(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值 方差为 1 的白噪声
// bias 随机游走的噪声方差从连续时间到离散之间需要乘以 √∆t
// 离散时间的bias随机游走gyro_bias_ ：bd[k] = bd[k-1] + sigema * √∆t * w[k] = param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
gyro_bias_ += param_.gyro_bias_sigma * sqrt(param_.imu_timestep ) * noise_gyro_bias;
data.imu_gyro_bias = gyro_bias_;// 陀螺仪 离散时间的bias随机游走 更新
// acc_bias update 加速度计 离散时间的bias随机游走 更新
Eigen::Vector3d noise_acc_bias(noise(generator_),noise(generator_),noise(generator_));//随机生成服从正态分布的值 方差为 1 的白噪声
// bias 随机游走的噪声方差从连续时间到离散之间需要乘以 √∆t
// 离散时间的bias随机游走acc_bias_ ：bd[k] = bd[k-1] + sigema * √∆t * w[k] = param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
acc_bias_ += param_.acc_bias_sigma * sqrt(param_.imu_timestep ) * noise_acc_bias;
data.imu_acc_bias = acc_bias_;// 加速度计 离散时间的bias随机游走 更新
}
2 ROS版的IMU仿真代码
2.1 下载编译vio_data_simulation的ros版本
创建catkin工作空间：先创建catkin工作空间，依次输入如下指令。
mkdir -p vio_sim_ws/src
cd vio_sim_ws/src
catkin_init_workspace //初始化工作空间
下载vio_data_simulation
终端输入如下指令，git下载vio_data_simulation包：
git clone https://github.com/HeYijia/vio_data_simulation
cd vio_data_simulation
git checkout ros_version
git checkout ros_version 命令是切换到 ros_version 分支
cd ..
cd ..
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);
我没改，默认的路径是： HOME/cgm/imu.bag
const std::string home_path = getenv("HOME"); const std::string bag_path = home_path + "/imu.bag"; rosbag::Bag bag; bag.open(bag_path, rosbag::bagmode::Write);
然后，启动节点，生成imu.bag，这个文件有1.1GB。
source devel/setup.bash //设置ros环境
rosrun vio_data_simulation vio_data_simulation_node
至此，我们得到了一个IMU仿真数据的ROS bag包。接下来利用Allan方差工具对其进行标定。
ROS省略source devel/setup.bash的方法
为了不每次运行程序的时候都source一次devel文件夹里的setup.bash，可以打开主目录 按下Crtl+h 显示隐藏文件，双击打开bashrc文件，在最后加入
source /home/############/catkin_ws/devel/setup.bash，其中###部分为你的计算机账户，保存后再重新打开终端，显示无错误即可。
3 使用Allan方差工具标定
主要是使用imu_utils和kalibr_allan进行标定
3.1 使用imu_utils标定
3.1.1 安装code_utils
我们的 code_utils 和 imu_utils 都安装在 imu-calibration/src文件夹下面。
首先，安装依赖：
sudo apt-get install libdw-dev
先code_utils，再编译code_utils
mkdir -p imu-calibration/src
cd imu-calibration/src
git clone https://github.com/gaowenliang/code_utils.git
cd ..
catkin_make
3.1.2 编译code_utils的报错解决
错误1：找不到Eigen
我们一般通过以下命令安装Eigen：
sudo apt-get install libeigen3-dev
**解决办法：**这样Eigen就默认安装在/usr/include/eigen3，需要在/home/cgm/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
**解决办法：**把文件/home/cgm/imu-calibration/srccode_utils/src/sumpixel_test.cpp中的#include "backward.hpp"改成#include "code_utils/backward.hpp"即可。
**错误3：**std::ofstream未定义
/home/cgm/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
以下错误4 5 6是由于opencv 版本导致的
**错误4：**error: ‘CV_LOAD_IMAGE_UNCHANGED
/home/cgm/imu-calibration/src/code_utils/src/mat_io_test.cpp:33:47: error: ‘CV_LOAD_IMAGE_UNCHANGED’ was not declared in this scope
33 | Mat img1 = imread( “/home/gao/IMG_1.png”, CV_LOAD_IMAGE_UNCHANGED );
**解决办法：**打开/home/cgm/imu-calibration/src/code_utils/src/mat_io_test.cpp 点击 “搜索”，输入“CV_LOAD_IMAGE_UNCHANGED”，替换成-1
替换成cv::IMREAD_UNCHANGED也可以
表示读取原图
Mat img1 = imread( "/home/gao/IMG_1.png", CV_LOAD_IMAGE_UNCHANGED);
改为
Mat img1 = imread( "/home/gao/IMG_1.png", -1 );
或者
Mat img1 = imread( "/home/gao/IMG_1.png", cv::IMREAD_UNCHANGED );
**错误5：**error: ‘CV_LOAD_IMAGE_GRAYSCALE
/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp:84:47: error: ‘CV_LOAD_IMAGE_GRAYSCALE’ was not declared in this scope
84 | Mat img1 = imread( “/home/gao/IMG_1.png”, CV_LOAD_IMAGE_GRAYSCALE );
| ^~~~~~~~~~~~~~~~~~~~~~~
**解决办法：**打开/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp 点击 “搜索”，输入“CV_LOAD_IMAGE_GRAYSCALE”，替换成0
替换成cv::IMREAD_GRAYSCALE也可以
表示读取灰度图
原因：OPENCV的API已改
我的opencv是4.4.5版本，查看ubuntu下的OpenCV安装版本：
pkg-config opencv --modversion
改为了
https://docs.opencv.org/3.4/da/d0a/group__imgcodecs__c.html#gga8420dba71b9cc240cf981b0bef892004a7dc9fd0941c4a96edd4dcbacdc98518c
https://docs.opencv.org/3.4/d8/d6a/group__imgcodecs__flags.html
报错6： error: ‘CV_MINMAX’
/home/cgm/imu-calibration/src/code_utils/src/sumpixel_test.cpp:94:35: error: ‘CV_MINMAX’ was not declared in this scope; did you mean ‘CV_MMX’?
94 | normalize( img, img2, 0, 255, CV_MINMAX );
**解决办法：**把CV_MINMAX改成NORM_MINMAX
3.1.3 安装imu_utils
先下载imu_utils，再编译imu_utils
cd imu-calibration/src
git clone https://github.com/gaowenliang/imu_utils.git
cd ..
catkin_make
至此，code_utils和imu_utils都安装好了。
ROS省略source devel/setup.bash的方法
将source /home/cgm/imu-calibration/devel/setup.bash 添加到.bashrc文件的末尾
3.1.3 标定
-
查看imu.bag的信息
这里的IMU数据就使用我们上面生成的imu.bag。当然作者也提供了几个包，可以先测试一下。
可以看到，我们之前生成的img.bag，topic为imu（这个信息后文中的.launch文件需要用到。）, 大概四个小时的时长。
-
写launch文件
可以参考**/home/cgm/imu-calibration/src/imu_utils/launch**下作者写的几个launch文件，根据自己imu的topic和name进行修改：
进入如图所示路径，复制16448.launch文件，将文件名改为my.launch，打开 my.launch，将其中的imu_topic与imu_name分别改为imu（前文提到过）和mytest。
还可一看到data的存储路径是imu_utils/data文件夹里，可以先清空data文件夹，或者新建个文件夹暂时全部移动过去，这样能更清楚地明白生成了哪些文件。
-
回放数据包，标定结果
（1）启动roscore
roscore
（2）进入imu-calibration文件夹里，新打开一个terminal窗口，以500倍速播放imu.bag。注意我的imu.bag在 HOME/cgm/imu.bag
rosbag play -r 500 ../imu.bag
（3）进入imu-calibration文件夹里，新打开一个terminal窗口，运行我们的my.launch
roslaunch imu_utils my.launch
我这里不要再配置环境命令了 source ./devel/setup.bash ，见ROS省略source devel/setup.bash的方法
执行结束之后，新生成的17个文件
（4）打开文件mytest_imu_param.yaml，部分文件内容如下，这就是标定结果。
这里应该格外注意单位rad/s和m/s^2。
%YAML:1.0
---
type: IMU
name: mytest
Gyr:
unit: " rad/s"
avg-axis:
gyr_n: 2.1174628193741066e-01
gyr_w: 8.5047491324959210e-04
x-axis:
gyr_n: 2.1124530533883673e-01
gyr_w: 7.4517858725206359e-04
y-axis:
gyr_n: 2.1327773065877687e-01
gyr_w: 9.6551201149405936e-04
z-axis:
gyr_n: 2.1071580981461840e-01
gyr_w: 8.4073414100265334e-04
Acc:
unit: " m/s^2"
avg-axis:
acc_n: 2.6979648977867060e-01
acc_w: 3.8990219990271773e-03
x-axis:
acc_n: 2.6807138890223103e-01
acc_w: 3.9648009709693111e-03
y-axis:
acc_n: 2.7120534163617294e-01
acc_w: 3.7621137218341555e-03
z-axis:
acc_n: 2.7011273879760778e-01
acc_w: 3.9701513042780636e-03
imu_utils默认给出了IMU的白噪声（ “white noise”）和零偏不稳定性（ “bias Instability”）这两个参数的标定结果。
而我们设置的噪声参数以及kalibr_allan标定的结果都是IMU的白噪声（ “white noise”）和传感器Bias的随机游走（Rate Random Walk）。至于如何用imu_utils给出传感器Bias的随机游走（Rate Random Walk）不在这里论述。
虽然我们得到了标定结果，但这个标定结果并不是我们最终的结果。现在得到的结果的单位是rad/s和m/s^2，而老师param.h代码中给出的单位如下面代码中最后两行，后面多了**/sqrt(hz)**。上面的图Units一栏也能说明。
噪声是个能量概念或者说功率概念，我们还要把标定得到的参数归一化到每单位sqrt(hz)尺度下。
从param.h代码中可以看出IUM的频率是200Hz，转换只需要每个量除以sqrt（200）,转换后的值与程序中的设定值比较如下，因为没得到给定的传感器Bias的随机游走（Rate Random Walk），这里只比较IMU的白噪声，可以看出标定效果不错。
int imu_frequency = 200; //IUM的频率是200Hz
double gyro_noise_sigma = 0.015; // rad/s * 1/sqrt(hz) 陀螺仪的高斯白噪声
double acc_noise_sigma = 0.019; // m/(s^2) * 1/sqrt(hz) 加速度的高斯白噪声
unit: rad/s rad/s * 1/sqrt(hz) 给定值
gyr_n: 2.1174628193741066e-01 0.0149727 0.015
gyr_w: 8.5047491324959210e-04 6.01377e-05
unit: m/s^2 m/(s^2) * 1/sqrt(hz)
acc_n: 2.6979648977867060e-01 0.0190775 0.019
acc_w: 3.8990219990271773e-03 0.000275702
可以在程序中增加代码进行简单计算验证：
cout << "2.1174628193741066e-01 / sqrt(200) = " << 2.1174628193741066e-01 / sqrt(200) << endl;
cout << "8.5047491324959210e-04 / sqrt(200) = " << 8.5047491324959210e-04 / sqrt(200) << endl;
cout << "2.6979648977867060e-01 / sqrt(200) = " << 2.6979648977867060e-01 / sqrt(200) << endl;
cout << "3.8990219990271773e-03 / sqrt(200) = " << 3.8990219990271773e-03 / sqrt(200) << endl;
结果：
2.1174628193741066e-01 / sqrt(200) = 0.0149727
8.5047491324959210e-04 / sqrt(200) = 6.01377e-05
2.6979648977867060e-01 / sqrt(200) = 0.0190775
3.8990219990271773e-03 / sqrt(200) = 0.000275702
3.2 使用MATLAB画Allan方差图
下面是使用MATLAB画Allan方差图(window系统下完成），将前面提到的文件夹data（新增17个文件）和imu_utils工具给定的scripts文件夹放到同一目录之下：
3.2.1 生成陀螺仪的Allen方差图
这里以生成陀螺仪的Allen方差图为例，修改scripts中的draw_allan.m代码（所有的A3替换成mytest）：
clear
close all
dt = dlmread('../data/data_mytest_gyr_t.txt');
data_x = dlmread('../data/data_mytest_gyr_x.txt');
data_y= dlmread('../data/data_mytest_gyr_y.txt');
data_z = dlmread('../data/data_mytest_gyr_z.txt');
data_draw=[data_x data_y data_z] ;
data_sim_x= dlmread('../data/data_mytest_sim_gyr_x.txt');
data_sim_y= dlmread('../data/data_mytest_sim_gyr_y.txt');
data_sim_z= dlmread('../data/data_mytest_sim_gyr_z.txt');
data_sim_draw=[data_sim_x data_sim_y data_sim_z] ;
figure
loglog(dt, data_draw , 'o');
% loglog(dt, data_sim_draw , '-');
xlabel('time:sec');
ylabel('Sigma:deg/h');
% legend('x','y','z');
grid on;
hold on;
loglog(dt, data_sim_draw , '-');
运行，得到陀螺仪的Allan方差图如下：
3.2.2 生成加速度计的Allen方差图
同理，将文件中的所有gyr替换成acc,得到加速度的Allan方差图如下：
clear
close all
dt = dlmread('../data/data_mytest_acc_t.txt');
data_x = dlmread('../data/data_mytest_acc_x.txt');
data_y= dlmread('../data/data_mytest_acc_y.txt');
data_z = dlmread('../data/data_mytest_acc_z.txt');
data_draw=[data_x data_y data_z] ;
data_sim_x= dlmread('../data/data_mytest_sim_acc_x.txt');
data_sim_y= dlmread('../data/data_mytest_sim_acc_y.txt');
data_sim_z= dlmread('../data/data_mytest_sim_acc_z.txt');
data_sim_draw=[data_sim_x data_sim_y data_sim_z] ;
figure
loglog(dt, data_draw , 'o');
% loglog(dt, data_sim_draw , '-');
xlabel('time:sec');
ylabel('Sigma:deg/h');
% legend('x','y','z');
grid on;
hold on;
loglog(dt, data_sim_draw , '-');
包括前面我们留下的传感器Bias的随机游走（Rate Random Walk）在内的所有IMU参数，都可以通过分析这Allan方差图得到。
具体可以参考论文：UCAM-CL-TR-696
Allan Variance. “Noise Analysis for Gyroscopes”. In: Freescale Semiconductor Document Number: AN5087 Application Note Rev. 0 2 (2015).]:
网址：chrome-extension://ikhdkkncnoglghljlkmcimlnlhkeamad/pdf-viewer/web/viewer.html?file=https%3A%2F%2Ftelesens.co%2Fwp-content%2Fuploads%2F2017%2F05%2FAllanVariance5087-1.pdf#=&zoom=190
例如：其中的19页
[5] IEEE Std 962-1997 (R2003) Standard Specification Format Guide and Test Procedure for Single-Axis Interferometric Fiber Optic Gyros, Annex C. IEEE, 2003.
3.2.3 Allan方差图读取误差系数
Allan方差法可用于5种随机误差的标定：
量化噪声(Quantization Noise)：误差系数为Q，Allan方差双对数曲线上斜率为-1的直线延长线与t=10°的交点的纵坐标读数为√3 Q；
角度随机游走(Angle Random Walk)：其误差系数N，Allan方差双对数曲线上斜率为-1/2的线的延长线与t=10°交点的纵坐标读数即为N；
零偏不稳定性(Bias Instability)：其误差系数B，Allan方差双对数曲线上斜率为0的线的延长线与t=10°交点的纵坐标读数为√((2 ln2)/π) B，一般常取底部平坦区的最小值或取t=101或t=102处的值；
角速率随机游走(Rate Random Walk)：其误差系数K，斜率为+1/2的线的延长线与t=10°交点的纵坐标读数为K/√3；
角速率斜坡(Rate Ramp)：其误差系数R，斜率为+1的线的延长线与t=10°交点的纵坐标读数为R/√2；
4 使用kalibr_allan工具标定
因为am=a+na+ba,即测量值=真实值+白噪声+bias
所以需要标定白噪声和bias。
allan方差斜率中用到的是①陀螺仪加速度计的随机游走，对应的斜率是-0.5；②bias的随机游走，对应的斜率是0.5；故allan方差需要是钩子形状，才能成功标定。
建议去看一下 https://github.com/rpng/kalibr_allan.git 的Readme.md文件
4.1 安装kalibr_allan
cd imu-calibration/src
git clone https://github.com/rpng/kalibr_allan.git
cd ..
catkin_make
安装Kalibr，在执行catkin_make时出现错误The following variables are used in this project, but they are set to NO
安装Kalibr 的时候，bagconvert设置的默认路径找不到matlab这样导致安装失败出现以下情况：
CMake Error: The following variables are used in this project, but they are set to NOTFOUND.
Please set them or make sure they are set and tested correctly in the CMake files:
MATLAB_ENG_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_INCLUDE_DIR (ADVANCED)
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
used as include directory in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MAT_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MEX_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_MX_LIBRARY (ADVANCED)
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
MATLAB_UT_LIBRARY
linked by target “bagconvert” in directory /home/cgm/imu-calibration/src/kalibr_allan/bagconvert
– Configuring incomplete, errors occurred!
See also “/home/cgm/imu-calibration/build/CMakeFiles/CMakeOutput.log”.
See also “/home/cgm/imu-calibration/build/CMakeFiles/CMakeError.log”.
make: *** [Makefile:824：cmake_check_build_system] 错误 1
Invoking “make cmake_check_build_system” failed
原因：缺少matlab
4.2 安装matlab
我极度不想安装matlab-_-，就不做了。
后面的参考这个吧
从零手写VIO（二）——IMU仿真、MU imu_utils或kalibr_allan标定