Multi sensor fusion positioning Chapter 6 inertial navigation settlement and error model

Multi sensor fusion positioning Chapter 6 inertial navigation settlement and error model

Reference blog: Deep Blue College - multi-sensor fusion positioning - Chapter 6 operation

Code download: https://github.com/kahowang/sensor-fusion-for-localization-and-mapping/tree/main/%E7%AC%AC%E4%BA%94%E7%AB%A0%20%E6%83%AF%E6%80%A7%E5%AF%BC%E8%88%AA%E5%8E%9F%E7%90%86%E5%8F%8A%E8%AF%AF%E5%B7%AE%E5%88%86%E6%9E%90/imu_tk

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 typeComment
1Directly 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
2Define 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.224361121.469170500000
command typeyaw (deg)pitch (deg)roll (deg)vx_body (m/s)vy_body (m/s)vz_body (m/s)command duration (s)GPS visibility
11000000361
1-1000000361
11000000361
1-1000000361

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.224361121.469170000000
command typeyaw (deg)pitch (deg)roll (deg)vx_body (m/s)vy_body (m/s)vz_body (m/s)command duration (s)GPS visibility
1000000601

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.224361121.469170555000
command typeyaw (deg)pitch (deg)roll (deg)vx_body (m/s)vy_body (m/s)vz_body (m/s)command duration (s)GPS visibility
1000000601

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.224361121.469170000000
command typeyaw (deg)pitch (deg)roll (deg)vx_body (m/s)vy_body (m/s)vz_body (m/s)command duration (s)GPS visibility
1000111601
1000022601
1000001601
1000110601
1000111601

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.224361121.469170500000
command typeyaw (deg)pitch (deg)roll (deg)vx_body (m/s)vy_body (m/s)vz_body (m/s)command duration (s)GPS visibility
1000101010301
1000-2-2-2601

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

evo evaluation TUM dataset

How to install evo, how to use evo, how to use the tutorial, SLAM trajectory accuracy evaluation tool, how to evaluate the Trajectory Accuracy generated by ORB-SLAM2, evaluate the trajectory accuracy of lidar SLAM and visual SLAM, and quantify the error of SLAM

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 methodEuler method

STATIC at rest

Median methodEuler method

Uniform motion speedconstant

Median methodEuler method

Acceleration

Median methodEuler method

Accelerate before decelerate_ down

Median methodEuler 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

Tags: C++ Deep Learning Autonomous vehicles

Posted on Sat, 02 Oct 2021 20:20:00 -0400 by gkelley091565