课程代码：

https://github.com/kahowang/Visual_Internal_Odometry/tree/main/第二章 IMU传感器/homework_chapter2

参考博客：

深蓝学院从零开始手写VIO》作业2

使用imu_utils工具生成IMU的Allan方差标定曲线

惯导笔记 - 传感器误差分析

信号噪声的组成;

1)量化噪声

所有量化操作中固有的噪声是数字传感器中不可避免的噪声； 原因：通过AD在将连续信号集成离散信号的过程中，精度会损失，精度损失的大小和AD转换越小，量化噪声越小。

2)随机游走角度

宽带角速率白噪声：陀螺输出角速率含有噪声，噪声中的白噪声成分； 原因：计算姿势的本质是对角速率的积分，这将不可避免地对噪声进行积分。白噪声的积分不是白噪声，而是一个马尔可夫过程，即当前时刻的误差是基于上次误差的随机白噪声。 角度误差中马尔可夫性质的误差称为角度随机游走。

3)角速随机游走

与角度随机行走类似，角速率误差中包含的马尔可夫性质的误差称为角速率随机行走。马尔可夫性质的误差是由宽带角加速率白噪声积累的结果。

4)零偏不稳定噪声

零偏：即常说bias,一般不是固定参数，而是在一定范围内慢慢随机漂移。 零偏不稳定性:零偏随时间缓慢变化,其变化值无法预估,需要假定一个概率区间描述它有多大的可能性落在这个区间内。时间越长,区间越大。

5)速率斜坡

这种误差是趋势误差，而不是随机误差。 随机误差意味着你不能用确定性模型拟合并消除它，最多只能用概率模型来描述它，所以预测结果也是概率 率性质的。 趋势误差可以直接拟合消除。这种误差最常见的原因是温度引起零位变化，可以通过温补来消除 消除。

6)零偏重复性

多次启动时，零偏差不相等，因此会出现重复误差。在实际使用中，每次上电都需要重新估计。 Allan方差分析不包括零偏重复性分析。

[外链图片存储失败，源站可能有防盗链机制，建议保存图片直接上传(img-g2pqoQpl-1646376743662)(/home/lory/Visual_Internal_Odometry/第二章 IMU传感器/pic/allen1.png)]

[外链图片存储失败，源站可能有防盗链机制，建议保存图片直接上传(img-zC8J28kj-1646376743663)(pic/allen2.png)]

误差分类：

加速度计和陀螺仪的误差可分为：确定性误差，随机误差。

确定性误差可提前确认，包括：bias，scale …

随机误差通常假设噪声服从高斯分布，包括：高斯白噪声，bias随机游走

[外链图片存储失败，源站可能有防盗链机制，建议保存图片直接上传(img-tArlRJw2-1646376743664)(pic/allen3.png)]

mkdir vio_sim_ws/src  #将vio_data_simulation-ros_version 放进src中 catkin_make  rosrun vio_data_simulation vio_data_simulation_node    # 生成 imu.bag 数据集 

rqt_bag  info  imu.bag  # 查看当前数据包的内容 path:        imu.bag version:     2.0 duration:    3hr 59:59s (14399s) start:       Feb 24 2022 13:04:10.04 (1645679050.04) end:         Feb 24 2022 17:04:10.04 (1645693450.04) size:        1.0 GB messages:    2880001 compression: none [1344/1344 chunks] types:       sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2] topics:      imu   2880001 msgs    : sensor_msgs/Imu 

编译，安装 imu_utils 工具包，主要参考网站：

LIO-SAM解决运行自己数据包遇到的问题–SLAM不学无数术小问题

这里使用贺博的vio_data_simulation 代码进行生成轨迹

cd  vio_data_simulation
mkdir build 
cd buid 
cmake ..
make  -j				# 编译

cd .. 
cd bin
./data_gen				#运行后即会在当前目录下生成 imu 轨迹数据集

cd ..
cd  python_tool
python  draw_trajcory.py   # 绘制轨迹曲线

FILE : vio_data_simulation/src/imu.cpp

贺博原版代码，以进行欧拉法的惯性解算，公式如下所示

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-n5mMK4kz-1646376743666)(pic/euler.png)]

对应代码如下所示： testImu_euler()

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。
void IMU::testImu_euler(std::string src, std::string dist)
{ 
        
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position : from imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond: from imu measurements
    Eigen::Vector3d acc_w_last ;
    Eigen::Vector3d Vw = init_velocity_;          // velocity : from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    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();
        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 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;
    }
    std::cout<<"test　end"<<std::endl;
}

FILE : vio_data_simulation/src/imu.cpp

中值法对比欧拉法，在获取 imu 的accel 和 gyro 信息时进行均值处理，更加接近真实值

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DR6hs95D-1646376743667)(pic/middle.png)]

核心修改代码为 获取accel 和 gyro 部分

Eigen::Vector3d  gyro_middle  = (imupose_last.imu_gyro +  imupose.imu_gyro) / 2.0;  // 中值积分后的gyro
Eigen::Vector3d dtheta_half =  gyro_middle  * dt /2.0;

Eigen::Vector3d acc_w =  (Qwb_last * (imupose_last.imu_acc) + gw  + Qwb *  (imupose.imu_acc) + gw) / 2.0  ; 

完整代码如下所示 ： testImu_middle()

//读取生成的imu数据并用imu动力学模型对数据进行计算，最后保存imu积分以后的轨迹，
//用来验证数据以及模型的有效性。 中值积分法
void IMU::testImu_middle(std::string src, std::string dist)
{ 
        
    std::vector<MotionData>imudata;
    LoadPose(src,imudata);

    std::ofstream save_points;
    save_points.open(dist);

    double dt = param_.imu_timestep;
    Eigen::Vector3d Pwb = init_twb_;              // position : from imu measurements
    Eigen::Quaterniond Qwb(init_Rwb_);            // quaterniond: from imu measurements
    Eigen::Quaterniond Qwb_last(init_Rwb_);    // 记录上一时刻的姿态
    Eigen::Vector3d acc_w_last ;
    Eigen::Vector3d Vw = init_velocity_;          // velocity : from imu measurements
    Eigen::Vector3d gw(0,0,-9.81);    // ENU frame
    Eigen::Vector3d temp_a;
    Eigen::Vector3d theta;
    for (int i = 1; i < imudata.size(); ++i) { 
        

        MotionData imupose = imudata[i];
        MotionData imupose_last = imudata[i-1];
        
        /// 中值积分
        Eigen::Quaterniond dq;
        Eigen::Vector3d  gyro_middle  = (imupose_last.imu_gyro +  imupose.imu_gyro) / 2.0;  // 中值积分后的gyro
        Eigen::Vector3d dtheta_half =  gyro_middle  * dt /2.0;
        dq.w() = 1;
        dq.x() = dtheta_half.x();
        dq.y() = dtheta_half.y();
        dq.z() = dtheta_half.z();
        dq.normalize();

        Eigen::Vector3d acc_w =  (Qwb_last * (imupose_last.imu_acc) + gw  + Qwb *  (imupose.imu_acc) + gw) / 2.0  ; 
        Qwb_last =  Qwb; 
        Qwb = 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;

    }
    std::cout<<"test　end"<<std::endl;
}

通过下图的轨迹曲线对比，可看出使用middle 中值法进行imu 惯性积分得到的轨迹与真实值更加接近。

blue: GroundTruth orange: imu _int

​ edited by kaho 2022.2.24