IMU参数辨识及标定
一、标定参数分析
标定的本质是参数辨识。首先明确哪些参数可辨识，其次弄清怎样辨识。
参数包括陀螺仪和加速度计各自的零偏、标度因数、安装误差。
- IMU需要标定的参数主要是确定性误差和随机误差，确定性误差主要标定bias，scale和misalignment，随机误差主要标定noise和random walk，
kalibr_allan是用于求取随机误差的开源工具。
可以中英对照
1.参数项
1) 零偏
这个比较好理解，就是输出比输入多了一个常值误差。
需要注意的是，通过Allan方差分析，得到了器件的量化噪声、角度随机游走、角速率随机游走、零偏不稳定性噪声、速率斜坡，仔细看，这些都是对零偏质量的分析，也可以直观的理解为零偏的波动和漂移程度，这里面并没有分析零偏本身的大小，而这个才是我们标定里要去估计的那个常值误差。
加速度计的零偏在这里表示为
陀螺仪的零偏在这里表示为
2) 标度因数误差
也叫刻度因数误差。假设器件输出的是标准单位角速度(rad/s)，那么输出和输入的比就是1。如果不是，就得需要标定，修正这个比例。
加速度计的标度因数这里表示如下：
陀螺仪的标度因数这里表示为：
- 修改配置文件
进入之前编译好的imu_utils文件夹下xxxx.launch文件（自己添加的）查看相关设置：
3) 安装误差
这里面b坐标系是正交的imu坐标系，g坐标系的三个轴是分别对应三个陀螺仪。由于加工工艺原因，陀螺仪的三个轴并不正交，和我们导航中使用的正交轴不重合。我们需要仔细想一想，这个安装误差怎么在陀螺输出中体现出来的，因为我们标定时只能采集到陀螺的输出，而无法直接去测量安装误差。理论上，在陀螺坐标轴和b系重合的情况下，我们沿b系某一个坐标轴旋转，那么其他两个轴是不会有角速度输出的，而有了安装误差以后，便有了输出，据此，我们就可以建立输出和误差之间的关系了。以图中一项误差为例，Sgxy表示的就是y轴的单位输入，在x陀螺上由安装误差造成的输出。由此，我们可以把所有的安装误差都成矩阵形式，即:
加速度计的安装误差原理和它一样，直接给出公式。
这样一共有12项安装误差参数。有的时候，可以简化为9项，具体什么情况下简化，以及怎样简化，我们会在本文的后面讲。
2）启动launch文件
source devel/setup.bash
roslaunch imu_utils xxxx.launch
2. 误差模型
通过上面的参数分析，我们已经可以很容易地写出误差模型了。
陀螺仪：
其中W是陀螺输出，ω是各坐标轴真实输入。该公式的展开形式为
同理，可以得到加速度计的展开形式为：
二、利用imu_utils 标定IMU参数
imu_utils是用于求取IMU随机误差的开源工具。
1. 下载imu_utils和code_utils
imu_utils下载地址为：GitHub - gaowenliang/imu_utils: A ROS package tool to analyze the IMU performance.
code_utils下载地址为：GitHub - gaowenliang/code_utils: my code utils
注意：
1、全局安装ceres库，code_imu依赖ceres；
2、不要同时把imu_utils和code_utils一起放到src下进行编译。由于imu_utils依赖code_utils，所以先把code_utils放在工作空间的src下面，进行编译。然后再将imu_utils放到src下面，再编译。
2. 安装依赖并编译
sudo apt-get install libdw-dev
2.1 编译code_utils
2.2 安装Ceres
2.3 构建工作空间编译code_utils
问题：
sumpixel_test.cpp文件
问题1：修改#include "backward.hpp"为 #include“code_utils/backward.hpp”
问题2：
解决：添加头文件：#include"opencv2/imgcodecs/legacy/constants_c.h"
问题3：
解决：V_MINMAX 改为 NORM_MINMAX
mat_io_test.cpp文件
问题：
解决：
opencv4.x以上，有些宏，API名字改了，需要改为新的：
CV_LOAD_IMAGE_UNCHANGED 改为 cv::IMREAD_UNCHANGED
CV_LOAD_IMAGE_GRAYSCALE 改为 cv::IMREAD_GRAYSCALE
CV_LOAD_IMAGE_COLOR 改为 cv::IMREAD_COLOR
CV_LOAD_IMAGE_ANYDEPTH 改为 cv::IMREAD_ANYDEPTH
3 imu_utils编译
文件结构：
将 ROS 包imu_utils放入code_utils的工作区src下面进行编译使用catkin_make;进行构建
CMakeLists.txt文件下
代码修改如下：
修改set(CMAKE_CXX_FLAGS "-std=c++11")为set(CMAKE_CXX_FLAGS "-std=c++14")
4 播放bag文件，进行离线标定IMU（使用imu_utils计算allan方差）
4.1 播放rosbag
rosbag play -r 200 imu.bag // 200 倍速播放rosbag
4.2 运行标定文件
source ./devel/setup.bash
roslaunch imu_utils gh5a5s1.launch
注意：等rosbag播放两秒左右在启动launch文件，因为在IMU前几分钟录制的数据误差比较大。
4.3 标定结果
数据包播放结束之后，在/imu_utils/data/这个文件夹下会出现一系列的参数文件，如上图所示。
打开xxxx_imu_param.yaml这个文件，会看到计算出来的噪声和随机游走的系数值，如下所示：
虽然我们得到了标定结果，但这个标定结果并不是我们最终的结果。现在得到的结果的单位是rad/s和m/s^2，噪声是个能量概念或者说功率概念，我们还要把标定得到的参数归一化到每单位sqrt(hz)尺度下,即/aqrt（Hz）。
加速度allan方差图
加速度每个轴的allan方差
陀螺的allan方差
陀螺每个轴的allan方差
有时候播放完数据，程序没有任何反映。试试300rete 播放。
三、 利用kalibr_allan标定IMU随机误差
该方法需要安装Matlab，才能编译kalibr_allan。
1. 下载kalibr_allan并编译
问题：
在/kalibr_allan-master/bagconvert/cmake目录下找到FindMatlab.cmake文件，更改matlab的地址。
find_program(MATLAB_EXE_PATH matlab
PATHS /usr/local/MATLAB/R2018a/bin)
把 /usr/local/MATLAB/R2018a/bin更换成自己matlab的地址
修改之后，删除build和dev文件夹，再次catkin_make
2. 生成mat文件
rosrun bagconvert bagconvert bag名字 topic名字
3. matlab计算IMU误差
利用/kalibr_allan/matlab/SCRIPT_allan_matparallel.m生成误差文件。
生成误差的.m文件：
注意：修改路径,最好改为绝对路径
/kalibr_allan/matlab/SCRIPT_allan_matparallel.m
4. 绘制allan方差曲线
4.1 运行/kalibr_allan/matlab/SCRIPT_process_results.m文件
修改路径mat_path的路径修改为步骤3中得到的.mat文件
4.2 标定结果
accelerometer_noise_density = 0.00102011
accelerometer_random_walk = 0.00003924
gyroscope_noise_density = 0.00001248
gyroscope_random_walk = NaN
