转载自：https://blog.csdn.net/qq_36170626/article/details/108498533

IMU与GPS传感器ESKF融合定位

纷繁中淡定 2020-09-09 18:29:38

1800

分类专栏：手写VIO 文章标签： slam

版权

IMU与GPS传感器ESKF融合定位

文章目录

IMU与GPS传感器ESKF融合定位
- 1、代码整体框架说明
- 2、主要函数介绍
- - 2.1 LocalizationWrapper构造函数
  - 2.2 滤波算法进行预测
  - 2.3 通过GPS位置测量数据更新系统的状态
- 3. 结果评价
- 参考文献：

分别在两个终端下运行命令：

rosbag play utbm_robocar_dataset_20180502_noimage1.bag
roslaunch imu_gps_localization imu_gps_localization.launch

1、代码整体框架说明

代码具体结构框架如下图所示：

cpp文件	文件具体实现作用	备注
localization_node.cpp	主函数文件，实现初始化ros，初始化LocalizationWrapper类
localization_wrapper.h	LocalizationWrapper类定义，主要声明构造函数、析构函数以及imu回调函数、gps回调函数、打印状态、打印gps数据，把状态格式数据转换成ros话题机制
localization_wrapper.cpp	构造函数中主要是加载参数，保存数据到文件夹下，订阅imu和gps的话题然后发布融合后的轨迹；析构函数主要是释放空间；imu回调函数主要是读取imu数据，然后对数据进行处理，转变状态发布成ros话题，打印状态；gps回调函数主要是实现读取数据，然后进行处理，最后打印。
imu_gps_localizer.cpp	ProcessImuData()函数初始化imu数据，对imu数据进行预测，从ENU坐标系转换到GPS坐标系，发布融合后的状态；ProcessGpsPositionData()函数主要实现初始化数据以及位置的更新。
imu_gps_localizer.h	ImuGpsLocalizer类构造函数，声明ProcessImuData()函数，声明ProcessGpsPositionData()函数
initializer.h	初始化类，主要是构造函数，添加imu数据，添加gps数据，计算imu数据到gps数据的旋转矩阵这里参考的是openvins
imu_processor.cpp	滤波融合的预测部分Predict()函数具体实现
imu_processor.h	ImuProcessor类主要是滤波融合的预测部分Predict()函数声明
gps_processor.h	GpsProcessor类通过GPS位置更新系统的状态UpdateStateByGpsPosition()以及计算残差以及雅可比矩阵和把得到的误差更新状态
base_type.h	主要是imu数据、GPS数据以及系统状态的数据结构
utils.h	主要包括弧度和角度之间的相互转换、ENU坐标系与LLA坐标系的转换以及得到反对称矩阵

2、主要函数介绍

2.1 LocalizationWrapper构造函数

LocalizationWrapper构造函数主要实现加载参数、添加数据保存位置、订阅话题和发布话题

LocalizationWrapper::LocalizationWrapper(ros::NodeHandle& nh) {// Load configs.double acc_noise, gyro_noise, acc_bias_noise, gyro_bias_noise;nh.param("acc_noise",       acc_noise, 1e-2);nh.param("gyro_noise",      gyro_noise, 1e-4);nh.param("acc_bias_noise",  acc_bias_noise, 1e-6);nh.param("gyro_bias_noise", gyro_bias_noise, 1e-8);double x, y, z;nh.param("I_p_Gps_x", x, 0.);nh.param("I_p_Gps_y", y, 0.);nh.param("I_p_Gps_z", z, 0.);const Eigen::Vector3d I_p_Gps(x, y, z);std::string log_folder= "/home/sunshine/catkin_imu_gps_localization/src/imu_gps_localization";ros::param::get("log_folder", log_folder);// Log.file_state_.open(log_folder + "/state.csv");file_gps_.open(log_folder +"/gps.csv");// Initialization imu gps localizer.imu_gps_localizer_ptr_ = std::make_unique<ImuGpsLocalization::ImuGpsLocalizer>(acc_noise, gyro_noise,acc_bias_noise, gyro_bias_noise,I_p_Gps);// Subscribe topics.imu_sub_ = nh.subscribe("/imu/data", 10,  &LocalizationWrapper::ImuCallback, this);//TODO 运行数据集需要修改的地方： gps的话题为/fixgps_position_sub_ = nh.subscribe("/fix", 10,  &LocalizationWrapper::GpsPositionCallback, this);//发布融合后的轨迹state_pub_ = nh.advertise<nav_msgs::Path>("fused_path", 10);
}

2.2 滤波算法进行预测

void ImuProcessor::Predict(const ImuDataPtr last_imu, const ImuDataPtr cur_imu, State* state) {// Time.const double delta_t = cur_imu->timestamp - last_imu->timestamp;const double delta_t2 = delta_t * delta_t;// Set last state.State last_state = *state;// mid_point integration methods// Acc and gyro.const Eigen::Vector3d acc_unbias = 0.5 * (last_imu->acc + cur_imu->acc) - last_state.acc_bias;const Eigen::Vector3d gyro_unbias = 0.5 * (last_imu->gyro + cur_imu->gyro) - last_state.gyro_bias;// Normal state. // Using P58. of "Quaternion kinematics for the error-state Kalman Filter".state->G_p_I = last_state.G_p_I + last_state.G_v_I * delta_t + 0.5 * (last_state.G_R_I * acc_unbias + gravity_) * delta_t2;state->G_v_I = last_state.G_v_I + (last_state.G_R_I * acc_unbias + gravity_) * delta_t;const Eigen::Vector3d delta_angle_axis = gyro_unbias * delta_t;if (delta_angle_axis.norm() > 1e-12) {// std::cout << "norm" << delta_angle_axis.norm() << "normlized"//           << delta_angle_axis.normalized() << std::endl;state->G_R_I = last_state.G_R_I* Eigen::AngleAxisd(delta_angle_axis.norm(),delta_angle_axis.normalized()).toRotationMatrix();}// Error-state. Not needed.// Covariance of the error-state.   Eigen::Matrix<double, 15, 15> Fx = Eigen::Matrix<double, 15, 15>::Identity();Fx.block<3, 3>(0, 3)   = Eigen::Matrix3d::Identity() * delta_t;Fx.block<3, 3>(3, 6)   = - state->G_R_I * GetSkewMatrix(acc_unbias) * delta_t;Fx.block<3, 3>(3, 9)   = - state->G_R_I * delta_t;if (delta_angle_axis.norm() > 1e-12) {Fx.block<3, 3>(6, 6) = Eigen::AngleAxisd(delta_angle_axis.norm(), delta_angle_axis.normalized()).toRotationMatrix().transpose();} else {Fx.block<3, 3>(6, 6).setIdentity();}Fx.block<3, 3>(6, 12)  = - Eigen::Matrix3d::Identity() * delta_t;Eigen::Matrix<double, 15, 12> Fi = Eigen::Matrix<double, 15, 12>::Zero();Fi.block<12, 12>(3, 0) = Eigen::Matrix<double, 12, 12>::Identity();Eigen::Matrix<double, 12, 12> Qi = Eigen::Matrix<double, 12, 12>::Zero();Qi.block<3, 3>(0, 0) = delta_t2 * acc_noise_ * Eigen::Matrix3d::Identity();Qi.block<3, 3>(3, 3) = delta_t2 * gyro_noise_ * Eigen::Matrix3d::Identity();Qi.block<3, 3>(6, 6) = delta_t * acc_bias_noise_ * Eigen::Matrix3d::Identity();Qi.block<3, 3>(9, 9) = delta_t * gyro_bias_noise_ * Eigen::Matrix3d::Identity();//协方差预测state->cov = Fx * last_state.cov * Fx.transpose() + Fi * Qi * Fi.transpose();// Time and imu.state->timestamp = cur_imu->timestamp;state->imu_data_ptr = cur_imu;
}

2.3 通过GPS位置测量数据更新系统的状态

bool GpsProcessor::UpdateStateByGpsPosition(const Eigen::Vector3d& init_lla, const GpsPositionDataPtr gps_data_ptr, State* state) {Eigen::Matrix<double, 3, 15> H;Eigen::Vector3d residual;ComputeJacobianAndResidual(init_lla, gps_data_ptr, *state, &H, &residual);const Eigen::Matrix3d& V = gps_data_ptr->cov;// EKF.const Eigen::MatrixXd& P = state->cov;//P：状态的协方差矩阵；V：gps数据的协方差矩阵；H：误差状态的雅可比矩阵const Eigen::MatrixXd K = P * H.transpose() * (H * P * H.transpose() + V).inverse();const Eigen::VectorXd delta_x = K * residual;// Add delta_x to state.AddDeltaToState(delta_x, state);// Covarance.const Eigen::MatrixXd I_KH = Eigen::Matrix<double, 15, 15>::Identity() - K * H;state->cov = I_KH * P * I_KH.transpose() + K * V * K.transpose();
}
//计算雅克比矩阵以及残差项 P61 Quaternion kinematics for the error-state Kalman filter
void GpsProcessor::ComputeJacobianAndResidual(const Eigen::Vector3d& init_lla,  const GpsPositionDataPtr gps_data, const State& state,Eigen::Matrix<double, 3, 15>* jacobian,Eigen::Vector3d* residual) {const Eigen::Vector3d& G_p_I   = state.G_p_I;const Eigen::Matrix3d& G_R_I   = state.G_R_I;// Convert wgs84 to ENU frame.Eigen::Vector3d G_p_Gps;ConvertLLAToENU(init_lla, gps_data->lla, &G_p_Gps);// Compute residual.*residual = G_p_Gps - (G_p_I + G_R_I * I_p_Gps_);// Compute jacobian.？？？对哪一项求导？jacobian->setZero();jacobian->block<3, 3>(0, 0)  = Eigen::Matrix3d::Identity();jacobian->block<3, 3>(0, 6)  = - G_R_I * GetSkewMatrix(I_p_Gps_);
}
//添加误差项到状态
void AddDeltaToState(const Eigen::Matrix<double, 15, 1>& delta_x, State* state) {state->G_p_I     += delta_x.block<3, 1>(0, 0);state->G_v_I     += delta_x.block<3, 1>(3, 0);state->acc_bias  += delta_x.block<3, 1>(9, 0);state->gyro_bias += delta_x.block<3, 1>(12, 0);if (delta_x.block<3, 1>(6, 0).norm() > 1e-12) {state->G_R_I *= Eigen::AngleAxisd(delta_x.block<3, 1>(6, 0).norm(), delta_x.block<3, 1>(6, 0).normalized()).toRotationMatrix();}
}

3. 结果评价

参考文献：

IMU & GPS定位 Quaternion kinematics for ESKF[part 3]

EU Long-term Dataset with Multiple Sensors for Autonomous Driving

Quaternion kinematics for the error-state Kalman filter

Woosik Lee, Intermittent GPS-aided VIO: Online Initialization and Calibration.

A General Optimization-based Framework for Global Pose Estimation with Multiple Sensors

A General Optimization-based Framework for Local Odometry Estimation with Multiple Sensors