Navigation源码阅读之robot_pose_ekf

卡尔曼滤波器是传感器融合的利器，而navigation源码中的对于小车感知这一块的处理也是依靠EKF来实现，我们来阅读一下这个包的实现方法，以及网上的贝叶斯滤波的官方API，在实际使用时，可以更快地定位错误与改写。

一、首先看看cmakelists，这个包只生成一个节点，主函数在odom_estimation_node.cpp中，定位到OdomEstimationNode类的构造函数，这里有一溜的param，我们在做小车时一般用里程计、IMU、视觉里程计进行定位，这三个默认是true，另外滤波器的刷新频率设为30Hz，也绰绰有余了，一般传感器的信息频率在100~1000Hz左右。

my_filter_.setOutputFrame(output_frame_);
my_filter_.setBaseFootprintFrame(base_footprint_frame_);

timer_ = nh_private.createTimer(ros::Duration(1.0/max(freq,1.0)),&OdomEstimationNode::spin, this);

我们一般将这两个frame设为odom与base_link，这样就可以通过EKF代替base_controller发送odom的tf变换。另外，用手写一个函数OdomEstimationNode::spin而不是一般见到的ros自带的spin函数，用来检查各个传感器的使用状态，以及发布我们需要的topic和tf，这个函数将以30Hz的频率运行。

二、OdomEstimationNode::odomCallback，这一系列的传感器回调函数，不同的传感器的数据维度与协方差矩阵尺寸不同，就拿里程计来说，协方差矩阵为６*６。

Quaternion q;
tf::quaternionMsgToTF(odom->pose.pose.orientation, q);
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];

my_filter_.addMeasurement(StampedTransform(odom_meas_.inverse(), odom_stamp_, base_footprint_frame_, "wheelodom"), odom_covariance_);

在这个代码块它调用了OdomEstimation::addMeasurement函数，它位于odom_estimation.cpp中： 

１．OdomEstimation::addMeasurement这个函数其实就是把组装好的transform发出去，它将在之后被OdomEstimation::update捕获并进行下一次的状态估计。

三、OdomEstimationNode::spin函数，这是整个包的输出信息的函数，一开始需要对时间戳进行检查：

if ( (odom_initializing_ || odom_active_) && (imu_initializing_ || imu_active_) )
{
  double diff = fabs( Duration(odom_stamp_ - imu_stamp_).toSec() );
  if (diff > 1.0) ROS_ERROR("Timestamps of odometry and imu are %f seconds apart.", diff);
}

这个错误的出现概率不算太大，除非把传感器频率设得太低，或者直接断开连接了。接着直接对小车的状态进行估计并发送tf变换：

if ( my_filter_.isInitialized() )
{
  bool diagnostics = true;
  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_));

...

 上面的代码段调用了OdomEstimation类的多个函数，我们依次进行分析：

１．先看看OdomEstimation的构造函数，下面的代码段是系统的概率模型。各变量的意义是：pdf是条件概率密度函数，即probability density function，model是概率密度模型，sysNoise_Mu是系统的噪声均值，sysNoise_Cov是协方差矩阵。在这里将协方差矩阵的对角线都赋成10^-6。

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);
sys_pdf_   = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
sys_model_ = new AnalyticSystemModelGaussianUncertainty(sys_pdf_);

相比系统模型，传感器模型直接使用线性模型进行估计， 首先创建一个高斯分布的观测噪声，再加入到里程计的概率密度函数中，在这里的噪声协方差比系统初始值要大很多。另外，Hodom指的是里程计自身的噪声。接下来在各个传感器模型中流程均相同。

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;
odom_meas_pdf_   = new LinearAnalyticConditionalGaussian(Hodom,　measurement_Uncertainty_Odom);
odom_meas_model_ = new　LinearAnalyticMeasurementModelGaussianUncertainty(odom_meas_pdf_);

２．OdomEstimation::initialize函数建立了先验概率模型。prior_Mu是先验概率的均值，prior_Cov是协方差矩阵，那么一开始的先验概率均值由odom_meas_提供。

ColumnVector prior_Mu(6);
decomposeTransform(prior, prior_Mu(1), prior_Mu(2), prior_Mu(3), prior_Mu(4), prior_Mu(5), prior_Mu(6));

 用这个先验概率新初始化一个EKF模型，将先验概率记下来，在update函数中会用于下一次的更新。

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;

 ３．OdomEstimation::update函数用于滤波器的实际更新。首先加上系统噪声，再依次将传感器的数据传入滤波器中予以状态估计。下面这一段指的是将里程计的数据转化为在水平面上的相对数据，加入传感器噪声之后更新协方差矩阵，最终给里程计模型更新这次的测量值。

if (odom_initialized_)
{
  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);
  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);
}

在这里有一句 filter_->Update(odom_meas_model_, odom_rel);查看头文件就可以知道这是EKF提供的接口，我们跳到贝叶斯滤波库的BFL::ExtendedKalmanFilter中看看程序是怎么写的。网页版链接http://docs.ros.org/jade/api/bfl/html/annotated.html。

 （１）在这里对sysmodel进行了强制转换，转到AnalyticSystemModelGaussianUncertainty这个类就可以知道，我们用AnalyticSystemModelGaussianUncertainty的成员函数把扩展卡尔曼滤波模型在当前状态下进行线性化：

 那么接下来就是KF模型的估计了。

void　ExtendedKalmanFilter::SysUpdate(SystemModel<ColumnVector>* const　sysmodel,const ColumnVector& u)
{
   _x = _post->ExpectedValueGet();
   _J = ((AnalyticSys*)sysmodel)->PredictionGet(u,_x);
   _F = ((AnalyticSys*)sysmodel)->df_dxGet(u,_x);
   _Q = ((AnalyticSys*)sysmodel)->CovarianceGet(u,_x);
 
   CalculateSysUpdate(_J, _F, _Q);
}

（２）这一段实际上就是用状态转移矩阵F，后验估计的协方差矩阵，系统噪声的协方差矩阵Q计算得到预测值和真实值之间误差的协方差矩阵。

void　KalmanFilter::CalculateSysUpdate(const ColumnVector& J, const Matrix& F, const SymmetricMatrix& Q)
{
  _Sigma_temp = F * ( (Matrix)_post->CovarianceGet() * F.transpose());
  _Sigma_temp += (Matrix)Q;
  _Sigma_temp.convertToSymmetricMatrix(_Sigma_new);

  PostMuSet   ( J );
  PostSigmaSet( _Sigma_new );
}

４．再回到OdomEstimation::update，当各传感器的数据均在滤波器内更新后，开始获取本轮的状态估计。

filter_estimate_old_vec_ = filter_->PostGet()->ExpectedValueGet();
tf::Quaternion q;
q.setRPY(filter_estimate_old_vec_(4), filter_estimate_old_vec_(5),filter_estimate_old_vec_(6));
filter_estimate_old_ = Transform(q,Vector3(filter_estimate_old_vec_(1), filter_estimate_old_vec_(2), filter_estimate_old_vec_(3)));
filter_time_old_ = filter_time;
addMeasurement(StampedTransform(filter_estimate_old_, filter_time,output_frame_, base_footprint_frame_));

重新覆盖filter_estimate_old_vec_作为下一次的先验，并且把估计的系统状态用transform的形式传给getEstimate，那么OdomEstimationNode::spin函数便可接收到并发出本次估计的tf变换。

用robot_pose_ekf包进行融合之后，tf树应该是由robot_pose_ekf发出的odom指向base_link。