VIO第二章作业

一、kalibr_allan标定

1.安装matlab、ROS等

2.编译功能包vio_data_simulation

3.执行，生成数据imu.bag

rosrun vio_data_simulation vio_data_simulation_node

 

4.下载kaliibr_allan功能包并进行编译

git clone https://github.com/rpng/kalibr_allan.git

5.使用bagconver将bag转成mat文件（注意这里最后一个参数为话题名，用rosbag info查看）

rosbag info imu.bag

rosrun bagconvert bagconvert imu.bag imu

6.打开matlab，注意sudo

sudo matlab

7.执行SCRIPT_allan_matparallel.m，注意要修改mat文件的路径；执行之后会生成一个新的mat；下面用SCRIPT_process_results.m来操作这个mat（也要注意修改文件位置），这样便计算出误差，并且绘画出allan曲线

8.结果

 

 　　与param.h中设定的值对比，十分接近，因此标定是成功的。

 二、中值积分代替欧拉积分

　　这里用的是老师给的另外一个包，因为给的python绘图工具已经匹配好了。过程为：

cd vio_data_simulation-master
mkdir build
cd build
cmake ..
make
cd ../bin
./data_gen
cd ../python_tool
python draw_trajcory.py

　　将imu.cpp中的欧拉法代码：

        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;

　　替换为中值法代码：

        MotionData imupose_ = imudata[i-1];//上一时刻的数据
        MotionData imupose = imudata[i];//下一时刻的数据
        Eigen::Quaterniond dq;
        Eigen::Vector3d dtheta_half = (imupose_.imu_gyro+ imupose.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();//这里要记得归一化
        Eigen::Vector3d acc_w = (Qwb * dq * (imupose.imu_acc) + gw + Qwb * (imupose_.imu_acc) + gw)/2;//这里注意下一时刻对应的转换矩阵是Qwb * dq，而不是Qwb
        Qwb = Qwb * dq;
        Pwb = Pwb + Vw * dt + 0.5 * dt * dt * acc_w;
        Vw = Vw + acc_w * dt;

　　结果分别为：

 

 　　对比可以看出，中值积分法更加接近真值。