第一题：设置IMU仿真代码中的不同参数，并用Allan方差进行标定
1 使用 imu_utils 完成 allan 标定
步骤1：ros下编译步骤2：执行vio_data_simulation_node，生成imu.bag步骤3：播放imu.bag，并用imu_utils接收步骤4：用matlab脚本画Allan曲线 2 使用kalibr_allan完成allan标定
步骤1：编译bagconver功能包步骤2：生成imu.bag并转为imu.mat步骤3：对mat文件进行Allan方差分析 第二题：将仿真代码中的欧拉积分替换为中值积分第三题：阅读论文
第一题：设置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文件中加入以下代码：
code_utils
网上普遍使用先编译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倍速率回放）
在终端中执行：
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噪声进行对比分析。
a
c
c
n
o
i
s
e
=
0.2676
∗
1
200
≈
0.019
accnoise=0.2676∗frac{1}{sqrt{200}}≈0.019
accnoise=0.2676∗200
1≈0.019
g
y
r
o
n
o
i
s
e
=
0.2088
∗
1
200
≈
0.015
gyronoise=0.2088∗frac{1}{sqrt{200}}≈0.015
gyronoise=0.2088∗200
1≈0.015
可以看出与预期值保持一致。
步骤4：用matlab脚本画Allan曲线用scripts文件夹下的matlab函数绘制曲线。
陀螺仪Allan方差图如下图所示：
加速度计Allan方差图如下图所示：
由于本人电脑中的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方法对离散点进行插值，从而获得连续的轨迹曲线，进而求导获得相关数据。本论文解决的就是后面的问题。
论文推导内容如下：
