/*
* Copyright 2018-2019 Autoware Foundation. All rights reserved.
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "ekf_localizer/ekf_localizer.h"
// clang-format off
#define PRINT_MAT(X) std::cout << #X << ":\n" << X << std::endl << std::endl
#define DEBUG_INFO(...) { if (show_debug_info_) { ROS_INFO(__VA_ARGS__); } }
#define DEBUG_PRINT_MAT(X) { if (show_debug_info_) { std::cout << #X << ": " << X << std::endl; } }
// clang-format on
/*
x, y：机器人位置。
yaw：机器人朝向（偏航角）。
yaw_bias：偏航角偏差（用于估计传感器误差）。
vx, wz：线速度和角速度。
*/
EKFLocalizer::EKFLocalizer() : nh_(""), pnh_("~"), dim_x_(8 /* x, y, yaw, yaw_bias, vx, wz */)
{
pnh_.param("show_debug_info", show_debug_info_, bool(false)); // 是否显示调试信息
pnh_.param("predict_frequency", ekf_rate_, double(50.0)); // EKF 预测频率（Hz）
ekf_dt_ = 1.0 / std::max(ekf_rate_, 0.1); // 计算时间步长（秒）
pnh_.param("enable_yaw_bias_estimation", enable_yaw_bias_estimation_, bool(true)); // 是否估计偏航角偏差
pnh_.param("extend_state_step", extend_state_step_, int(50)); // 状态扩展步数（用于滑动窗口优化）
pnh_.param("pose_frame_id", pose_frame_id_, std::string("map")); // 输出位姿的坐标系 ID
pnh_.param("output_frame_id", output_frame_id_, std::string("base_link")); // 输出坐标系
/* pose measurement 位姿测量参数*/
pnh_.param("pose_additional_delay", pose_additional_delay_, double(0.0)); // 额外延迟（秒）
pnh_.param("pose_measure_uncertainty_time", pose_measure_uncertainty_time_, double(0.01)); // 测量不确定性时间
pnh_.param("pose_rate", pose_rate_, double(10.0)); // 位姿测量频率（用于协方差计算） // used for covariance calculation
pnh_.param("pose_gate_dist", pose_gate_dist_, double(10000.0)); // 马氏距离阈值（异常值过滤） // Mahalanobis limit
pnh_.param("pose_stddev_x", pose_stddev_x_, double(0.05)); // X 方向标准差（米）
pnh_.param("pose_stddev_y", pose_stddev_y_, double(0.05)); // Y 方向标准差（米）
pnh_.param("pose_stddev_yaw", pose_stddev_yaw_, double(0.035)); // 偏航角标准差（弧度）
pnh_.param("use_pose_with_covariance", use_pose_with_covariance_, bool(false)); // 是否使用带协方差的位姿输入
/* twist measurement 速度测量参数*/
pnh_.param("twist_additional_delay", twist_additional_delay_, double(0.0)); // 额外延迟（秒）
pnh_.param("twist_rate", twist_rate_, double(10.0)); // 速度测量频率（用于协方差计算） // used for covariance calculation
pnh_.param("twist_gate_dist", twist_gate_dist_, double(10000.0)); // 马氏距离阈值（异常值过滤） // Mahalanobis limit
pnh_.param("twist_stddev_vx", twist_stddev_vx_, double(0.2)); // 线速度标准差（米/秒）
pnh_.param("twist_stddev_wz", twist_stddev_wz_, double(0.03)); // 角速度标准差（弧度/秒）
pnh_.param("use_twist_with_covariance", use_twist_with_covariance_, bool(false)); // 是否使用带协方差的速度输入
/* IMU measurement parameters */
pnh_.param("use_imu", use_imu_, bool(true));
pnh_.param("imu_rate", imu_rate_, double(50.0));
pnh_.param("imu_gate_dist", imu_gate_dist_, double(10000.0));
pnh_.param("imu_stddev_ax", imu_stddev_ax_, double(0.5));
pnh_.param("imu_stddev_wz", imu_stddev_wz_, double(0.01));
/* process noise 过程噪声参数*/
double proc_stddev_yaw_c, proc_stddev_yaw_bias_c, proc_stddev_vx_c, proc_stddev_wz_c;
double proc_stddev_ax_c, proc_stddev_wz_imu_c;
pnh_.param("proc_stddev_yaw_c", proc_stddev_yaw_c, double(0.005)); // 偏航角过程噪声（连续时间）
pnh_.param("proc_stddev_yaw_bias_c", proc_stddev_yaw_bias_c, double(0.001)); // 偏航角偏差过程噪声
pnh_.param("proc_stddev_vx_c", proc_stddev_vx_c, double(2.0)); // 线速度过程噪声
pnh_.param("proc_stddev_wz_c", proc_stddev_wz_c, double(0.2)); // 角速度过程噪声
if (!enable_yaw_bias_estimation_)
{
proc_stddev_yaw_bias_c = 0.0;
}
/* convert to continuous to discrete 转换为离散时间噪声（乘以时间步长）*/
proc_cov_vx_d_ = std::pow(proc_stddev_vx_c, 2.0) * ekf_dt_;
proc_cov_wz_d_ = std::pow(proc_stddev_wz_c, 2.0) * ekf_dt_;
proc_cov_yaw_d_ = std::pow(proc_stddev_yaw_c, 2.0) * ekf_dt_;
proc_cov_yaw_bias_d_ = std::pow(proc_stddev_yaw_bias_c, 2.0) * ekf_dt_;
proc_cov_ax_d_ = std::pow(proc_stddev_ax_c, 2.0) * ekf_dt_;
proc_cov_wz_imu_d_ = std::pow(proc_stddev_wz_imu_c, 2.0) * ekf_dt_;
/* initialize ros system */
// 定时器（用于 EKF 预测步）
timer_control_ = nh_.createTimer(ros::Duration(ekf_dt_), &EKFLocalizer::timerCallback, this);
// 发布话题
//pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ekf_pose", 1);
pub_pose_ = nh_.advertise<geometry_msgs::PoseStamped>("/ndt_pose", 10);
pub_pose_cov_ = nh_.advertise<geometry_msgs::PoseWithCovarianceStamped>("ekf_pose_with_covariance", 10);
//pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/ekf_twist", 1);
pub_twist_ = nh_.advertise<geometry_msgs::TwistStamped>("/estimate_twist", 10);
pub_twist_cov_ = nh_.advertise<geometry_msgs::TwistWithCovarianceStamped>("ekf_twist_with_covariance", 10);
pub_yaw_bias_ = pnh_.advertise<std_msgs::Float64>("estimated_yaw_bias", 10);
// 订阅话题
sub_initialpose_ = nh_.subscribe("initialpose", 10, &EKFLocalizer::callbackInitialPose, this);
sub_pose_with_cov_ = nh_.subscribe("in_pose_with_covariance", 10, &EKFLocalizer::callbackPoseWithCovariance, this);
sub_pose_ = nh_.subscribe("/in_pose", 10, &EKFLocalizer::callbackPose, this);
sub_twist_with_cov_ = nh_.subscribe("in_twist_with_covariance", 10, &EKFLocalizer::callbackTwistWithCovariance, this);
//sub_twist_ = nh_.subscribe("/can_info", 10, &EKFLocalizer::callbackTwist, this);
imu_sub_.subscribe(nh_, "/imu_raw", 100);
vehicle_sub_.subscribe(nh_, "/can_info", 50);
sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10));
sync_->connectInput(imu_sub_, vehicle_sub_);
sync_->registerCallback(boost::bind(&EKFLocalizer::sensorCallback, this, _1, _2));
sync_->setMaxIntervalDuration(ros::Duration(0.003)); // 3ms容差
dim_x_ex_ = dim_x_ * extend_state_step_; // 扩展状态维度（用于滑动窗口优化）
initEKF(); // 初始化 EKF 内部状态
last_timer_call_time_ = 0.0;
/* debug */
pub_debug_ = pnh_.advertise<std_msgs::Float64MultiArray>("debug", 1); // 调试信息（数组）
pub_measured_pose_ = pnh_.advertise<geometry_msgs::PoseStamped>("debug/measured_pose", 1); // 调试用测量位姿
pub_measured_imu_ = pnh_.advertise<sensor_msgs::Imu>("debug/measured_imu", 1);
};
EKFLocalizer::~EKFLocalizer(){};
/*
* timerCallback
*/
void EKFLocalizer::timerCallback(const ros::TimerEvent& e)
{
DEBUG_INFO("========================= timer called =========================");
/* predict model in EKF 预测步（Predict）*/
auto start = std::chrono::system_clock::now();
DEBUG_INFO("------------------------- start prediction -------------------------");
double actual_dt = (last_timer_call_time_ > 0.0) ? (ros::Time::now().toSec() - last_timer_call_time_) : ekf_dt_;
predictKinematicsModel(actual_dt); // 执行运动模型预测
double elapsed =
std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count(); // 计算耗时
DEBUG_INFO("[EKF] predictKinematicsModel calculation time = %f [ms]", elapsed * 1.0e-6);
DEBUG_INFO("------------------------- end prediction -------------------------\n");
/* pose measurement update */
if (current_pose_ptr_ != nullptr) // 位姿更新（当有新位姿数据时）
{
DEBUG_INFO("------------------------- start Pose -------------------------");
start = std::chrono::system_clock::now();
measurementUpdatePose(*current_pose_ptr_); // 融合传感器位姿数据
elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
DEBUG_INFO("[EKF] measurementUpdatePose calculation time = %f [ms]", elapsed * 1.0e-6);
DEBUG_INFO("------------------------- end Pose -------------------------\n");
}
/* twist measurement update */
if (current_twist_ptr_ != nullptr) // 速度更新（当有新速度数据时）
{
DEBUG_INFO("------------------------- start twist -------------------------");
start = std::chrono::system_clock::now();
measurementUpdateTwist(*current_twist_ptr_); // 融合速度数据
elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
DEBUG_INFO("[EKF] measurementUpdateTwist calculation time = %f [ms]", elapsed * 1.0e-6);
DEBUG_INFO("------------------------- end twist -------------------------\n");
}
/* IMU measurement update */
if (use_imu_ && current_imu_ptr_ != nullptr)
{
DEBUG_INFO("------------------------- start IMU -------------------------");
start = std::chrono::system_clock::now();
measurementUpdateIMU(*current_imu_ptr_);
elapsed = std::chrono::duration_cast<std::chrono::nanoseconds>(std::chrono::system_clock::now() - start).count();
DEBUG_INFO("[EKF] measurementUpdateIMU calculation time = %f [ms]", elapsed * 1.0e-6);
DEBUG_INFO("------------------------- end IMU -------------------------\n");
}
/* set current pose, twist */
setCurrentResult(); // 更新内部状态
last_timer_call_time_ = ros::Time::now().toSec();
/* publish ekf result */
publishEstimateResult(); // 发布最终估计结果
}
void EKFLocalizer::showCurrentX()
{
// 检查调试信息显示标志是否开启
if (show_debug_info_)
{
// 创建临时矩阵存储状态向量
Eigen::MatrixXd X(dim_x_, 1);
// 从EKF获取最新状态估计值
ekf_.getLatestX(X);
// 打印转置后的状态向量（行向量形式）
DEBUG_PRINT_MAT(X.transpose());
}
}
/*
* setCurrentResult
*/
void EKFLocalizer::setCurrentResult()
{
current_ekf_pose_.header.frame_id = pose_frame_id_;
current_ekf_pose_.header.stamp = ros::Time::now();
current_ekf_pose_.pose.position.x = ekf_.getXelement(IDX::X);
current_ekf_pose_.pose.position.y = ekf_.getXelement(IDX::Y);
tf2::Quaternion q_tf;
double roll, pitch, yaw;
if (current_pose_ptr_ != nullptr)
{
current_ekf_pose_.pose.position.z = current_pose_ptr_->pose.position.z;
tf2::fromMsg(current_pose_ptr_->pose.orientation, q_tf); /* use Pose pitch and roll */
tf2::Matrix3x3(q_tf).getRPY(roll, pitch, yaw);
}
else
{
current_ekf_pose_.pose.position.z = 0.0;
roll = 0;
pitch = 0;
}
yaw = ekf_.getXelement(IDX::YAW) + ekf_.getXelement(IDX::YAWB);
q_tf.setRPY(roll, pitch, yaw);
tf2::convert(q_tf, current_ekf_pose_.pose.orientation);
current_ekf_twist_.header.frame_id = output_frame_id_;
current_ekf_twist_.header.stamp = ros::Time::now();
current_ekf_twist_.twist.linear.x = ekf_.getXelement(IDX::VX);
current_ekf_twist_.twist.angular.z = ekf_.getXelement(IDX::WZ) + ekf_.getXelement(IDX::WZ_IMU);
}
/*
* broadcastTF
*/
void EKFLocalizer::broadcastTF(ros::Time time)
{
// if (current_ekf_pose_.header.frame_id == "")
// {
// return;
// }
// tf::Transform transform;
// transform.setOrigin(tf::Vector3(current_ekf_pose_.pose.position.x, current_ekf_pose_.pose.position.y, current_ekf_pose_.pose.position.z));
// tf::Quaternion current_q(
// current_ekf_pose_.pose.orientation.x,
// current_ekf_pose_.pose.orientation.y,
// current_ekf_pose_.pose.orientation.z,
// current_ekf_pose_.pose.orientation.w
// );
// transform.setRotation(current_q);
// tf_br_.sendTransform(tf::StampedTransform(transform, time, "/map", output_frame_id_));
if (current_ekf_pose_.header.frame_id == "")
{
return;
}
geometry_msgs::TransformStamped transformStamped;
transformStamped.header = current_ekf_pose_.header;
transformStamped.child_frame_id = output_frame_id_;
transformStamped.transform.translation.x = current_ekf_pose_.pose.position.x;
transformStamped.transform.translation.y = current_ekf_pose_.pose.position.y;
transformStamped.transform.translation.z = current_ekf_pose_.pose.position.z;
transformStamped.transform.rotation.x = current_ekf_pose_.pose.orientation.x;
transformStamped.transform.rotation.y = current_ekf_pose_.pose.orientation.y;
transformStamped.transform.rotation.z = current_ekf_pose_.pose.orientation.z;
transformStamped.transform.rotation.w = current_ekf_pose_.pose.orientation.w;
tf_br_.sendTransform(transformStamped);
}
/*
* getTransformFromTF
*/
bool EKFLocalizer::getTransformFromTF(std::string parent_frame, std::string child_frame,
geometry_msgs::TransformStamped& transform)
{
// tf::TransformListener listener;
// for (int i = 0; i < 50; ++i)
// {
// try
// {
// auto now = ros::Time(0);
// listener.waitForTransform(parent_frame, child_frame, now, ros::Duration(10.0));
// listener.lookupTransform(parent_frame, child_frame, now, transform);
// return true;
// }
// catch (tf::TransformException& ex)
// {
// ROS_ERROR("%s", ex.what());
// ros::Duration(0.1).sleep();
// }
// }
// return false;
tf2_ros::Buffer tf_buffer;
tf2_ros::TransformListener tf_listener(tf_buffer);
ros::Duration(0.1).sleep();
if (parent_frame.front() == '/')
parent_frame.erase(0, 1);
if (child_frame.front() == '/')
child_frame.erase(0, 1);
for (int i = 0; i < 50; ++i)
{
try
{
transform = tf_buffer.lookupTransform(parent_frame, child_frame, ros::Time(0));
return true;
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
ros::Duration(0.1).sleep();
}
}
return false;
}
/*
* callbackInitialPose
*/
void EKFLocalizer::callbackInitialPose(const geometry_msgs::PoseWithCovarianceStamped& initialpose)
{
// (1) 获取 TF 变换
// tf::StampedTransform transform;
// if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform))
// {
// ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s",
// pose_frame_id_.c_str(), initialpose.header.frame_id.c_str());
// return; // 必须返回，避免使用无效变换
// }
// // (2) 初始化状态向量 X
// Eigen::MatrixXd X(dim_x_, 1);
// X.setZero(); // 显式初始化所有状态为 0
// // 将 initialpose 变换到 pose_frame_id_ 坐标系
// tf::Pose tf_initial_pose;
// tf::poseMsgToTF(initialpose.pose.pose, tf_initial_pose);
// tf::Pose transformed_pose = transform * tf_initial_pose; // 正确应用 TF 变换
// X(IDX::X) = transformed_pose.getOrigin().x();
// X(IDX::Y) = transformed_pose.getOrigin().y();
// X(IDX::YAW) = tf::getYaw(transformed_pose.getRotation());
// X(IDX::YAWB) = 0.0; // 偏航角偏差初始化为 0
// X(IDX::VX) = 0.0; // 速度初始化为 0
// X(IDX::WZ) = 0.0; // 角速度初始化为 0
// X(IDX::AX) = 0.0; // 加速度初始化为 0
// X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0
// // (3) 初始化协方差矩阵 P
// Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
// const auto& cov = initialpose.pose.covariance;
// // 检查协方差矩阵是否有效（非负且非全零）
// if (cov[0] > 0.0) P(IDX::X, IDX::X) = cov[0]; // X variance
// if (cov[7] > 0.0) P(IDX::Y, IDX::Y) = cov[7]; // Y variance
// if (cov[35] > 0.0) P(IDX::YAW, IDX::YAW) = cov[35]; // YAW variance
// // 其他状态的协方差（默认值）
// P(IDX::YAWB, IDX::YAWB) = 0.0001; // 偏航角偏差
// P(IDX::VX, IDX::VX) = 0.01; // 速度
// P(IDX::WZ, IDX::WZ) = 0.01; // 角速度
// P(IDX::AX, IDX::AX) = 0.01; // 加速度
// P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度
// // (4) 初始化 EKF
// ekf_.init(X, P, extend_state_step_);
geometry_msgs::TransformStamped transform;
if (!getTransformFromTF(pose_frame_id_, initialpose.header.frame_id, transform))
{
ROS_ERROR("[EKF] TF transform failed. parent = %s, child = %s", pose_frame_id_.c_str(),
initialpose.header.frame_id.c_str());
};
Eigen::MatrixXd X(dim_x_, 1);
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
X(IDX::X) = initialpose.pose.pose.position.x /* + transform.transform.translation.x */;
X(IDX::Y) = initialpose.pose.pose.position.y /* + transform.transform.translation.y */;
X(IDX::YAW) = tf2::getYaw(initialpose.pose.pose.orientation) /* + tf2::getYaw(transform.transform.rotation) */;
X(IDX::YAWB) = 0.0;
X(IDX::VX) = 0.0;
X(IDX::WZ) = 0.0;
X(IDX::AX) = 0.0; // 加速度初始化为 0
X(IDX::WZ_IMU) = 0.0; // IMU 角速度初始化为 0
P(IDX::X, IDX::X) = initialpose.pose.covariance[0];
P(IDX::Y, IDX::Y) = initialpose.pose.covariance[6 + 1];
P(IDX::YAW, IDX::YAW) = initialpose.pose.covariance[6 * 5 + 5];
P(IDX::YAWB, IDX::YAWB) = 0.0001;
P(IDX::VX, IDX::VX) = 0.01;
P(IDX::WZ, IDX::WZ) = 0.01;
P(IDX::AX, IDX::AX) = 0.01; // 加速度
P(IDX::WZ_IMU, IDX::WZ_IMU) = 0.01; // IMU 角速度
ekf_.init(X, P, extend_state_step_);
};
/*
* callbackPose
*/
void EKFLocalizer::callbackPose(const geometry_msgs::PoseStamped::ConstPtr& msg)
{
if (!use_pose_with_covariance_)
{
current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(*msg);
}
};
/*
* callbackPoseWithCovariance
*/
void EKFLocalizer::callbackPoseWithCovariance(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg)
{
if (use_pose_with_covariance_)
{
geometry_msgs::PoseStamped pose;
pose.header = msg->header;
pose.pose = msg->pose.pose;
current_pose_ptr_ = std::make_shared<geometry_msgs::PoseStamped>(pose);
current_pose_covariance_ = msg->pose.covariance;
}
};
/*
* callbackTwist
*/
void EKFLocalizer::sensorCallback(const sensor_msgs::Imu::ConstPtr& imu_msg,
const autoware_can_msgs::CANInfo::ConstPtr& vehicle_msg) {
geometry_msgs::TwistStamped twist_msg;
twist_msg.header = vehicle_msg->header;
twist_msg.header.frame_id = "/base_link"; // 根据实际坐标系设置
// 设置线速度 (来自CAN)
twist_msg.twist.linear.x = (vehicle_msg->speed / 3.6) * cos(vehicle_msg->angle);
twist_msg.twist.linear.y = 0.0;
twist_msg.twist.linear.z = 0.0;
// 设置角速度 (来自IMU)
//twist_msg.twist.angular = imu_msg->angular_velocity;
// 计算运动学模型的角速度
twist_msg.twist.angular.x = 0;
twist_msg.twist.angular.y = 0;
twist_msg.twist.angular.z = ((vehicle_msg->speed / 3.6)*sin(vehicle_msg->angle))/1.137;
current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist_msg);
if (imu_msg->header.frame_id != "imu")
{
ROS_WARN_DELAYED_THROTTLE(2, "IMU frame_id is %s, but expected 'imu'", imu_msg->header.frame_id.c_str());
}
current_imu_ptr_ = std::make_shared<sensor_msgs::Imu>(*imu_msg);
// // 估计IMU零偏
// estimateIMUBias();
}
/*
* callbackTwistWithCovariance
*/
void EKFLocalizer::callbackTwistWithCovariance(const geometry_msgs::TwistWithCovarianceStamped::ConstPtr& msg)
{
if (use_twist_with_covariance_)
{
geometry_msgs::TwistStamped twist;
twist.header = msg->header;
twist.twist = msg->twist.twist;
current_twist_ptr_ = std::make_shared<geometry_msgs::TwistStamped>(twist);
current_twist_covariance_ = msg->twist.covariance;
}
};
/*
* initEKF
*/
void EKFLocalizer::initEKF()
{
Eigen::MatrixXd X = Eigen::MatrixXd::Zero(dim_x_, 1);
Eigen::MatrixXd P = Eigen::MatrixXd::Identity(dim_x_, dim_x_) * 1.0E15; // for x & y
P(IDX::YAW, IDX::YAW) = 50.0; // for yaw
P(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_; // for yaw bias
P(IDX::VX, IDX::VX) = 1000.0; // for vx
P(IDX::WZ, IDX::WZ) = 50.0; // for wz
P(IDX::AX, IDX::AX) = 10.0;
P(IDX::WZ_IMU, IDX::WZ_IMU) = 1.0;
ekf_.init(X, P, extend_state_step_);
}
/*
* predictKinematicsModel
*/
void EKFLocalizer::predictKinematicsModel(double actual_dt)
{
Eigen::MatrixXd X_curr(dim_x_, 1);
Eigen::MatrixXd X_next(dim_x_, 1);
ekf_.getLatestX(X_curr);
DEBUG_PRINT_MAT(X_curr.transpose());
Eigen::MatrixXd P_curr;
ekf_.getLatestP(P_curr);
const double yaw = X_curr(IDX::YAW);
const double yaw_bias = X_curr(IDX::YAWB);
const double vx = X_curr(IDX::VX);
const double wz = X_curr(IDX::WZ);
const double ax = X_curr(IDX::AX);
const double wz_imu = X_curr(IDX::WZ_IMU);
const double dt = actual_dt;
/* Update for latest state */
X_next(IDX::X) = X_curr(IDX::X) + vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt;
X_next(IDX::Y) = X_curr(IDX::Y) + vx * sin(yaw + yaw_bias) * dt + 0.5 * ax * sin(yaw + yaw_bias) * dt * dt;
X_next(IDX::YAW) = X_curr(IDX::YAW) + (wz + wz_imu) * dt;
X_next(IDX::YAWB) = yaw_bias;
X_next(IDX::VX) = vx + ax * dt;
X_next(IDX::WZ) = wz;
X_next(IDX::AX) = ax;
X_next(IDX::WZ_IMU) = wz_imu;
X_next(IDX::YAW) = std::atan2(std::sin(X_next(IDX::YAW)), std::cos(X_next(IDX::YAW)));
/* Set A matrix for latest state */
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(dim_x_, dim_x_);
A(IDX::X, IDX::YAW) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt;
A(IDX::X, IDX::YAWB) = -vx * sin(yaw + yaw_bias) * dt - 0.5 * ax * sin(yaw + yaw_bias) * dt * dt;
A(IDX::X, IDX::VX) = cos(yaw + yaw_bias) * dt;
A(IDX::X, IDX::AX) = 0.5 * cos(yaw + yaw_bias) * dt * dt;
A(IDX::Y, IDX::YAW) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt;
A(IDX::Y, IDX::YAWB) = vx * cos(yaw + yaw_bias) * dt + 0.5 * ax * cos(yaw + yaw_bias) * dt * dt;
A(IDX::Y, IDX::VX) = sin(yaw + yaw_bias) * dt;
A(IDX::Y, IDX::AX) = 0.5 * sin(yaw + yaw_bias) * dt * dt;
A(IDX::YAW, IDX::WZ) = dt;
A(IDX::YAW, IDX::WZ_IMU) = dt;
A(IDX::VX, IDX::AX) = dt;
/* Process noise covariance matrix Q */
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(dim_x_, dim_x_);
// 位置过程噪声（由速度和加速度引起）
const double dvx = std::sqrt(P_curr(IDX::VX, IDX::VX));
const double dax = std::sqrt(P_curr(IDX::AX, IDX::AX));
const double dyaw = std::sqrt(P_curr(IDX::YAW, IDX::YAW));
if (dvx < 10.0 && dyaw < 1.0 && dax < 5.0)
{
Eigen::MatrixXd Jp_pos = Eigen::MatrixXd::Zero(2, 3);
Jp_pos << cos(yaw), -vx*sin(yaw), 0.5*cos(yaw),
sin(yaw), vx*cos(yaw), 0.5*sin(yaw);
Eigen::MatrixXd Q_vx_yaw_ax = Eigen::MatrixXd::Zero(3, 3);
Q_vx_yaw_ax(0, 0) = P_curr(IDX::VX, IDX::VX) * dt;
Q_vx_yaw_ax(1, 1) = P_curr(IDX::YAW, IDX::YAW) * dt;
Q_vx_yaw_ax(2, 2) = P_curr(IDX::AX, IDX::AX) * dt;
Q_vx_yaw_ax(0, 1) = P_curr(IDX::VX, IDX::YAW) * dt;
Q_vx_yaw_ax(1, 0) = P_curr(IDX::YAW, IDX::VX) * dt;
Q_vx_yaw_ax(0, 2) = P_curr(IDX::VX, IDX::AX) * dt;
Q_vx_yaw_ax(2, 0) = P_curr(IDX::AX, IDX::VX) * dt;
Q_vx_yaw_ax(1, 2) = P_curr(IDX::YAW, IDX::AX) * dt;
Q_vx_yaw_ax(2, 1) = P_curr(IDX::AX, IDX::YAW) * dt;
Q.block(0, 0, 2, 2) = Jp_pos * Q_vx_yaw_ax * Jp_pos.transpose();
}
else
{
Q(IDX::X, IDX::X) = 0.1;
Q(IDX::Y, IDX::Y) = 0.1;
}
// 角度过程噪声
Q(IDX::YAW, IDX::YAW) = proc_cov_yaw_d_;
Q(IDX::YAWB, IDX::YAWB) = proc_cov_yaw_bias_d_;
// 速度过程噪声
Q(IDX::VX, IDX::VX) = proc_cov_vx_d_;
Q(IDX::WZ, IDX::WZ) = proc_cov_wz_d_;
// 加速度和IMU角速度过程噪声
Q(IDX::AX, IDX::AX) = proc_cov_ax_d_;
Q(IDX::WZ_IMU, IDX::WZ_IMU) = proc_cov_wz_imu_d_;
ekf_.predictWithDelay(X_next, A, Q);
// debug
Eigen::MatrixXd X_result(dim_x_, 1);
ekf_.getLatestX(X_result);
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
}
/*
* measurementUpdatePose
*/
void EKFLocalizer::measurementUpdatePose(const geometry_msgs::PoseStamped& pose)
{
if (pose.header.frame_id != pose_frame_id_)
{
ROS_WARN_DELAYED_THROTTLE(2, "pose frame_id is %s, but pose_frame is set as %s. They must be same.",
pose.header.frame_id.c_str(), pose_frame_id_.c_str());
}
Eigen::MatrixXd X_curr(dim_x_, 1); // curent state
ekf_.getLatestX(X_curr);
DEBUG_PRINT_MAT(X_curr.transpose());
constexpr int dim_y = 3; // pos_x, pos_y, yaw, depending on Pose output
const ros::Time t_curr = ros::Time::now();
/* Calculate delay step */
double delay_time = (t_curr - pose.header.stamp).toSec() + pose_additional_delay_;
if (delay_time < 0.0)
{
delay_time = 0.0;
ROS_WARN_DELAYED_THROTTLE(1.0, "Pose time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
}
int delay_step = std::roundf(delay_time / ekf_dt_);
if (delay_step > extend_state_step_ - 1)
{
ROS_WARN_DELAYED_THROTTLE(1.0,
"Pose delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
"extend_state_step * ekf_dt : %f [s]",
delay_time, extend_state_step_ * ekf_dt_);
return;
}
DEBUG_INFO("delay_time: %f [s]", delay_time);
/* Set yaw */
const double yaw_curr = ekf_.getXelement((unsigned int)(delay_step * dim_x_ + IDX::YAW));
double yaw = tf2::getYaw(pose.pose.orientation);
const double ekf_yaw = ekf_.getXelement(delay_step * dim_x_ + IDX::YAW);
const double yaw_error = normalizeYaw(yaw - ekf_yaw); // normalize the error not to exceed 2 pi
yaw = yaw_error + ekf_yaw;
/* Set measurement matrix */
Eigen::MatrixXd y(dim_y, 1);
y << pose.pose.position.x, pose.pose.position.y, yaw;
if (isnan(y.array()).any() || isinf(y.array()).any())
{
ROS_WARN("[EKF] pose measurement matrix includes NaN of Inf. ignore update. check pose message.");
return;
}
/* Gate */
Eigen::MatrixXd y_ekf(dim_y, 1);
y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::X), ekf_.getXelement(delay_step * dim_x_ + IDX::Y), ekf_yaw;
Eigen::MatrixXd P_curr, P_y;
ekf_.getLatestP(P_curr);
P_y = P_curr.block(0, 0, dim_y, dim_y);
if (!mahalanobisGate(pose_gate_dist_, y_ekf, y, P_y))
{
ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Pose measurement update, mahalanobis distance is over limit. ignore "
"measurement data.");
return;
}
DEBUG_PRINT_MAT(y.transpose());
DEBUG_PRINT_MAT(y_ekf.transpose());
DEBUG_PRINT_MAT((y - y_ekf).transpose());
/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
C(0, IDX::X) = 1.0; // for pos x
C(1, IDX::Y) = 1.0; // for pos y
C(2, IDX::YAW) = 1.0; // for yaw
/* Set measurement noise covariancs */
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (use_pose_with_covariance_)
{
R(0, 0) = current_pose_covariance_.at(0); // x - x
R(0, 1) = current_pose_covariance_.at(1); // x - y
R(0, 2) = current_pose_covariance_.at(5); // x - yaw
R(1, 0) = current_pose_covariance_.at(6); // y - x
R(1, 1) = current_pose_covariance_.at(7); // y - y
R(1, 2) = current_pose_covariance_.at(11); // y - yaw
R(2, 0) = current_pose_covariance_.at(30); // yaw - x
R(2, 1) = current_pose_covariance_.at(31); // yaw - y
R(2, 2) = current_pose_covariance_.at(35); // yaw - yaw
}
else
{
const double ekf_yaw = ekf_.getXelement(IDX::YAW);
const double vx = ekf_.getXelement(IDX::VX);
const double wz = ekf_.getXelement(IDX::WZ);
const double cov_pos_x = std::pow(pose_measure_uncertainty_time_ * vx * cos(ekf_yaw), 2.0);
const double cov_pos_y = std::pow(pose_measure_uncertainty_time_ * vx * sin(ekf_yaw), 2.0);
const double cov_yaw = std::pow(pose_measure_uncertainty_time_ * wz, 2.0);
R(0, 0) = std::pow(pose_stddev_x_, 2) + cov_pos_x; // pos_x
R(1, 1) = std::pow(pose_stddev_y_, 2) + cov_pos_y; // pos_y
R(2, 2) = std::pow(pose_stddev_yaw_, 2) + cov_yaw; // yaw
}
/* In order to avoid a large change at the time of updating, measuremeent update is performed by dividing at every
* step. */
R *= (ekf_rate_ / pose_rate_);
ekf_.updateWithDelay(y, C, R, delay_step);
// debug
Eigen::MatrixXd X_result(dim_x_, 1);
ekf_.getLatestX(X_result);
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
}
/*
* measurementUpdateTwist
*/
void EKFLocalizer::measurementUpdateTwist(const geometry_msgs::TwistStamped& twist)
{
if (twist.header.frame_id != output_frame_id_)
{
ROS_WARN_DELAYED_THROTTLE(2.0, "twist frame_id must be %s", output_frame_id_.c_str());
}
Eigen::MatrixXd X_curr(dim_x_, 1); // curent state
ekf_.getLatestX(X_curr);
DEBUG_PRINT_MAT(X_curr.transpose());
constexpr int dim_y = 2; // vx, wz
const ros::Time t_curr = ros::Time::now();
/* Calculate delay step */
double delay_time = (t_curr - twist.header.stamp).toSec() + twist_additional_delay_;
if (delay_time < 0.0)
{
ROS_WARN_DELAYED_THROTTLE(1.0, "Twist time stamp is inappropriate (delay = %f [s]), set delay to 0[s].",
delay_time);
delay_time = 0.0;
}
int delay_step = std::roundf(delay_time / ekf_dt_);
if (delay_step > extend_state_step_ - 1)
{
ROS_WARN_DELAYED_THROTTLE(1.0,
"Twist delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
"extend_state_step * ekf_dt : %f [s]",
delay_time, extend_state_step_ * ekf_dt_);
return;
}
DEBUG_INFO("delay_time: %f [s]", delay_time);
/* Set measurement matrix */
Eigen::MatrixXd y(dim_y, 1);
y << twist.twist.linear.x, twist.twist.angular.z;
if (isnan(y.array()).any() || isinf(y.array()).any())
{
ROS_WARN("[EKF] twist measurement matrix includes NaN of Inf. ignore update. check twist message.");
return;
}
/* Gate */
Eigen::MatrixXd y_ekf(dim_y, 1);
y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::VX), ekf_.getXelement(delay_step * dim_x_ + IDX::WZ);
Eigen::MatrixXd P_curr, P_y;
ekf_.getLatestP(P_curr);
P_y = P_curr.block(4, 4, dim_y, dim_y);
if (!mahalanobisGate(twist_gate_dist_, y_ekf, y, P_y))
{
ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] Twist measurement update, mahalanobis distance is over limit. ignore "
"measurement data.");
return;
}
DEBUG_PRINT_MAT(y.transpose());
DEBUG_PRINT_MAT(y_ekf.transpose());
DEBUG_PRINT_MAT((y - y_ekf).transpose());
/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
C(0, IDX::VX) = 1.0; // for vx
C(1, IDX::WZ) = 1.0; // for wz
/* Set measurement noise covariancs */
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
if (use_twist_with_covariance_)
{
R(0, 0) = current_twist_covariance_.at(0); // vx - vx
R(0, 1) = current_twist_covariance_.at(5); // vx - wz
R(1, 0) = current_twist_covariance_.at(30); // wz - vx
R(1, 1) = current_twist_covariance_.at(35); // wz - wz
}
else
{
R(0, 0) = twist_stddev_vx_ * twist_stddev_vx_ * ekf_dt_; // for vx
R(1, 1) = twist_stddev_wz_ * twist_stddev_wz_ * ekf_dt_; // for wz
}
/* In order to avoid a large change by update, measurement update is performed by dividing at every step. */
R *= (ekf_rate_ / twist_rate_);
ekf_.updateWithDelay(y, C, R, delay_step);
// debug
Eigen::MatrixXd X_result(dim_x_, 1);
ekf_.getLatestX(X_result);
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
};
/*
* mahalanobisGate
*/
bool EKFLocalizer::mahalanobisGate(const double& dist_max, const Eigen::MatrixXd& x, const Eigen::MatrixXd& obj_x,
const Eigen::MatrixXd& cov)
{
Eigen::MatrixXd mahalanobis_squared = (x - obj_x).transpose() * cov.inverse() * (x - obj_x);
// DEBUG_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max);
ROS_INFO("measurement update: mahalanobis = %f, gate limit = %f", std::sqrt(mahalanobis_squared(0)), dist_max);
if (mahalanobis_squared(0) > dist_max * dist_max)
{
return false;
}
return true;
}
/*
* publishEstimateResult
*/
void EKFLocalizer::publishEstimateResult()
{
ros::Time current_time = ros::Time::now();
Eigen::MatrixXd X(dim_x_, 1);
Eigen::MatrixXd P(dim_x_, dim_x_);
ekf_.getLatestX(X);
ekf_.getLatestP(P);
/* publish latest pose */
pub_pose_.publish(current_ekf_pose_);
/* publish latest pose with covariance */
geometry_msgs::PoseWithCovarianceStamped pose_cov;
pose_cov.header.stamp = current_time;
pose_cov.header.frame_id = current_ekf_pose_.header.frame_id;
pose_cov.pose.pose = current_ekf_pose_.pose;
pose_cov.pose.covariance[0] = P(IDX::X, IDX::X);
pose_cov.pose.covariance[1] = P(IDX::X, IDX::Y);
pose_cov.pose.covariance[5] = P(IDX::X, IDX::YAW);
pose_cov.pose.covariance[6] = P(IDX::Y, IDX::X);
pose_cov.pose.covariance[7] = P(IDX::Y, IDX::Y);
pose_cov.pose.covariance[11] = P(IDX::Y, IDX::YAW);
pose_cov.pose.covariance[30] = P(IDX::YAW, IDX::X);
pose_cov.pose.covariance[31] = P(IDX::YAW, IDX::Y);
pose_cov.pose.covariance[35] = P(IDX::YAW, IDX::YAW);
pub_pose_cov_.publish(pose_cov);
/* publish latest twist */
pub_twist_.publish(current_ekf_twist_);
/* publish latest twist with covariance */
geometry_msgs::TwistWithCovarianceStamped twist_cov;
twist_cov.header.stamp = current_time;
twist_cov.header.frame_id = current_ekf_twist_.header.frame_id;
twist_cov.twist.twist = current_ekf_twist_.twist;
twist_cov.twist.covariance[0] = P(IDX::VX, IDX::VX);
twist_cov.twist.covariance[5] = P(IDX::VX, IDX::WZ) + P(IDX::VX, IDX::WZ_IMU);
twist_cov.twist.covariance[30] = P(IDX::WZ, IDX::VX) + P(IDX::WZ_IMU, IDX::VX);
twist_cov.twist.covariance[35] = P(IDX::WZ, IDX::WZ) + P(IDX::WZ, IDX::WZ_IMU) + P(IDX::WZ_IMU, IDX::WZ) + P(IDX::WZ_IMU, IDX::WZ_IMU);
pub_twist_cov_.publish(twist_cov);
/* Send transform of pose */
broadcastTF(current_time);
/* publish yaw bias */
std_msgs::Float64 yawb;
yawb.data = X(IDX::YAWB);
pub_yaw_bias_.publish(yawb);
/* debug measured pose */
if (current_pose_ptr_ != nullptr)
{
geometry_msgs::PoseStamped p;
p = *current_pose_ptr_;
p.header.stamp = current_time;
pub_measured_pose_.publish(p);
}
/* debug publish */
double RAD2DEG = 180.0 / 3.141592;
double pose_yaw = 0.0;
if (current_pose_ptr_ != nullptr)
pose_yaw = tf2::getYaw(current_pose_ptr_->pose.orientation) * RAD2DEG;
std_msgs::Float64MultiArray msg;
msg.data.push_back(X(IDX::YAW) * RAD2DEG); // [0] ekf yaw angle
msg.data.push_back(pose_yaw); // [1] measurement yaw angle
msg.data.push_back(X(IDX::YAWB) * RAD2DEG); // [2] yaw bias
msg.data.push_back(X(IDX::AX)); // [3] estimated x acceleration
msg.data.push_back(X(IDX::WZ_IMU)); // [4] estimated wz from IMU
pub_debug_.publish(msg);
}
double EKFLocalizer::normalizeYaw(const double& yaw)
{
return std::atan2(std::sin(yaw), std::cos(yaw));
}
/*
* measurementUpdateIMU
*/
void EKFLocalizer::measurementUpdateIMU(const sensor_msgs::Imu& imu)
{
Eigen::MatrixXd X_curr(dim_x_, 1);
ekf_.getLatestX(X_curr);
DEBUG_PRINT_MAT(X_curr.transpose());
constexpr int dim_y = 2; // ax, wz
const ros::Time t_curr = ros::Time::now();
/* Calculate delay step */
double delay_time = (t_curr - imu.header.stamp).toSec();
if (delay_time < 0.0)
{
ROS_WARN_DELAYED_THROTTLE(1.0, "IMU time stamp is inappropriate, set delay to 0[s]. delay = %f", delay_time);
delay_time = 0.0;
}
int delay_step = std::roundf(delay_time / ekf_dt_);
if (delay_step > extend_state_step_ - 1)
{
ROS_WARN_DELAYED_THROTTLE(1.0,
"IMU delay exceeds the compensation limit, ignored. delay: %f[s], limit = "
"extend_state_step * ekf_dt : %f [s]",
delay_time, extend_state_step_ * ekf_dt_);
return;
}
DEBUG_INFO("delay_time: %f [s]", delay_time);
/* Set measurement matrix */
Eigen::MatrixXd y(dim_y, 1);
y << imu.linear_acceleration.x, imu.angular_velocity.z;
if (isnan(y.array()).any() || isinf(y.array()).any())
{
ROS_WARN("[EKF] IMU measurement matrix includes NaN or Inf. ignore update. check IMU message.");
return;
}
/* Gate */
Eigen::MatrixXd y_ekf(dim_y, 1);
y_ekf << ekf_.getXelement(delay_step * dim_x_ + IDX::AX),
ekf_.getXelement(delay_step * dim_x_ + IDX::WZ_IMU);
Eigen::MatrixXd P_curr, P_y;
ekf_.getLatestP(P_curr);
P_y = P_curr.block(IDX::AX, IDX::AX, dim_y, dim_y);
if (!mahalanobisGate(imu_gate_dist_, y_ekf, y, P_y))
{
ROS_WARN_DELAYED_THROTTLE(2.0, "[EKF] IMU measurement update, mahalanobis distance is over limit. ignore "
"measurement data.");
return;
}
DEBUG_PRINT_MAT(y.transpose());
DEBUG_PRINT_MAT(y_ekf.transpose());
DEBUG_PRINT_MAT((y - y_ekf).transpose());
/* Set measurement matrix */
Eigen::MatrixXd C = Eigen::MatrixXd::Zero(dim_y, dim_x_);
C(0, IDX::AX) = 1.0; // for ax
C(1, IDX::WZ_IMU) = 1.0; // for wz_imu
/* Set measurement noise covariance */
Eigen::MatrixXd R = Eigen::MatrixXd::Zero(dim_y, dim_y);
R(0, 0) = imu_stddev_ax_ * imu_stddev_ax_; // for ax
R(1, 1) = imu_stddev_wz_ * imu_stddev_wz_; // for wz_imu
/* In order to avoid a large change by update, measurement update is performed by dividing at every step. */
R *= (ekf_rate_ / imu_rate_);
ekf_.updateWithDelay(y, C, R, delay_step);
// debug
Eigen::MatrixXd X_result(dim_x_, 1);
ekf_.getLatestX(X_result);
DEBUG_PRINT_MAT(X_result.transpose());
DEBUG_PRINT_MAT((X_result - X_curr).transpose());
// Publish debug IMU message
if (pub_measured_imu_.getNumSubscribers() > 0)
{
sensor_msgs::Imu debug_imu = imu;
debug_imu.header.stamp = t_curr;
pub_measured_imu_.publish(debug_imu);
}
}
小车转向时ekf发布的base_link在rviz中的转向不变但点云在转动，而且小车相对点云位置准确而且稳定
最新发布
最低0.47元/天 解锁文章
扫一扫
1021
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
