Chapter II IMU sensor
Course Code:
Reference blog:
Homework 2 of handwriting VIO from scratch of dark blue College
Using imu_utils tool generates the Allan variance calibration curve of IMU
Error analysis of inertial navigation sensor
1. Generate simulation data set and calibrate Allan variance
1.1. Noise analysis of inertial devices
Multisensor fusion location notes (ren Qian):
Composition of signal and noise;
1) Quantization noise
The inherent noise of all quantization operations is the inevitable noise of digital sensors;
Cause: in the process of collecting continuous time signals into discrete signals through AD acquisition, the accuracy will be lost. The size of accuracy loss is related to the step size of AD conversion. The smaller the step size is, the smaller the quantization noise is.
2) Angular random walk
Wide band angular rate white noise: the gyro output angular rate contains noise, which is the white noise component;
Cause: the essence of attitude calculation is to integrate the diagonal velocity, which must also integrate the noise. The integration of white noise is not white noise, but a Markov process, that is, the error of the current time is obtained by accumulating a random white noise on the basis of the error of the previous time.
The error of Markov property contained in the angle error is called angle random walk.
3) Angular rate random walk
Similar to angular random walk, the Markov error contained in angular rate error is called angular rate random walk. The error of Markov property is the result of white noise accumulation of broadband angular acceleration rate.
4) Zero bias instability noise
Zero bias: bias, as it is often said, is generally not a fixed parameter, but a slow random drift within a certain range.
Zero bias instability: the zero bias changes slowly with time, and its change value cannot be estimated. It is necessary to assume a probability interval to describe how likely it is to fall within this interval. The longer the time, the larger the interval.
5) Rate ramp
This error is a trend error, not a random error.
Random error means that you can't use deterministic model to simulate and eliminate it. At most, you can only use probabilistic model to describe it. In this way, the prediction result is also approximate
Rate nature.
Trend error can be eliminated by direct fitting. The most common reason for this error in gyro is the change of zero position caused by temperature, which can be corrected by temperature compensation
Eliminate.
6) Zero bias repeatability
There will be an error when the start is not equal to zero. In actual use, it needs to be re estimated every time the power is on.
Allan analysis of variance does not include the analysis of zero bias repeatability.
[the external chain picture transfer fails, and the source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-g2pqoqpl-16463743662) (/ home / Lory / visual_internal_odometry / Chapter II IMU sensor / pic/allen1.png)]
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-zc8j28kj-16463743663) (PIC / Allen2. PNG)]
VIO notes (he Yijia)
Error classification:
The errors of accelerometer and gyroscope can be divided into deterministic error and random error.
Deterministic errors can be calibrated and confirmed in advance, including bias, scale
Random error usually assumes that the noise obeys Gaussian distribution, including Gaussian white noise and bias random walk
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-tarlrjw2-16463743664) (PIC / allen3. PNG)]
1.2 generate imu data set of ROS
mkdir vio_sim_ws/src #Will Vio_ data_ simulation-ros_ Put version into src catkin_make rosrun vio_data_simulation vio_data_simulation_node # Generate IMU Bag dataset
rqt_bag info imu.bag # View the contents of the current packet 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
1.3 imu_utils completes allan calibration
1.3 imu_utils completes allan calibration
Compile and install imu_utils toolkit, main reference website:
Using imu_utils tool generates the Allan variance calibration curve of IMU
Note: code_utils and imu_utils has sequence and cannot be compiled together
FILE : imu_utils_ws/src/imu_utils/launch new sim_imu.launch
<launch>
<node pkg="imu_utils" type="imu_an" name="imu_an" output="screen">
<param name="imu_topic" type="string" value= "/imu"/> <!--imu ROS topic of conversation /imu -->
<param name="imu_name" type="string" value= "sim_imu"/>
<param name="data_save_path" type="string" value= "$(find imu_utils)/data/"/> <!--Save the data path after calibration of the table-->
<param name="max_time_min" type="int" value= "230"/> <!--Maximum time to read data-->
<param name="max_cluster" type="int" value= "100"/>
</node>
</launch>
roslaunch imu_utils sim_imu.launch rosbag play imu.bag -r 500 # Play at 500X
Calibration result: File: IMU_ utils_ ws/src/imu_ utils/data
Note: Sim_ The preset noise unit of IMU is gyro: rad / (s * sqrt (Hz)) ACC: M / (s ^ 2 * sqrt (Hz)), while imu_utils preset noise unit is gyro: rad/s acc: m/s^2. When unifying units, IMU needs to be_ The results of utils are divided by sqrt (Hz), sim_ The sampling frequency of IMU is 200 Hz, so IMU_ The results of utils need to be divided by sqrt(200) = 14.14
Simulation imu data, preset imu_noise unit
Simulation imu data, preset imu_noise unit
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (IMG cjwtyifk-16463743664) (PIC / phd_he_imu_nose_uint. PNG)]
imu_utils calibrated units
[the transfer of external chain pictures fails, and the source station may have anti-theft chain mechanism. It is recommended to save the pictures and upload them directly (img-cmnk9hij-16463743665) (PIC / imu_utils_imu_noise_uint. PNG)]
| Error type | Truth rad/(s * sqrt(hz)) m/(s^2*sqrt(hz)) | imu_utils rad/s m/s^2 | imu_utils unit aligned rad/(s * sqrt(hz)) m/(s^2*sqrt(hz)) |
|---|---|---|---|
| gyro white noise | 0.015 | 0.21165531 | 0.014968 |
| gyro bias random walk | 0.00005 | 0.00095079 | 0.0000672 |
| acc white noise | 0.019 | 0.23441296 | 0.016578 |
| acc bias random walk | 0.0005 | 0.00349127 | 0.00024691 |
Using matlab to describe the allan variance curve of gyro acc
FILE: imu_utils_ws/src/imu_utils/scripts/draw_allan.m
gyro_allan curve
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-zhv0gnhk-16463743665) (PIC / allan_gyro_matlab. PNG)]
acc_allan curve
[the external chain picture transfer fails, and the source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-tczybdyh-16463743666) (PIC / allan_acc_matlab. PNG)]
2. Generate motion imu data and use median method and Euler method for inertial integration
Use herb's vio here_ data_ The simulation code generates the trajectory
2.1 compilation and operation
cd vio_data_simulation mkdir build cd buid cmake .. make -j # compile cd .. cd bin ./data_gen #After running, the imu track data set will be generated in the current directory cd .. cd python_tool python draw_trajcory.py # Draw trajectory curve
2.2 Euler method for inertial calculation of imu
FILE : vio_data_simulation/src/imu.cpp
Hebo's original code is used to calculate the inertia of Euler method. The formula is as follows
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-n5mmk4kz-16463743666) (PIC / Euler. PNG)]
The corresponding code is as follows: testImu_euler()
//Read the generated imu data, calculate the data with the imu dynamic model, and finally save the trajectory after imu integration,
//Used to verify the effectiveness of data and model.
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();
///Euler integral of imu dynamic model
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;
//It is stored in the format of imu position, imu quaternion, cam position and cam quaternion. Since there is no cam, imu is saved twice
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;
}
2.3 inertia calculation of imu by median method
FILE : vio_data_simulation/src/imu.cpp
Compared with Euler method, the median method processes the mean value when obtaining the accel and gyro information of imu, which is closer to the real value
[the external chain picture transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (img-dr6hs95d-16463743667) (PIC / middle. PNG)]
The core modification code is to obtain the accel and gyro parts
Eigen::Vector3d gyro_middle = (imupose_last.imu_gyro + imupose.imu_gyro) / 2.0; // gyro after median integration 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 ;
The complete code is as follows: testImu_middle()
//Read the generated imu data, calculate the data with the imu dynamic model, and finally save the trajectory after imu integration,
//Used to verify the effectiveness of data and model. Median integral method
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_); // Record the posture of the last moment
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];
///Median integral
Eigen::Quaterniond dq;
Eigen::Vector3d gyro_middle = (imupose_last.imu_gyro + imupose.imu_gyro) / 2.0; // After the median value of gyro integral
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;
//It is stored in the format of imu position, imu quaternion, cam position and cam quaternion. Since there is no cam, imu is saved twice
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;
}
2.4 comparison between Euler method and median method
By comparing the trajectory curves in the figure below, it can be seen that the trajectory obtained by imu inertia integration using the middle median method is closer to the real value.
blue: GroundTruth orange: imu _int
| Euler Euler inertial integral | Middle median method inertia integral |
|---|---|
| [the external chain picture transfer fails, and the source station may have an anti-theft chain mechanism. It is recommended to save the picture and upload it directly (IMG pptrjz34-16463743668) (PIC / euler_trajectory. PNG)] | [the external chain image transfer fails. The source station may have an anti-theft chain mechanism. It is recommended to save the image and upload it directly (img-zjkuits9-16463743669) (PIC / middle_trajectory. PNG)] |
edited by kaho 2022.2.24