# Chapter II IMU sensor

Course Code:

https://github.com/kahowang/Visual_Internal_Odometry/tree/main/%E7%AC%AC%E4%BA%8C%E7%AB%A0%20IMU%E4%BC%A0%E6%84%9F%E5%99%A8/homework_chapter2

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

Compile and install imu_utils toolkit, main reference website:

Solutions to problems encountered by LIO-SAM in running its own data package – SLAM does not learn numerous technical problems

Use imu_utils calibrates the IMU, which is then used for the joint calibration of the camera and IMU in kalibr

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 typeTruth rad/(s * sqrt(hz)) m/(s^2*sqrt(hz))imu_utils rad/s m/s^2imu_utils unit aligned rad/(s * sqrt(hz)) m/(s^2*sqrt(hz))
gyro white noise0.0150.211655310.014968
gyro bias random walk0.000050.000950790.0000672
acc white noise0.0190.234412960.016578
acc bias random walk0.00050.003491270.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;

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;

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 integralMiddle 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

Keywords: C++ AI Computer Vision

Added by jogisarge on Sat, 05 Mar 2022 04:54:40 +0200