IMU和GPS融合定位（ESKF）
说明
1.本文理论部分参考文章https://zhuanlan.zhihu.com/p/152662055和https://blog.csdn.net/brightming/article/details/118057262
ROS下的实践参考https://blog.csdn.net/qinqinxiansheng/article/details/107108475和https://zhuanlan.zhihu.com/p/163038275
理论
坐标系
error-state
ESKF GPS更新
初始化
初始位置为0,方向的roll和pitch可由加速度的重力方向确定,bias设置为0.
代码
代码请见github：https://github.com/ydsf16/imu_gps_localization
总体流程如下：接收IMU数据，进行积分，更新系统方程，接收GPS数据，计算K，更新状态量
代码整体框架说明
代码具体结构框架如下图所示：
主要函数介绍
1）状态定义：
struct State {
double timestamp;
Eigen::Vector3d lla; // WGS84 position.
Eigen::Vector3d G_p_I; // The original point of the IMU frame in the Global frame.
Eigen::Vector3d G_v_I; // The velocity original point of the IMU frame in the Global frame.
Eigen::Matrix3d G_R_I; // The rotation from the IMU frame to the Global frame.
Eigen::Vector3d acc_bias; // The bias of the acceleration sensor.
Eigen::Vector3d gyro_bias; // The bias of the gyroscope sensor.
// Covariance.
Eigen::Matrix<double, 15, 15> cov;
// The imu data.
ImuDataPtr imu_data_ptr;
};
包含：
Eigen::Vector3d G_p_I; // The original point of the IMU frame in the Global frame.
Eigen::Vector3d G_v_I; // The velocity original point of the IMU frame in the Global frame.
Eigen::Matrix3d G_R_I; // The rotation from the IMU frame to the Global frame.
Eigen::Vector3d acc_bias; // The bias of the acceleration sensor.
Eigen::Vector3d gyro_bias; // The bias of the gyroscope sensor.
5个状态量，在协方差表示时，旋转也用三维的旋转角表示，所以，其协方差矩阵为15。
2） 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的话题为/fix
gps_position_sub_ = nh.subscribe("/fix", 10, &LocalizationWrapper::GpsPositionCallback, this);
//发布融合后的轨迹
state_pub_ = nh.advertise<nav_msgs::Path>("fused_path", 10);
}
3）滤波算法进行预测
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;
}
predict主要是对nominal state的运动学估计，以及对协方差的递推（为了在观测值来的时候，结合两者的协方差算出K值）。
协方差传递的Fx的计算，与《Quaternion Kinematics for the error-state KF》中的一致：
（误差的传递与nominal state的传递都是一样的，遵循的都是相同的运动学模型，只是误差会在之前的数值的基础上继续包括新增的各种运动误差，如imu的bias，随机游走等，而nominal则不管这些值，按照正常的运动学模型递推。）
accelerometer_noise_density: 0.012576 #continous
accelerometer_random_walk: 0.000232
gyroscope_noise_density: 0.0012615 #continous
gyroscope_random_walk: 0.0000075
代码中是这样设置的：
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();
这些参数，代码中是在初始化时设置的：
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);
<launch>
<param name="acc_noise" type="double" value="1e-2" />
<param name="gyro_noise" type="double" value="1e-4" />
<param name="acc_bias_noise" type="double" value="1e-6" />
<param name="gyro_bias_noise" type="double" value="1e-8" />
<param name="I_p_Gps_x" type="double" value="0.0" />
<param name="I_p_Gps_y" type="double" value="0.0" />
<param name="I_p_Gps_z" type="double" value="0.0" />
<param name="log_folder" type="string" value="$(find imu_gps_localization)" />
<node name="nmea_topic_driver" pkg="nmea_navsat_driver" type="nmea_topic_driver" output="screen" />
<node name="imu_gps_localization_node" pkg="imu_gps_localization" type="imu_gps_localization_node" output="screen" />
<node pkg="rviz" type="rviz" name="rviz" output="screen"
args="-d $(find imu_gps_localization)/ros_wrapper/rviz/default.rviz" required="true">
</node>
</launch>
代码中：
state->cov = Fx * last_state.cov * Fx.transpose() + Fi * Qi * Fi.transpose();
也就是以下代码实现：
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.
//I_p_Gps_在imu坐标系下的位移？是固定值？
//G_p_I + G_R_I * I_p_Gps_预测的状态计算出来的坐标点
*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_);
}
第二项，其实就是真值对误差项目的求导，看下表：
// 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_);
通过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：观测数据（gps数据）对误差状态的雅可比矩阵
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();
}
}