写在前面:本文中对应的文件会用文件表示，对应的函数用粗体表示，对应的变量用斜体表示。会略过一些简单的以及注释写明白的部分。本文会假定读者已经已经对卡尔曼滤波器有一定的了解并且已经阅读过本文中将会使用的贝叶斯滤波器bfl包教程。鉴于本人水平有限难免会有疏漏，欢迎一起来交流，这是我的邮箱：[email protected]
Let’s the fun begin().
文章目录
以main函数开始：OdomEstimationNode的实例
从odom_estimation_node.cpp文件中开始，这个文件是对整个传感器融合过程进行ROS包装，odom_estimation_node.cpp中将会使用到的函数和变量均定义在odom_estimation_node.h中。从其中的main函数开始运行。对类OdomEstimationNode实例my_filter_node：
OdomEstimationNode my_filter_node;
OdomEstimation的实例
OdomEstimationNode实例化过程中声明了许多变量，比较重要的是它为系统、里程计、imu等声明了概率密度函数（简写为pdf）、模型，然后声明了滤波器为扩展卡尔曼滤波器（EKF）。
而重要的是第113行：
OdomEstimation my_filter_;
类OdomEstimation声明在odom_estimation.h中，实例为my_filter_。
创建系统模型和测量更新模型：OdomEstimation的构造函数
OdomEstimation的构造函数声明在odom_estimation.h中，公式在odom_estimation.cpp中。构造函数为各个传感器以及系统本身创造pdf以及模型。系统模型和传感器模型模型分为两种。其中系统模型（在概率机器人中也写做预测模型，以下统一为系统模型）中声明属于robot_pose_ekf包自身设定的概率密度函数NonLinearAnalyticConditionalGaussianOdo。它声明在nonlinearanalyticconditionalgaussianodo.h，表达式在nonlinearanalyticconditionalgaussianodo.cpp，构造函数如下所示：
#define NUMCONDARGUMENTS_MOBILE 2
// 继承自AnalyticConditionalGaussianAdditiveNoise，两个参数：第一个是增加的噪音，第二个是条件参数的数目（本例中为2）
NonLinearAnalyticConditionalGaussianOdo::NonLinearAnalyticConditionalGaussianOdo(const Gaussian& additiveNoise)
: AnalyticConditionalGaussianAdditiveNoise(additiveNoise,NUMCONDARGUMENTS_MOBILE),
df(6,6)
{
// 初始化导数矩阵
for (unsigned int i=1; i<=6; i++){
for (unsigned int j=1; j<=6; j++){
if (i==j) df(i,j) = 1;
else df(i,j) = 0;
}
}
}
回到odom_estimation.h中，系统模型的声明如下：
// 创造系统模型：
// 初始化系统模型噪音的均值和协方差矩阵
ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
for (unsigned int i=1; i<=6; i++) sysNoise_Cov(i,i) = pow(1000.0,2);
// 采用均值和协方差矩阵生成代表系统不确定性的高斯分布
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
// 利用高斯分布生成系统噪音的概率密度函数（pdf），根据pdf创建带有高斯不确定度的系统模型。
sys_pdf_ = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);
初始化系统模型之后，剩下是传感器的测量更新模型的初始化。内容都大同小异，只以里程计odom的测量更新模型为例。测量更新模型都是线性模型，和BFL包教程中的第一个例子完全一样，代码如下：
// 创造里程计测量更新模型
// 初始化代表测量噪音的高斯分布
ColumnVector measNoiseOdom_Mu(6); measNoiseOdom_Mu = 0;
SymmetricMatrix measNoiseOdom_Cov(6); measNoiseOdom_Cov = 0;
for (unsigned int i=1; i<=6; i++) measNoiseOdom_Cov(i,i) = 1;
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
// 初始化代表测量更新线性模型本身的高斯分布
Matrix Hodom(6,6); Hodom = 0;
Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(6,6) = 1;
// 用线性模型和噪音生成pdf以及测量更新模型
odom_meas_pdf_ = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
odom_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);
数据读取：OdomEstimationNode的构造函数
模型的创建终于结束了，回到odom_estimation_node.cpp运行OdomEstimationNode的构造函数，这一部分就比较简单了。读取将要使用哪些传感器、更新的频率、tf树转换等。到第101行比较重要，但是先不展开说，只是摘录如下：
timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
广播(advertise)要发布的话题，记录滤波开始的时间，
传感器数据读取与滤波器坐标转换：odomCallback与imucallback
如果使用了某个传感器就订阅该传感器的话题并执行对应的callback函数。仍然以里程计为例，其订阅代码如下：
// 订阅里程计消息
if (odom_used_){
ROS_DEBUG("Odom sensor can be used");
odom_sub_ = nh.subscribe("odom", 10, &OdomEstimationNode::odomCallback, this);
}
else ROS_DEBUG("Odom sensor will NOT be used");
对应的callback函数还挺重要的，因为读取的过程中已经为滤波器收集数据了，其代码如下：
// odom的callback函数
void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)
{
odom_callback_counter_++;
ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());
assert(odom_used_);
// 接收数据
odom_stamp_ = odom->header.stamp;
odom_time_ = Time::now();
Quaternion q;
// 将四元数msg（消息）转换为四元数
tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
// odom的转换，从里程计坐标系到base_footprint转换，q代表坐标系的旋转，Vector3代表坐标系的平移。
odom_meas_ = Transform(q, Vector3(odom->pose.pose.position.x, odom->pose.pose.position.y, 0));
// 读取协方差矩阵
for (unsigned int i=0; i<6; i++)
for (unsigned int j=0; j<6; j++)
odom_covariance_(i+1, j+1) = odom->pose.covariance[6*i+j];
//给滤波器增加一个测量更新，这个函数接下来会说。因为这里需要的是从base_footprint_frame_-->wheelodom的转换，所以前面的到的odom_meas要反转。
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(),
odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);
// 启动odom，初始化并检查odom是否一直在激活状态
if (!odom_active_) {
if (!odom_initializing_){
odom_initializing_ = true;
odom_init_stamp_ = odom_stamp_;
ROS_INFO("Initializing Odom sensor");
}
if ( filter_stamp_ >= odom_init_stamp_){
odom_active_ = true;
odom_initializing_ = false;
ROS_INFO("Odom sensor activated");
}
else ROS_DEBUG("Waiting to activate Odom, because Odom measurements are still %f sec in the future.",
(odom_init_stamp_ - filter_stamp_).toSec());
}
if (debug_){
// write to file
double tmp, yaw;
odom_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
odom_file_<< fixed <<setprecision(5) << ros::Time::now().toSec() << " " << odom_meas_.getOrigin().x() << " " << odom_meas_.getOrigin().y() << " " << yaw << " " << endl;
}
};
接下来是关于：
my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(),
odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);
该函数被定义在odom_estimation.cpp第332行，定义如下：
void OdomEstimation::addMeasurement(const StampedTransform& meas, const MatrixWrapper::SymmetricMatrix& covar)
{
// 检查协方差矩阵
for (unsigned int i=0; i<covar.rows(); i++){
if (covar(i+1,i+1) == 0){
ROS_ERROR("Covariance specified for measurement on topic %s is zero", meas.child_frame_id_.c_str());
return;
}
}
// 增加测量，定义在odom_estimation.cpp第322行，实际上就是把对应的转换记录下来
addMeasurement(meas);
if (meas.child_frame_id_ == "wheelodom") odom_covariance_ = covar;
else if (meas.child_frame_id_ == "imu") imu_covariance_ = covar;
else if (meas.child_frame_id_ == "vo") vo_covariance_ = covar;
else if (meas.child_frame_id_ == "gps") gps_covariance_ = covar;
else ROS_ERROR("Adding a measurement for an unknown sensor %s", meas.child_frame_id_.c_str());
};
imucallback与odomcallback略有不同，主要体现在imu转换到base_footprint的tf直接在机器人状态中寻找。
滤波器更新
滤波器初始化：OdomEstimationNode::spin其①
终于回到第101行了，重新抄写如下：
timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)), &OdomEstimationNode::spin, this);
开始都是一些参数检测，都很明白，我就不赘述。
因为这是我们第一次调用这个函数，当然没有对滤波器进行初始化，所以427行至457行这次loop也跳过。
因为本文使用的是imu和里程计融合，所以直接以这句初始化滤波器：
else if ( odom_active_ && !gps_used_ && !my_filter_.isInitialized()){
my_filter_.initialize(odom_meas_, odom_stamp_);
ROS_INFO("Kalman filter initialized with odom measurement");
}
my_filter_.initialize定义在odom_estimation.cpp中，代码如下：
// 初始化滤波器的概率
void OdomEstimation::initialize(const Transform& prior, const Time& time)
{
// 设定先验分布的均值prior_Mu和协方差矩阵prior_Cov
ColumnVector prior_Mu(6);
// 将TF树分解至xyz以及偏航、俯仰、滚转（YPR）
decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));
SymmetricMatrix prior_Cov(6);
for (unsigned int i=1; i<=6; i++) {
for (unsigned int j=1; j<=6; j++){
if (i==j) prior_Cov(i,j) = pow(0.001,2);
else prior_Cov(i,j) = 0;
}
}
// 根据以上建立一个新的高斯分布，再用这个高斯分布建立新的扩展卡尔曼滤波器
prior_ = new Gaussian(prior_Mu,prior_Cov);
filter_ = new ExtendedKalmanFilter(prior_);
// 记录初始值
addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));
filter_estimate_old_vec_ = prior_Mu;
filter_estimate_old_ = prior;
filter_time_old_ = time;
// 给滤波器初始化的flag设定为true
filter_initialized_ = true;
}
至此滤波器初始化完成
滤波器更新与消息发布：OdomEstimationNode::spin其②
上文中为第一次spin，完成了滤波器初始化，之后的每次调用spin都会更新滤波器同时发布消息。
首先仍然是一些传感器检查，检查传感器是否仍然在工作。之后正式开始更新传感器与发布消息：
// 只有当滤波器已经初始化才开始更新滤波器
if ( my_filter_.isInitialized() ) {
bool diagnostics = true;
// 这一句if中实际更新滤波器，也是核心语句，接下来会分析
if (my_filter_.update(odom_active_, imu_active_,gps_active_, vo_active_, filter_stamp_, diagnostics)){
// 发布最近的估计与协方差矩阵
my_filter_.getEstimate(output_);
pose_pub_.publish(output_);
ekf_sent_counter_++;
// 广播最近的变换
StampedTransform tmp;
my_filter_.getEstimate(ros::Time(), tmp);
if(!vo_active_ && !gps_active_)
tmp.getOrigin().setZ(0.0);
odom_broadcaster_.sendTransform(StampedTransform(tmp, tmp.stamp_, output_frame_, base_footprint_frame_));
if (debug_){
// write to file
ColumnVector estimate;
my_filter_.getEstimate(estimate);
corr_file_ << fixed << setprecision(5)<<ros::Time::now().toSec()<<" ";
for (unsigned int i=1; i<=6; i++)
corr_file_ << estimate(i) << " ";
corr_file_ << endl;
}
}
if (self_diagnose_ && !diagnostics)
ROS_WARN("Robot pose ekf diagnostics discovered a potential problem");
}
my_filter_.update定义在odom_estimation.cpp第159-320行。第161行-第174行是一些参数检查，之后开始才真正的测量更新。因为各个传感器测量更新内容类似，这里只以里程计odom举例，其代码如下：
// 开始滤波器更新
// --------------------
// 首先只加上系统噪音
ColumnVector vel_desi(2); vel_desi = 0;
filter_->Update(sys_model_, vel_desi);
// 开始里程计测量更新
// ------------------------
ROS_DEBUG("Process odom meas");
if (odom_active){
if (!transformer_.canTransform(base_footprint_frame_,"wheelodom", filter_time)){
ROS_ERROR("filter time older than odom message buffer");
return false;
}
transformer_.lookupTransform("wheelodom", base_footprint_frame_, filter_time, odom_meas_);
if (odom_initialized_){
// 将绝对里程测量值转换至水平平面内的相对里程计测量值（坐标转换tf仍然由旋转和平移构成）
Transform odom_rel_frame = Transform(tf::createQuaternionFromYaw(filter_estimate_old_vec_(6)),
filter_estimate_old_.getOrigin()) * odom_meas_old_.inverse() * odom_meas_;
ColumnVector odom_rel(6);
// 将转换分解到x,y,z,Rx,Ry,Rz
decomposeTransform(odom_rel_frame, odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
// 角度溢出的话就进行修正
angleOverflowCorrect(odom_rel(6), filter_estimate_old_vec_(6));
// 更新滤波器
// 更新协方差矩阵
odom_meas_pdf_->AdditiveNoiseSigmaSet(odom_covariance_ * pow(dt,2));
ROS_DEBUG("Update filter with odom measurement %f %f %f %f %f %f",
odom_rel(1), odom_rel(2), odom_rel(3), odom_rel(4), odom_rel(5), odom_rel(6));
// 给里程计系统模型更新这次的测量值
filter_->Update(odom_meas_model_, odom_rel);
diagnostics_odom_rot_rel_ = odom_rel(6);
}
else{
odom_initialized_ = true;
diagnostics_odom_rot_rel_ = 0;
}
odom_meas_old_ = odom_meas_;
}
// 传感器未初始化
else odom_initialized_ = false;
结束
由此，整个robot_pose_ekf包源码解读就结束了。
这里还得重新强调一下，如果看到任何问题或者错误，欢迎与我练习～
如果感觉对整个过程还是有点糊涂的话，这里给出我理解的流程图：