Robot_pose_ekf源码笔记

        Robot_pose_ekf是ROS中实现多传感器融合输出机器人位姿的程序包。其基本原理是利用扩展卡尔曼滤波，利用多种传感器实现机器人位姿的最优估计。目前支持轮式里程计、IMU、GPS和视觉里程计4种传感器。官方说明文档：robot_pose_ekf - ROS Wiki

        以下是源码的一些笔记，其中有些不懂的地方也找学长交流过，有的问题解决了，有的还是不太清楚，也都在笔记中标记出来了，欢迎大家一起交流、批评指正！

0、对于整体算法流程的理解

      假设系统在时刻t-1的融合滤波输出为，在时刻t接收到了不同传感器的数据设这些传感器的观测矩阵依次为协方差矩阵依次为那么，按照卡尔曼滤波的步骤，首先执行状态预测：

之后利用第一种传感器进行测量更新：

在得到yt(1)和Σt(1)的基础上，以相同的步骤利用第二种传感器再进行一次测量更新：

之后再融合剩下的第三种、第四种传感器，步骤与之前相同。

    Robot_pose_ekf可以处理4种传感器：轮式里程计、惯性元件（IMU）、视觉里程计和GPS。其中里程计的融合滤波过程会比其他传感器稍微复杂一些，因为程序是取里程计每一个时间段内的相对变化值，而并不是直接将里程计输出的绝对观测量进行测量更新。这一部分在下文里程计的部分会说明。

    不过值得注意的是Robot_pose_ekf虽然确实用到了扩展卡尔曼滤波，但是它对系统的预测模型作出了很大的简化，所以完全可以仅用卡尔曼滤波的思路去理解整个程序。这一部分将在下文的建立系统模型部分会做详细说明。

1、建立模型：

a.建立系统模型

设机器人的空间坐标为，机器人的姿态用欧拉角来描述，机器人线速度为，角速度为。则系统状态变量选择为，控制量为，如果默认系统只在水平方向运动，则对应的方程为（注意：加粗的字母代表向量，而未加粗的字母则代表某一个状态变量）：

明显这是一个非线性系统，按道理上讲我们可以利用扩展卡尔曼滤波的算法，通过泰勒展开将系统近似成一个线性系统。但是，此处Robot_pose_ekf做出了一些简化，认为系统的控制量始终为0。假设系统噪声为那么，系统模型利用矩阵表示如下：

即认为

【源码】

（nonlinearanalyticconditionalgaussianodo.cpp：43-51行）

ColumnVector NonLinearAnalyticConditionalGaussianOdo::ExpectedValueGet() const

  {

    ColumnVector state = ConditionalArgumentGet(0);

    ColumnVector vel  = ConditionalArgumentGet(1);

    state(1) += cos(state(6)) * vel(1);

    state(2) += sin(state(6)) * vel(1);

    state(6) += vel(2);

    return state + AdditiveNoiseMuGet();

  }

这里调用了ConditionalArgumentGet(unsigned int n_argument)，其功能解释如下：

Get the n-th argument of the list. Return the current value of the n-th conditional argument(starting from 0!)（本人水平太菜，完全没看懂BFL这里的代码，就只能先贴一下官方写的代码注释在这里，大概意思就是会返回系统目前的状态参数，ConditionalArgumentGet(0)是融合滤波后得到的目前状态变量的数值，ConditionalArgumentGet(1)是控制量，此处恒为0，至于为什么，我也没太搞懂……）

之后利用BFL建立系统模型：

（odom_estimation.cpp：61-66行）

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_);    

所以第一步的预测部分其实只是加入了系统噪声，而真正状态的更新则要靠后面传感器的数据进行测量更新。

b.建立里程计测量模型

观测矩阵为：，

初始化测量噪声协方差矩阵为：

测量方程为：

对应源码为：（odom_estimation.cpp：69-76行）

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_);

c.建立IMU模型

观测矩阵为,初始化测量噪声协方差矩阵：，

观测方程为：

对应源码为：（odom_estimation.cpp：79-86行）

ColumnVector measNoiseImu_Mu(3);  measNoiseImu_Mu = 0;

    SymmetricMatrix measNoiseImu_Cov(3);  measNoiseImu_Cov = 0;

    for (unsigned int i=1; i<=3; i++) measNoiseImu_Cov(i,i) = 1;

    Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);

    Matrix Himu(3,6);  Himu = 0;

    Himu(1,4) = 1;    Himu(2,5) = 1;    Himu(3,6) = 1;

    imu_meas_pdf_   = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);

imu_meas_model_ = new LinearAnalyticMeasurementModelGaussianUncertainty(imu_meas_pdf_);

2、获取传感器数据

获取传感器数据由odom_estimation_node.cpp中的回调函数完成。里程计信息赋值给odom_meas_,协方差幅值给odom_covariance_中，IMU数据获取方式相同（对应imu_meas_和imu_covariance_）。

对应源码（odom_estimation_node.cpp：173行往下）：

void OdomEstimationNode::odomCallback(const OdomConstPtr& odom)

  {

    odom_callback_counter_++

    ROS_DEBUG("Odom callback at time %f ", ros::Time::now().toSec());

    assert(odom_used_);

    // receive data

    odom_stamp_ = odom->header.stamp;

    odom_time_  = Time::now();

    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_);

//.inverse()意思是矩阵求逆，两个时间内的姿态矩阵相除，获得相对的里程计信息

    // activate 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;

    }

  };

3、初始化滤波器

给定滤波器最初的高斯分布参数：（odom_estimation.cpp：129-152行）

void OdomEstimation::initialize(const Transform& prior, const Time& time)

  {

    // set prior of filter

    ColumnVector prior_Mu(6);

    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_);

    // remember prior

    addMeasurement(StampedTransform(prior, time, output_frame_, base_footprint_frame_));

    filter_estimate_old_vec_ = prior_Mu;

    filter_estimate_old_ = prior;

    filter_time_old_     = time;

    // filter initialized

    filter_initialized_ = true;

  }

4、更新滤波器

功能在函数bool OdomEstimation::update(bool odom_active, bool imu_active, bool gps_active, bool vo_active, const Time&  filter_time, bool& diagnostics_res)中实现（odom_estimation.cpp第159行）。

首先根据系统噪声更新系统模型：（这里应该对应的是EKF中的预测部分）（177-181行）

// system update filter

    // --------------------

    // for now only add system noise

    ColumnVector vel_desi(2); vel_desi = 0;

    filter_->Update(sys_model_, vel_desi);

如果接受到了里程计数据，则更新里程计模型：

先将绝对里程计数据转换成相对里程计数据：

// convert absolute odom measurements to relative odom measurements in horizontal plane

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));

最后更新滤波器（应该对应EKF的测量更新部分）：

filter_->Update(odom_meas_model_, odom_rel);

问题：源代码是用了下面3步依次更新滤波器：

filter_->Update(sys_model_, vel_desi);

filter_->Update(odom_meas_model_, odom_rel);

filter_->Update(imu_meas_model_,  imu_rel);

但是网上关于BFL的教程显示，更新卡尔曼滤波器的代码为：

filter->Update(&sys_model,input,&meas_model, measurement);

函数对应的4个参数依次为：系统模型，输入量，测量模型，测量量。为什么在robot_pose_ekf中这个函数的输入参数只有两个？

答：函数重载，filter_->Update(sys_model_, vel_desi)只进行模型预测，filter_->Update(odom_meas_model_, odom_rel)与filter_->Update(imu_meas_model_,  imu_rel)只进行测量更新。

5、输出

输出功能在odom_estimation_node .cpp的380行的void OdomEstimationNode::spin(const ros::TimerEvent& e)函数实现。该函数定义了整个程序运行的频率以及最终的输出。（问题：频率这块看的稀里糊涂，大致上应该是程序会等所有传感器的数据都收到了之后才会发布新的消息，比如我试着令odom以40Hz的频率发送，IMU以1Hz频率发送，最后robot_pose_ekf的发送频率是1Hz，也就是和IMU发送的时间一致。在是在刚刚的例子里，robot_pose_ekf是融合了40个odom数据和1个imu数据，还是只取了1个odom数据和1个imu数据进行融合？我感觉是后者。还有：ROS官网举例：收到有关时间戳t_1> t_0的odom主题的消息，并且在时间戳t_2> t_1> t_0的imu_data主题上，过滤器现在将更新为关于所有传感器的信息可用的最新时间，在这种情况下为时间T_1。 直接给出t_1处的odom姿态，并且通过在t_0和t_2之间的imu姿态的线性插值来获得t_1上的imu姿态。 在t_0和t_1之间，使用odom和imu的相对姿势来更新机器人姿势过滤器。不过官网中提到的有关于线性插值的这一块我在源代码里也没有找到……）

答：Robot_pose_ekf会将时间最相近的odom与imu数据进行融合，如果二者时间间隔太远会报错。