1.姿态更新-基于中值法的解算
1.1 基于四元数的姿态更新
在imu_integration/src/estimator/activity.cpp中补充代码：
bool Activity::UpdatePose(void) {
if (!initialized_) {
// use the latest measurement for initialization:
OdomData &odom_data = odom_data_buff_.back();
IMUData imu_data = imu_data_buff_.back();
pose_ = odom_data.pose;
vel_ = odom_data.vel;
initialized_ = true;
odom_data_buff_.clear();
imu_data_buff_.clear();
// keep the latest IMU measurement for mid-value integration:
imu_data_buff_.push_back(imu_data);
} else {
//
// TODO: implement your estimation here
//
// get deltas:
Eigen::Vector3d angular_delta = Eigen::Vector3d::Zero();
size_t index_curr = 1;
size_t index_prev = 0;
if(!Activity::GetAngularDelta(index_curr, index_prev, angular_delta)){
ROS_ERROR("Angular index error!");
return false;
}
// update orientation:
Eigen::Matrix3d R_curr = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R_prev = Eigen::Matrix3d::Identity();
Activity::UpdateOrientation(angular_delta, R_curr, R_prev);
// get velocity delta:
Eigen::Vector3d velocity_delta = Eigen::Vector3d::Zero();
double delta_t = .0;
if(!Activity::GetVelocityDelta(index_curr, index_prev,R_curr, R_prev, delta_t, velocity_delta)){
ROS_ERROR("Velocity index error!");
return false;
}
// update position:
Activity::UpdatePosition(delta_t, velocity_delta);
// move forward --
// NOTE: this is NOT fixed. you should update your buffer according to the method of your choice:
imu_data_buff_.pop_front();
}
return true;
}
1.2 速度更新
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);
// Median method
velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);
// Euler method
//velocity_delta = delta_t*linear_acc_prev;
return true;
}
1.3 位置更新
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.4 结果
基于中值法的结果
欧拉法只需要改变旋转矢量计算
// 角速度更新
// Median method
// angular_delta = 0.5*delta_t*(angular_vel_curr + angular_vel_prev);
// Euler method
angular_delta = delta_t*angular_vel_prev;
// 速度更新
// Median method
//velocity_delta = 0.5*delta_t*(linear_acc_curr + linear_acc_prev);
// Euler method
velocity_delta = delta_t*linear_acc_prev;
基于欧拉法的结果
可以明显看到，中值法的精度优于欧拉法
2. IMU仿真数据，基于中值法和欧拉法对比精度
2.1 gnss-ins-sim 使用
step1 定义IMU误差模型
#axis = 6 to generate only gyro and accelerometer data.
#axis = 9 to generate magnetometer data besides gyro and accelerometer data.
#gps = True to generate GPS data, gps = False not.
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])
}
Step 2 创建运动配置文件
step3 创建自己的算法
(1) 生成对应的rosbag包
源码下载下来以后，需要setup
sudo python3 setup.py install
我的环境是ubuntu20.04 ，python3.8，但是作业框架是较低版本，主要有两处需要修改：
如果已经安装了python2.7的版本，需指定使用python3.8
#!/usr/bin/python3
这里画3D的图形会报错，暂时未解决，使用最新的源码，在提供的demo中指定画3D的图也会出现同样的问题，
暂时先画2D图形：
#sim.plot(['ref_pos', 'ref_vel'], opt={'ref_pos': '3d'})
sim.plot(['ref_pos', 'ref_vel'])
其余部分参考demo里的写法实现：
#!/usr/bin/python3
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'})
sim.plot(['ref_pos', 'ref_vel'])
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')
print("motion def is %s",motion_def_name)
## 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
(2) 定义运动模型
# motion def:
motion_file: static.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/ubuntu/codebase/slam/sim_data/static/
# output name:
output_name: static.bag
motion1: static
motion2: speed up
motion3: speed constant
motion4: speed up & down
motion5: 8 circle
2.2 存储为TUM或KITTI数据
// 读取odom数据转存成kitti数据
static void save_kitti(fstream &file, nav_msgs::Odometry &data)
{
// std::lock_guard<std::mutex> lock(mtx);
if (gtQueue.empty())
return;
while (gtQueue.front().header.stamp.toSec() < data.header.stamp.toSec() - 0.1)
{
gtQueue.pop_front();
}
nav_msgs::Odometry thisGT = gtQueue.front();
ros::Time gt_time = thisGT.header.stamp;
float gt_x = thisGT.pose.pose.position.x;
float gt_y = thisGT.pose.pose.position.y;
float gt_z = thisGT.pose.pose.position.z;
float gt_ww = thisGT.pose.pose.orientation.w;
float gt_xx = thisGT.pose.pose.orientation.x;
float gt_yy = thisGT.pose.pose.orientation.y;
float gt_zz = thisGT.pose.pose.orientation.z;
ros::Time data_time = data.header.stamp;
float data_x = data.pose.pose.position.x;
float data_y = data.pose.pose.position.y;
float data_z = data.pose.pose.position.z;
float data_ww = data.pose.pose.orientation.w;
float data_xx = data.pose.pose.orientation.x;
float data_yy = data.pose.pose.orientation.y;
float data_zz = data.pose.pose.orientation.z;
Eigen::Quaterniond data_q(data_ww, data_xx, data_yy, data_zz);
Eigen::Matrix3d data_R = data_q.toRotationMatrix();
Eigen::Quaterniond gt_q(gt_ww, gt_xx, gt_yy, gt_zz);
Eigen::Matrix3d gt_R = gt_q.toRotationMatrix();
file << fixed << data_R(0,0) << " " << data_R(0,1) << " " << data_R(0,2) << " " << data_x << " "
<< data_R(1,0) << " " << data_R(1,1) << " " << data_R(1,2) << " " << data_y << " "
<< data_R(2,0) << " " << data_R(2,1) << " " << data_R(2,2) << " " << data_z << "\n";
gt_file <<fixed << gt_R(0,0) << " " << gt_R(0,1) << " " << gt_R(0,2) << " " << gt_x << " "
<< gt_R(1,0) << " " << gt_R(1,1) << " " << gt_R(1,2) << " " << gt_y << " "
<< gt_R(2,0) << " " << gt_R(2,1) << " " << gt_R(2,2) << " " << gt_z << "\n";
}
转存成TUM格式
map_file << fixed << data.header.stamp.toSec()-stamp_init << " " << data.pose.pose.position.x << " " << data.pose.pose.position.y << " "
<< data.pose.pose.position.z << " " << data.pose.pose.orientation.x << " " << data.pose.pose.orientation.y << " "
<< data.pose.pose.orientation.z << " " << data.pose.pose.orientation.w << std::endl;
3 使用EVO评估欧拉法和中值法的精度
motion1: static
欧拉法 中值法
motion2: speed up
欧拉法 中值法
motion3: speed constant
欧拉法 中值法
motion4: speed up & down
欧拉法 中值法
motion5: 8 circle
欧拉法 中值法
总结：
1、静止和匀速运动，欧拉法和中值法的差异较小，这是由于imu测得的加速度接近0，速度的变化也较小，所以对于中值法和欧拉法来说差异较小
2、加速运动和加减速运动，中值法的精度大于欧拉法，这是由于imu测得的加速度不再是0，速度的变化较大，所以中值法更为合理，误差相对欧拉法较小
扫一扫
7689
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
