Multi sensor fusion positioning Chapter 6 inertial navigation settlement and error model
Reference blog: Deep Blue College - multi-sensor fusion positioning - Chapter 6 operation
1. Attitude update - solution based on median method
1.1 quaternion based attitude update
Get equivalent rotation vector
// get deltas:
size_t index_curr_ = 1;
size_t index_prev_ =0;
Eigen::Vector3d angular_delta = Eigen::Vector3d::Zero();
if(! (GetAngularDelta(index_curr_, index_prev_, angular_delta)) ){
std::cout << "GetAngularDelta(): index error" << std::endl; // Get equivalent rotation vector
}
/**********************************************************************************************************************/
bool Activity::GetAngularDelta(
const size_t index_curr, const size_t index_prev,
Eigen::Vector3d &angular_delta
) {
//
// TODO: this could be a helper routine for your own implementation
//
if (
index_curr <= index_prev ||
imu_data_buff_.size() <= index_curr
) {
return false;
}
const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);
double delta_t = imu_data_curr.time - imu_data_prev.time;
Eigen::Vector3d angular_vel_curr = GetUnbiasedAngularVel(imu_data_curr.angular_velocity); // omega_k
Eigen::Vector3d angular_vel_prev = GetUnbiasedAngularVel(imu_data_prev.angular_velocity); // omega_k-1
angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev); // Calculation of angular by median method
return true;
}
Update quaternion
// update orientation:
Eigen::Matrix3d R_curr_ = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R_prev_ = Eigen::Matrix3d::Identity();
UpdateOrientation(angular_delta,R_curr_, R_prev_); // Update quaternion
/*********************************************************************************************************************/
void Activity::UpdateOrientation(
const Eigen::Vector3d &angular_delta,
Eigen::Matrix3d &R_curr, Eigen::Matrix3d &R_prev
) {
//
// TODO: this could be a helper routine for your own implementation
//
// magnitude:
double angular_delta_mag = angular_delta.norm();
// direction:
Eigen::Vector3d angular_delta_dir = angular_delta.normalized();
// build delta q:
double angular_delta_cos = cos(angular_delta_mag/2.0);
double angular_delta_sin = sin(angular_delta_mag/2.0);
Eigen::Quaterniond dq(
angular_delta_cos,
angular_delta_sin*angular_delta_dir.x(),
angular_delta_sin*angular_delta_dir.y(),
angular_delta_sin*angular_delta_dir.z()
);
Eigen::Quaterniond q(pose_.block<3, 3>(0, 0));
// update:
q = q*dq;
// write back:
R_prev = pose_.block<3, 3>(0, 0);
pose_.block<3, 3>(0, 0) = q.normalized().toRotationMatrix();
R_curr = pose_.block<3, 3>(0, 0);
}
1.2 location update
Update speed
// get velocity delta:
double delta_t_;
Eigen::Vector3d velocity_delta_;
if(! (GetVelocityDelta(index_curr_, index_prev_, R_curr_, R_prev_, delta_t_, velocity_delta_)) ){
std::cout << "GetVelocityDelta(): index error" << std::endl; // Get speed difference
}
/**********************************************************************************************************************/
bool Activity::GetVelocityDelta(
const size_t index_curr, const size_t index_prev,
const Eigen::Matrix3d &R_curr, const Eigen::Matrix3d &R_prev,
double &delta_t, Eigen::Vector3d &velocity_delta
) {
//
// TODO: this could be a helper routine for your own implementation
//
if (
index_curr <= index_prev ||
imu_data_buff_.size() <= index_curr
) {
return false;
}
const IMUData &imu_data_curr = imu_data_buff_.at(index_curr);
const IMUData &imu_data_prev = imu_data_buff_.at(index_prev);
delta_t = imu_data_curr.time - imu_data_prev.time;
Eigen::Vector3d linear_acc_curr = GetUnbiasedLinearAcc(imu_data_curr.linear_acceleration, R_curr);
Eigen::Vector3d linear_acc_prev = GetUnbiasedLinearAcc(imu_data_prev.linear_acceleration, R_prev);
velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);
return true;
}
Update location
// update position:
UpdatePosition(delta_t_, velocity_delta_);
/****************************************************************************************************/
void Activity::UpdatePosition(const double &delta_t, const Eigen::Vector3d &velocity_delta) {
//
// TODO: this could be a helper routine for your own implementation
//
pose_.block<3, 1>(0, 3) += delta_t*vel_ + 0.5*delta_t*velocity_delta;
vel_ += velocity_delta;
}
1.3 results
roslaunch imu_integration imu_integration.launch
2. Attitude update - solution based on Euler method
bool Activity::GetAngularDelta( )
angular_delta = delta_t*angular_vel_prev; // Euler method
bool Activity::GetVelocityDelta( )
velocity_delta = delta_t*linear_acc_prev; // Euler method
result:
roslaunch imu_integration imu_integration.launch
3. GNSS ins SIM simulation data set, compare the median method and Euler method to obtain the solution accuracy
3.1 GNSS ins SIM usage
GNSS ins SIM GitHub download link
Using the example tutorial, open source GNSS/INS simulation based on Python
Key points of use:
1. Define the error model of simulation IMU
You can select three IMU models with different accuracy, 'low accuracy', 'mid accuracy' and 'high accuracy', or customize the IMU model
imu = imu_model.IMU(accuracy=imu_err, axis=6, gps=False) imu = imu_model.IMU(accuracy='low accuracy', axis=9, gps=True)
imu_err = {
# gyro bias, deg/hr
'gyro_b': np.array([0.0, 0.0, 0.0]),
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0.25, 0.25, 0.25]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([3.5, 3.5, 3.5]),
# gyro bias instability correlation, sec.
# set this to 'inf' to use a random walk model
# set this to a positive real number to use a first-order Gauss-Markkov model
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# accelerometer bias, m/s^2
'accel_b': np.array([0.0e-3, 0.0e-3, 0.0e-3]),
# accelerometer velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0.03119, 0.03009, 0.04779]),
# accelerometer bias instability, m/s^2
'accel_b_stability': np.array([4.29e-5, 5.72e-5, 8.02e-5]),
# accelerometer bias instability correlation, sec. Similar to gyro_b_corr
'accel_b_corr': np.array([200.0, 200.0, 200.0]),
# magnetometer noise std, uT
'mag_std': np.array([0.2, 0.2, 0.2])
}
2. Motion definition command type
By writing to csv, the motion is defined. Two instruction formats are mainly used, command type 1 and command type 2
command type 1 defines the rate and angular rate changes within the command duration time, which can be used for accelerated and uniform motion
command type 2 defines the angle (absolute) and speed to reach the preset within the command duration time
| Command type | Comment |
|---|---|
| 1 | Directly define the Euler angles change rate and body frame velocity change rate. The change rates are given by column 2~7. The units are deg/s and m/s/s. Column 8 gives how long the command will last. If you want to fully control execution time of each command by your own, you should always choose the motion type to be 1 |
| 2 | Define the absolute attitude and absolute velocity to reach. The target attitude and velocity are given by column 2~7. The units are deg and m/s. Column 8 defines the maximum time to execute the command. If actual executing time is less than max time, the remaining time will not be used and the next command will be executed immediately. If the command cannot be finished within max time, the next command will be executed after max time. |
3.2 prepare sim dataset
3.2.1 generating rosbag simulation data set
By referring to recorder_node_allan_variance_analysis.py and Teamo ta GitHub Writing method: copy the code to generate the dataset. The GNSS ins SIM source code saves the dataset in csv. Here, in order to facilitate visualization, it is saved in rosbag. The simulated data are saved in IMU: gyro accel; Ground truth: orientation, position, velocity.
FILE: catkin_ws/src/gnss_ins_sim/src/recorder_node_sim.py
#!/usr/bin/python
import os
import rospkg
import rospy
import rosbag
import math
import numpy as np
import pandas as pd
from gnss_ins_sim.sim import imu_model
from gnss_ins_sim.sim import ins_sim
# from gnss_ins_sim.geoparams import geoparams
from std_msgs import msg
from std_msgs.msg import String
from sensor_msgs.msg import Imu
from nav_msgs.msg import Odometry
def get_gnss_ins_sim(motion_def_file, fs_imu, fs_gps):
# set origin x y z
origin_x = 2849886.61825
origin_y = -4656214.27294
origin_z = -3287190.60046
'''
Generate simulated GNSS/IMU data using specified trajectory.
'''
# set IMU model:
D2R = math.pi/180.0
# imu_err = 'low-accuracy'
imu_err = {
# 1. gyro:
# a. random noise:
# gyro angle random walk, deg/rt-hr
'gyro_arw': np.array([0., 0., 0.]),
# gyro bias instability, deg/hr
'gyro_b_stability': np.array([0.0, 0.0, 0.0]),
# gyro bias isntability correlation time, sec
'gyro_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'gyro_b': np.array([0.0, 0.0, 0.0]),
'gyro_k': np.array([1.0, 1.0, 1.0]),
'gyro_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 2. accel:
# a. random noise:
# accel velocity random walk, m/s/rt-hr
'accel_vrw': np.array([0., 0., 0.]),
# accel bias instability, m/s2
'accel_b_stability': np.array([0., 0., 0.]),
# accel bias isntability correlation time, sec
'accel_b_corr': np.array([100.0, 100.0, 100.0]),
# b. deterministic error:
'accel_b': np.array([0.0, 0.0, 0.0]),
'accel_k': np.array([1.0, 1.0, 1.0]),
'accel_s': np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
# 3. mag:
'mag_si': np.eye(3) + np.random.randn(3, 3)*0.0,
'mag_hi': np.array([10.0, 10.0, 10.0])*0.0,
'mag_std': np.array([0.1, 0.1, 0.1])
}
# generate GPS and magnetometer data:
imu = imu_model.IMU(accuracy=imu_err, axis=9, gps=True)
# init simulation:
sim = ins_sim.Sim(
# here sync GPS with other measurements as marker:
[fs_imu, fs_imu, fs_imu],
motion_def_file,
ref_frame=1,
imu=imu,
mode=None,
env=None,
algorithm=None
)
# run:
sim.run(1)
# get simulated data:
rospy.logwarn(
'Simulated data size: Gyro-{}, Accel-{}, pos-{}'.format(
len(sim.dmgr.get_data_all('gyro').data[0]),
len(sim.dmgr.get_data_all('accel').data[0]),
len(sim.dmgr.get_data_all('ref_pos').data)
)
)
# calibration stages:
step_size = 1.0 / fs_imu
for i, (gyro, accel, ref_q, ref_pos, ref_vel) in enumerate(
zip(
# a. gyro:
sim.dmgr.get_data_all('gyro').data[0],
# b. accel:
sim.dmgr.get_data_all('accel').data[0],
# c. gt_pose:
sim.dmgr.get_data_all('ref_att_quat').data, # groundtruth
sim.dmgr.get_data_all('ref_pos').data,
# d. true_vel :
sim.dmgr.get_data_all('ref_vel').data
)
):
yield {
'stamp': i * step_size,
'data': {
# a. gyro:
'gyro_x': gyro[0],
'gyro_y': gyro[1],
'gyro_z': gyro[2],
# b. accel:
'accel_x': accel[0],
'accel_y': accel[1],
'accel_z': accel[2],
# c. true orientation:
'gt_quat_w': ref_q[0],
'gt_quat_x': ref_q[1],
'gt_quat_y': ref_q[2],
'gt_quat_z': ref_q[3],
# d. true position:
'gt_pos_x': ref_pos[0] + origin_x,
'gt_pos_y': ref_pos[1] + origin_y,
'gt_pos_z': ref_pos[2] + origin_z,
# d. true velocity:
'gt_vel_x': ref_vel[0],
'gt_vel_y': ref_vel[1],
'gt_vel_z': ref_vel[2]
}
}
sim.results()
sim.plot(['ref_pos', 'ref_vel'], opt={'ref_pos': '3d'})
def gnss_ins_sim_recorder():
"""
Record simulated GNSS/IMU data as ROS bag
"""
# ensure gnss_ins_sim_node is unique:
rospy.init_node('gnss_ins_sim_recorder_node')
# parse params:
motion_def_name = rospy.get_param('/gnss_ins_sim_recorder_node/motion_file')
sample_freq_imu = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/imu')
sample_freq_gps = rospy.get_param('/gnss_ins_sim_recorder_node/sample_frequency/gps')
topic_name_imu = rospy.get_param('/gnss_ins_sim_recorder_node/topic_name_imu')
topic_name_gt = rospy.get_param('/gnss_ins_sim_recorder_node/topic_name_gt')
## save scv
output_path = rospy.get_param('/gnss_ins_sim_recorder_node/output_path')
output_name = rospy.get_param('/gnss_ins_sim_recorder_node/output_name')
## save rosbag
rosbag_output_path = rospy.get_param('/gnss_ins_sim_recorder_node/output_path')
rosbag_output_name = rospy.get_param('/gnss_ins_sim_recorder_node/output_name')
# generate simulated data:
motion_def_path = os.path.join(
rospkg.RosPack().get_path('gnss_ins_sim'), 'config', 'motion_def', motion_def_name
)
imu_simulator = get_gnss_ins_sim(
# motion def file:
motion_def_path,
# gyro-accel/gyro-accel-mag sample rate:
sample_freq_imu,
# GPS sample rate:
sample_freq_gps
)
# write as csv:
# data = pd.DataFrame(
# list(imu_simulator)
# )
# data.to_csv(
# os.path.join(output_path, output_name)
# )
#write rosbag
with rosbag.Bag(
os.path.join(rosbag_output_path, rosbag_output_name), 'w'
) as bag:
# get timestamp base:
timestamp_start = rospy.Time.now()
for measurement in imu_simulator:
# init:
msg_imu = Imu()
# a. set header:
msg_imu.header.frame_id = 'inertial'
msg_imu.header.stamp = timestamp_start + rospy.Duration.from_sec(measurement['stamp'])
# b. set orientation estimation:
msg_imu.orientation.x = 0.0
msg_imu.orientation.y = 0.0
msg_imu.orientation.z = 0.0
msg_imu.orientation.w = 1.0
# c. gyro:
msg_imu.angular_velocity.x = measurement['data']['gyro_x']
msg_imu.angular_velocity.y = measurement['data']['gyro_y']
msg_imu.angular_velocity.z = measurement['data']['gyro_z']
msg_imu.linear_acceleration.x = measurement['data']['accel_x']
msg_imu.linear_acceleration.y = measurement['data']['accel_y']
msg_imu.linear_acceleration.z = measurement['data']['accel_z']
# write:
bag.write(topic_name_imu, msg_imu, msg_imu.header.stamp)
# write:
bag.write(topic_name_imu, msg_imu, msg_imu.header.stamp)
# init : groundtruth
msg_odom = Odometry()
# a.set header:
msg_odom.header.frame_id = 'inertial'
msg_odom.header.stamp = msg_imu.header.stamp
# b.set gt_pose
msg_odom.pose.pose.position.x = measurement['data']['gt_pos_x']
msg_odom.pose.pose.position.y = measurement['data']['gt_pos_y']
msg_odom.pose.pose.position.z = measurement['data']['gt_pos_z']
msg_odom.pose.pose.orientation.w = measurement['data']['gt_quat_w']
msg_odom.pose.pose.orientation.x = measurement['data']['gt_quat_x']
msg_odom.pose.pose.orientation.y = measurement['data']['gt_quat_y']
msg_odom.pose.pose.orientation.z = measurement['data']['gt_quat_z']
#c.set gt_vel
msg_odom.twist.twist.linear.x = measurement['data']['gt_vel_x']
msg_odom.twist.twist.linear.y = measurement['data']['gt_vel_y']
msg_odom.twist.twist.linear.z = measurement['data']['gt_vel_z']
# write
bag.write(topic_name_gt, msg_odom, msg_odom.header.stamp)
if __name__ == '__main__':
try:
gnss_ins_sim_recorder()
except rospy.ROSInterruptException:
pass
3.2.2 custom motion state
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def
According to the command type definition and dimensional units of GNSS ins SIM, modify the csv to generate the corresponding rosbag. The configuration file is in config
FILE: catkin_ws/src/gnss_ins_sim/config/recorder_gnss_ins_sim.yaml
# motion def:
motion_file: recorder_gnss_ins_sim_speedup_down.csv
# IMU params:
imu: 1
# sample frequency of simulated GNSS/IMU data:
sample_frequency:
imu: 100.0
gps: 10.0
# topic name:
topic_name_imu: /sim/sensor/imu
topic_name_gt: /pose/ground_truth
# output rosbag path:
output_path: /home/kaho/catkin_ws/src/data/gnss_ins_sim/recorder_gnss_ins_sim
# output name:
output_name: speedup_down.bag
motion1: wrap around "8"
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def/recorder_gnss_ins_sim_8circle.csv
| ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 31.224361 | 121.46917 | 0 | 5 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 10 | 0 | 0 | 0 | 0 | 0 | 36 | 1 |
| 1 | -10 | 0 | 0 | 0 | 0 | 0 | 36 | 1 |
| 1 | 10 | 0 | 0 | 0 | 0 | 0 | 36 | 1 |
| 1 | -10 | 0 | 0 | 0 | 0 | 0 | 36 | 1 |
motion2: static
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def/recorder_gnss_ins_sim_static.csv
| ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 31.224361 | 121.46917 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 60 | 1 |
motion3: uniform velocity
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def/recorder_gnss_ins_sim_speedconstant.csv
| ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 31.224361 | 121.46917 | 0 | 5 | 5 | 5 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0 | 0 | 0 | 0 | 0 | 0 | 60 | 1 |
motion4: acceleration
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def/recorder_gnss_ins_sim_speedup.csv
| ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 31.224361 | 121.46917 | 0 | 0 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0 | 0 | 0 | 1 | 1 | 1 | 60 | 1 |
| 1 | 0 | 0 | 0 | 0 | 2 | 2 | 60 | 1 |
| 1 | 0 | 0 | 0 | 0 | 0 | 1 | 60 | 1 |
| 1 | 0 | 0 | 0 | 1 | 1 | 0 | 60 | 1 |
| 1 | 0 | 0 | 0 | 1 | 1 | 1 | 60 | 1 |
motion5: accelerate first and then decelerate
FILE: catkin_ws/src/gnss_ins_sim/config/motion_def/recorder_gnss_ins_sim_speedup_down.csv
| ini lat (deg) | ini lon (deg) | ini alt (m) | ini vx_body (m/s) | ini vy_body (m/s) | ini vz_body (m/s) | ini yaw (deg) | ini pitch (deg) | ini roll (deg) |
|---|---|---|---|---|---|---|---|---|
| 31.224361 | 121.46917 | 0 | 5 | 0 | 0 | 0 | 0 | 0 |
| command type | yaw (deg) | pitch (deg) | roll (deg) | vx_body (m/s) | vy_body (m/s) | vz_body (m/s) | command duration (s) | GPS visibility |
| 1 | 0 | 0 | 0 | 10 | 10 | 10 | 30 | 1 |
| 1 | 0 | 0 | 0 | -2 | -2 | -2 | 60 | 1 |
3.2.3 use
roslaunch gnss_ins_sim recorder_gnss_ins_sim.launch
FILE: catkin_ws/src/gnss_ins_sim/launch/recorder_gnss_ins_sim.launch
<launch>
<node name="gnss_ins_sim_recorder_node" pkg="gnss_ins_sim" type="recorder_node_sim.py">
<!-- load default params -->
<rosparam command="load" file="$(find gnss_ins_sim)/config/recorder_gnss_ins_sim.yaml" />
<!-- configuration -->
</node>
</launch>
After running, in catkin_ ws/src/data/gnss_ ins_ sim/recorder_ gnss_ ins_ The corresponding rosbag will be generated in the SIM directory
3.3 storage of pose in Evo format
FILE:catkin_ws/src/imu_integration/src/evo_evaluate/evaluate.cpp
The evo tool is used for accuracy evaluation, and the evo format uses the TUM format, referring to the teaching assistant of eamo GitHub Writing method: in order to facilitate subsequent calls, the format of the saved file is written as ROS Node through subscription:
/sim/sensor/imu /pose/ground_truth two topics are stored in files of corresponding formats for evo evaluation.
For the use of evo, please refer to:
SLAM Trajectory Accuracy Evaluation Tool evo usage
3.3.1 evo TUM format storage
The commonly used evo KITTI pose format is known as timestamp. The accuracy is evaluated and compared through a fixed number of sequences. There is a timestamp in the format of the TUM data set. It will be more accurate if compared through the timestamp. Therefore, the stored evo data format is TUM
evo TUM requires that the pose storage format is timestamp x y z q_x q_y q_z q_w ； Refer to evo official file Formats for details
Storage writing method
/* write2txt format TUM*/
void WriteText(std::ofstream& ofs, pose data){
ofs << std::fixed << data.timestamp << " " << data.pos.x() << " " << data.pos.y() << " " << data.pos.z() << " "
<< data.q.x() << " " << data.q.y() <<" " << data.q.z() << " " << data.q.w() << std::endl;
}
3.3.1 time alignment of tum timestamps
At the beginning, when timestamps are written in gt.txt and ins.txt, the timestamps are the timestamps of their respective ROS topic s. Run Evo_ During RPE segment evaluation, an error is reported, and the data with a timestamp difference of 0.01s in the two files cannot be found. After observation, it is because the estimated IMU inertia solution node subscribes to IMU first_ The SIM topic data is only solved, which will be a little slower than the groundtruth topic timestamp. Therefore, the respective time can be aligned by subtracting the data timestamp of the first frame from their current timestamp.
if(flag_ins){
stamp_ins_init = msg->header.stamp.toSec();
flag_ins = 0;
}
pose_ins.timestamp = msg->header.stamp.toSec() - stamp_ins_init; //Convert timestamp to floating point format
/******************************************************************************************************/
if(flag_gt){
stamp_gt_init = msg->header.stamp.toSec();
flag_gt = 0;
}
pose_gt.timestamp = msg->header.stamp.toSec() - stamp_gt_init;
3.3.3 evo TUM ROS_ Complete implementation of node
FILE:catkin_ws/src/imu_integration/src/evo_evaluate/evaluate.cpp
#include <iostream>
#include <vector>
#include <string>
#include <list>
#include <sstream>
#include <fstream>
#include <iomanip>
/*ROS INCLUDE*/
#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include <Eigen/Core>
using namespace std;
/*Define pose structure*/
struct pose
{
double timestamp;
Eigen::Vector3d pos ;
Eigen::Quaterniond q;
};
pose pose_gt; // GroundTruth pose
pose pose_ins; // Estimate pose
std::ofstream gt;
std::ofstream ins;
double stamp_gt = 0;
double stamp_ins = 0;
double stamp_gt_init = 0;
double stamp_ins_init = 0;
int flag_gt = 1;
int flag_ins = 1;
bool CreateFile(std::ofstream& ofs, std::string file_path) {
ofs.open(file_path, std::ios::out); // Use std::ios::out to achieve coverage
if(!ofs)
{
std::cout << "open csv file error " << std::endl;
return false;
}
return true;
}
/* write2txt format TUM*/
void WriteText(std::ofstream& ofs, pose data){
ofs << std::fixed << data.timestamp << " " << data.pos.x() << " " << data.pos.y() << " " << data.pos.z() << " "
<< data.q.x() << " " << data.q.y() <<" " << data.q.z() << " " << data.q.w() << std::endl;
}
void insCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
if(flag_ins){
stamp_ins_init = msg->header.stamp.toSec();
flag_ins = 0;
}
pose_ins.timestamp = msg->header.stamp.toSec() - stamp_ins_init; //Convert timestamp to floating point format
/*update position*/
pose_ins.pos.x() = msg->pose.pose.position.x ;
pose_ins.pos.y() = msg->pose.pose.position.y ;
pose_ins.pos.z() = msg->pose.pose.position.z ;
/*update orientation*/
pose_ins.q.w() = msg->pose.pose.orientation.w;
pose_ins.q.x() = msg->pose.pose.orientation.x;
pose_ins.q.y() = msg->pose.pose.orientation.y;
pose_ins.q.z() = msg->pose.pose.orientation.z;
/*write to txt, fomat TUM*/
WriteText(ins,pose_ins);
}
void gtCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
if(flag_gt){
stamp_gt_init = msg->header.stamp.toSec();
flag_gt = 0;
}
pose_gt.timestamp = msg->header.stamp.toSec() - stamp_gt_init;
/*update position*/
pose_gt.pos.x() = msg->pose.pose.position.x ;
pose_gt.pos.y() = msg->pose.pose.position.y ;
pose_gt.pos.z() = msg->pose.pose.position.z ;
/*update orientation*/
pose_gt.q.w() = msg->pose.pose.orientation.w;
pose_gt.q.x() = msg->pose.pose.orientation.x;
pose_gt.q.y() = msg->pose.pose.orientation.y;
pose_gt.q.z() = msg->pose.pose.orientation.z;
/*write to txt, fomat TUM*/
WriteText(gt,pose_gt);
}
int main(int argc, char **argv)
{
char path_gt[] = "/home/kaho/catkin_ws/src/data/gnss_ins_sim/recorder_gnss_ins_sim/gt.txt";
char path_ins[] = "/home/kaho/catkin_ws/src/data/gnss_ins_sim/recorder_gnss_ins_sim/ins.txt";
CreateFile(gt,path_gt );
CreateFile(ins,path_ins );
// init node
ros::init(argc, argv, "evaluate_node");
// create nodehandle
ros::NodeHandle nh;
// create subscriber
ros::Subscriber sub_ins = nh.subscribe("/pose/estimation", 10, insCallback);
ros::Subscriber sub_gt = nh.subscribe("/pose/ground_truth",10, gtCallback);
ros::Rate loop_rate(100); // frequence 100hz
while (ros::ok())
{
ros::spinOnce(); // goto callback function
loop_rate.sleep();
}
gt.close();
ins.close();
return 0;
}
3.4 comparison of IMU solution between median method and Euler method under different motion scenes
Start imu inertial solution node
roslaunch imu_integration imu_ins_gnss.launch rosbag play 8circle.bag
evo evaluation
evo_rpe tum gt.txt ins.txt -r full --delta 100 --plot --plot_mode xyz
Vigorous exercise: around the word '8'
| Median method | Euler method |
|---|---|
 |  |
 |  |
 |  |
STATIC at rest
| Median method | Euler method |
|---|---|
 |  |
 |  |
 |  |
Uniform motion speedconstant
| Median method | Euler method |
|---|---|
 |  |
 |  |
 |  |
Acceleration
| Median method | Euler method |
|---|---|
 |  |
 |  |
 |  |
Accelerate before decelerate_ down
| Median method | Euler method |
|---|---|
 |  |
 |  |
 |  |
3.5 conclusion:
Generally speaking, imu has high angular velocity accuracy and low linear acceleration accuracy
3.5.1 for static and uniform motion (acceleration is 0), the accuracy of the median method is lower than that of the Euler method
Reason: the linear acceleration and angular velocity measured by imu are not 0. Because the angular velocity change of imu is small, so
The error is small, and the effect of Euler method is similar to that of median method. The velocity calculated by linear acceleration will be tired
Product error, the average value of median method will increase the position error (relative to Euler method).
3.5.2 for acceleration and deceleration motion, the accuracy of the median method is higher than that of the Euler method
Reason: under variable speed motion, the average value of the median method is more reasonable, and the angular acceleration and linear acceleration are absolutely different
The larger the pair value, the larger the Euler error will be.
edited by kaho 2021.9.30