robot_localization 源码解析（1）ekf_localization_node

1. 简介

1.1 坐标系定义

状态向量为：
$$X=（x,y,z,{\theta }_x,{\theta }_y,{\theta }_z,\dot{x},\dot{y},\dot{z},\dot{{\theta }_x},\dot{{\theta }_y},\dot{{\theta }_z},\ddot{x},\ddot{y},\ddot{z}{)}^T$$
状态向量中$（x,y,z,{\theta }_x,{\theta }_y,{\theta }_z)$为世界坐标系下的绝对姿态，$（\dot{x},\dot{y},\dot{z},\dot{{\theta }_x},\dot{{\theta }_y},\dot{{\theta }_z},\ddot{x},\ddot{y},\ddot{z})$为载体坐标系下的姿态变化；

ekf_localization_node 对应的文件为ekf_localization_node.cpp，其以Ekf为参数实例化了模板类 RosFilter：

namespace RobotLocalization
{

typedef RosFilter<Ekf> RosEkf;//以Ekf为参数实例化了模板类 RosFilter
typedef RosFilter<Ukf> RosUkf;

}

ekf_localization_node.cpp代码内容如下,主要分为实例化RosEkf和初始化RosEkf两个部分，下面将分为这两个部分进行展开：

int main(int argc, char **argv)
{
  ros::init(argc, argv, "ekf_navigation_node");

  ros::NodeHandle nh;
  ros::NodeHandle nh_priv("~");

  RobotLocalization::RosEkf ekf(nh, nh_priv);//实例化RosEkf
  ekf.initialize();//初始化RosEkf
  ros::spin();

  return EXIT_SUCCESS;
}

2. 实例化RosEkf

实例化RosEkf通过构造函数完成，构造函数进行普通成员变量初始化，以及状态向量名初始化:

  template<typename T>
  RosFilter<T>::RosFilter(ros::NodeHandle nh,
                          ros::NodeHandle nh_priv,
                          std::string node_name,
                          std::vector<double> args) :
      disabledAtStartup_(false),
      enabled_(false),
      predictToCurrentTime_(false),
      printDiagnostics_(true),
      publishAcceleration_(false),
      publishTransform_(true),
      resetOnTimeJump_(false),
      smoothLaggedData_(false),
      toggledOn_(true),
      twoDMode_(false),
      useControl_(false),
      silentTfFailure_(false),
      dynamicDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
      staticDiagErrorLevel_(diagnostic_msgs::DiagnosticStatus::OK),
      frequency_(30.0),
      gravitationalAcc_(9.80665),
      historyLength_(0),
      minFrequency_(frequency_ - 2.0),
      maxFrequency_(frequency_ + 2.0),
      baseLinkFrameId_("base_link"),
      mapFrameId_("map"),
      odomFrameId_("odom"),
      worldFrameId_(odomFrameId_),
      lastDiagTime_(0),
      lastSetPoseTime_(0),
      latestControlTime_(0),
      tfTimeOffset_(ros::Duration(0)),
      tfTimeout_(ros::Duration(0)),
      filter_(args),
      nh_(nh),
      nhLocal_(nh_priv),
      diagnosticUpdater_(nh, nh_priv, node_name),
      tfListener_(tfBuffer_)
  {
    stateVariableNames_.push_back("X");//状态向量
    stateVariableNames_.push_back("Y");
    stateVariableNames_.push_back("Z");
    stateVariableNames_.push_back("ROLL");
    stateVariableNames_.push_back("PITCH");
    stateVariableNames_.push_back("YAW");
    stateVariableNames_.push_back("X_VELOCITY");
    stateVariableNames_.push_back("Y_VELOCITY");
    stateVariableNames_.push_back("Z_VELOCITY");
    stateVariableNames_.push_back("ROLL_VELOCITY");
    stateVariableNames_.push_back("PITCH_VELOCITY");
    stateVariableNames_.push_back("YAW_VELOCITY");
    stateVariableNames_.push_back("X_ACCELERATION");
    stateVariableNames_.push_back("Y_ACCELERATION");
    stateVariableNames_.push_back("Z_ACCELERATION");

    diagnosticUpdater_.setHardwareID("none");
  }

2. 初始化RosEkf

  template<typename T>
  void RosFilter<T>::initialize()
  {
    loadParams();//加载参数

    if (printDiagnostics_)
    {
      diagnosticUpdater_.add("Filter diagnostic updater", this, &RosFilter<T>::aggregateDiagnostics);
    }

    // Set up the frequency diagnostic
    minFrequency_ = frequency_ - 2;
    maxFrequency_ = frequency_ + 2;
    freqDiag_ = std::make_unique<diagnostic_updater::HeaderlessTopicDiagnostic>(
      "odometry/filtered",
      diagnosticUpdater_,
      diagnostic_updater::FrequencyStatusParam(
        &minFrequency_,
        &maxFrequency_,
        0.1,
        10));

    // Publisher
    positionPub_ = nh_.advertise<nav_msgs::Odometry>("odometry/filtered", 20);

    // Optional acceleration publisher
    if (publishAcceleration_)
    {
      accelPub_ = nh_.advertise<geometry_msgs::AccelWithCovarianceStamped>("accel/filtered", 20);
    }

    lastDiagTime_ = ros::Time::now();

    periodicUpdateTimer_ = nh_.createTimer(ros::Duration(1./frequency_), &RosFilter<T>::periodicUpdate, this);
  }

2.1 loadParams()加载参数

订阅odom0: example/odom、odom1: example/another_odom等话题，并生成相关更新向量，观测以及观测协方差矩阵；
订阅pose0: example/pose等话题，并生成相关更新向量，观测以及观测协方差矩阵；
订阅imu0: example/imu等话题，并生成相关更新向量，观测以及观测协方差矩阵；
加载过程噪声
加载初始估计噪声

  template<typename T>
  void RosFilter<T>::loadParams()
  {
    /* For diagnostic purposes, collect information about how many different
     * sources are measuring each absolute pose variable and do not have
     * differential integration enabled.
     */
    std::map<StateMembers, int> absPoseVarCounts;
    absPoseVarCounts[StateMemberX] = 0;
    absPoseVarCounts[StateMemberY] = 0;
    absPoseVarCounts[StateMemberZ] = 0;
    absPoseVarCounts[StateMemberRoll] = 0;
    absPoseVarCounts[StateMemberPitch] = 0;
    absPoseVarCounts[StateMemberYaw] = 0;
    // Same for twist variables
    std::map<StateMembers, int> twistVarCounts;
    twistVarCounts[StateMemberVx] = 0;
    twistVarCounts[StateMemberVy] = 0;
    twistVarCounts[StateMemberVz] = 0;
    twistVarCounts[StateMemberVroll] = 0;
    twistVarCounts[StateMemberVpitch] = 0;
    twistVarCounts[StateMemberVyaw] = 0;

    // Determine if we'll be printing diagnostic information
    nhLocal_.param("print_diagnostics", printDiagnostics_, true);

    // Check for custom gravitational acceleration value
    nhLocal_.param("gravitational_acceleration", gravitationalAcc_, 9.80665);

    // Grab the debug param. If true, the node will produce a LOT of output.
    bool debug;
    nhLocal_.param("debug", debug, false);

    if (debug)
    {
      std::string debugOutFile;

      try
      {
        nhLocal_.param("debug_out_file", debugOutFile, std::string("robot_localization_debug.txt"));
        debugStream_.open(debugOutFile.c_str());

        // Make sure we succeeded
        if (debugStream_.is_open())
        {
          filter_.setDebug(debug, &debugStream_);
        }
        else
        {
          ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file " << debugOutFile);
        }
      }
      catch(const std::exception &e)
      {
        ROS_WARN_STREAM("RosFilter::loadParams() - unable to create debug output file" << debugOutFile
                        << ". Error was " << e.what() << "\n");
      }
    }

    // These params specify the name of the robot's body frame (typically
    // base_link) and odometry frame (typically odom)
    nhLocal_.param("map_frame", mapFrameId_, std::string("map"));
    nhLocal_.param("odom_frame", odomFrameId_, std::string("odom"));
    nhLocal_.param("base_link_frame", baseLinkFrameId_, std::string("base_link"));
    nhLocal_.param("base_link_frame_output", baseLinkOutputFrameId_, baseLinkFrameId_);

    /*
     * These parameters are designed to enforce compliance with REP-105:
     * http://www.ros.org/reps/rep-0105.html
     * When fusing absolute position data from sensors such as GPS, the state
     * estimate can undergo discrete jumps. According to REP-105, we want three
     * coordinate frames: map, odom, and base_link. The map frame can have
     * discontinuities, but is the frame with the most accurate position estimate
     * for the robot and should not suffer from drift. The odom frame drifts over
     * time, but is guaranteed to be continuous and is accurate enough for local
     * planning and navigation. The base_link frame is affixed to the robot. The
     * intention is that some odometry source broadcasts the odom->base_link
     * transform. The localization software should broadcast map->base_link.
     * However, tf does not allow multiple parents for a coordinate frame, so
     * we must *compute* map->base_link, but then use the existing odom->base_link
     * transform to compute *and broadcast* map->odom.
     *
     * The state estimation nodes in robot_localization therefore have two "modes."
     * If your world_frame parameter value matches the odom_frame parameter value,
     * then robot_localization will assume someone else is broadcasting a transform
     * from odom_frame->base_link_frame, and it will compute the
     * map_frame->odom_frame transform. Otherwise, it will simply compute the
     * odom_frame->base_link_frame transform.
     *
     * The default is the latter behavior (broadcast of odom->base_link).
     */
    nhLocal_.param("world_frame", worldFrameId_, odomFrameId_);

    ROS_FATAL_COND(mapFrameId_ == odomFrameId_ ||
                   odomFrameId_ == baseLinkFrameId_ ||
                   mapFrameId_ == baseLinkFrameId_ ||
                   odomFrameId_ == baseLinkOutputFrameId_ ||
                   mapFrameId_ == baseLinkOutputFrameId_,
                   "Invalid frame configuration! The values for map_frame, odom_frame, "
                   "and base_link_frame must be unique. If using a base_link_frame_output values, it "
                   "must not match the map_frame or odom_frame.");

    // Try to resolve tf_prefix
    std::string tfPrefix = "";
    std::string tfPrefixPath = "";
    if (nhLocal_.searchParam("tf_prefix", tfPrefixPath))
    {
      nhLocal_.getParam(tfPrefixPath, tfPrefix);
    }

    // Append the tf prefix in a tf2-friendly manner
    FilterUtilities::appendPrefix(tfPrefix, mapFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, odomFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, baseLinkFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, baseLinkOutputFrameId_);
    FilterUtilities::appendPrefix(tfPrefix, worldFrameId_);

    // Whether we're publshing the world_frame->base_link_frame transform
    nhLocal_.param("publish_tf", publishTransform_, true);

    // Whether we're publishing the acceleration state transform
    nhLocal_.param("publish_acceleration", publishAcceleration_, false);

    // Transform future dating
    double offsetTmp;
    nhLocal_.param("transform_time_offset", offsetTmp, 0.0);
    tfTimeOffset_.fromSec(offsetTmp);

    // Transform timeout
    double timeoutTmp;
    nhLocal_.param("transform_timeout", timeoutTmp, 0.0);
    tfTimeout_.fromSec(timeoutTmp);

    // Update frequency and sensor timeout
    double sensorTimeout;
    nhLocal_.param("frequency", frequency_, 30.0);
    nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_);
    filter_.setSensorTimeout(sensorTimeout);

    // Determine if we're in 2D mode
    nhLocal_.param("two_d_mode", twoDMode_, false);

    // Whether or not to print warning for tf lookup failure
    // Note: accesses the root of the parameter tree, not the local parameters
    nh_.param("/silent_tf_failure", silentTfFailure_, false);

    // Smoothing window size
    nhLocal_.param("smooth_lagged_data", smoothLaggedData_, false);
    nhLocal_.param("history_length", historyLength_, 0.0);

    // Wether we reset filter on jump back in time
    nhLocal_.param("reset_on_time_jump", resetOnTimeJump_, false);

    if (!smoothLaggedData_ && ::fabs(historyLength_) > 1e-9)
    {
      ROS_WARN_STREAM("Filter history interval of " << historyLength_ <<
                      " specified, but smooth_lagged_data is set to false. Lagged data will not be smoothed.");
    }

    if (smoothLaggedData_ && historyLength_ < -1e9)
    {
      ROS_WARN_STREAM("Negative history interval of " << historyLength_ <<
                      " specified. Absolute value will be assumed.");
    }

    historyLength_ = ::fabs(historyLength_);

    nhLocal_.param("predict_to_current_time", predictToCurrentTime_, false);

    // Determine if we're using a control term
    bool stampedControl = false;
    double controlTimeout = sensorTimeout;
    std::vector<int> controlUpdateVector(TWIST_SIZE, 0);
    std::vector<double> accelerationLimits(TWIST_SIZE, 1.0);
    std::vector<double> accelerationGains(TWIST_SIZE, 1.0);
    std::vector<double> decelerationLimits(TWIST_SIZE, 1.0);
    std::vector<double> decelerationGains(TWIST_SIZE, 1.0);

    nhLocal_.param("use_control", useControl_, false);
    nhLocal_.param("stamped_control", stampedControl, false);
    nhLocal_.param("control_timeout", controlTimeout, sensorTimeout);

    if (useControl_)
    {
      if (nhLocal_.getParam("control_config", controlUpdateVector))
      {
        if (controlUpdateVector.size() != TWIST_SIZE)
        {
          ROS_ERROR_STREAM("Control configuration must be of size " << TWIST_SIZE << ". Provided config was of "
            "size " << controlUpdateVector.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
        ROS_ERROR_STREAM("use_control is set to true, but control_config is missing. No control term will be used.");
        useControl_ = false;
      }

      if (nhLocal_.getParam("acceleration_limits", accelerationLimits))
      {
        if (accelerationLimits.size() != TWIST_SIZE)
        {
          ROS_ERROR_STREAM("Acceleration configuration must be of size " << TWIST_SIZE << ". Provided config was of "
            "size " << accelerationLimits.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
        ROS_WARN_STREAM("use_control is set to true, but acceleration_limits is missing. Will use default values.");
      }

      if (nhLocal_.getParam("acceleration_gains", accelerationGains))
      {
        const int size = accelerationGains.size();
        if (size != TWIST_SIZE)
        {
          ROS_ERROR_STREAM("Acceleration gain configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
          std::fill_n(accelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
          accelerationGains.resize(TWIST_SIZE, 1.0);
        }
      }

      if (nhLocal_.getParam("deceleration_limits", decelerationLimits))
      {
        if (decelerationLimits.size() != TWIST_SIZE)
        {
          ROS_ERROR_STREAM("Deceleration configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << decelerationLimits.size() << ". No control term will be used.");
          useControl_ = false;
        }
      }
      else
      {
        ROS_INFO_STREAM("use_control is set to true, but no deceleration_limits specified. Will use acceleration "
          "limits.");
        decelerationLimits = accelerationLimits;
      }

      if (nhLocal_.getParam("deceleration_gains", decelerationGains))
      {
        const int size = decelerationGains.size();
        if (size != TWIST_SIZE)
        {
          ROS_ERROR_STREAM("Deceleration gain configuration must be of size " << TWIST_SIZE <<
            ". Provided config was of size " << size << ". All gains will be assumed to be 1.");
          std::fill_n(decelerationGains.begin(), std::min(size, TWIST_SIZE), 1.0);
          decelerationGains.resize(TWIST_SIZE, 1.0);
        }
      }
      else
      {
        ROS_INFO_STREAM("use_control is set to true, but no deceleration_gains specified. Will use acceleration "
          "gains.");
        decelerationGains = accelerationGains;
      }
    }

    bool dynamicProcessNoiseCovariance = false;
    nhLocal_.param("dynamic_process_noise_covariance", dynamicProcessNoiseCovariance, false);
    filter_.setUseDynamicProcessNoiseCovariance(dynamicProcessNoiseCovariance);

    std::vector<double> initialState(STATE_SIZE, 0.0);
    if (nhLocal_.getParam("initial_state", initialState))
    {
      if (initialState.size() != STATE_SIZE)
      {
        ROS_ERROR_STREAM("Initial state must be of size " << STATE_SIZE << ". Provided config was of size " <<
          initialState.size() << ". The initial state will be ignored.");
      }
      else
      {
        Eigen::Map<Eigen::VectorXd> eigenState(initialState.data(), initialState.size());
        filter_.setState(eigenState);
      }
    }

    // Check if the filter should start or not
    nhLocal_.param("disabled_at_startup", disabledAtStartup_, false);
    enabled_ = !disabledAtStartup_;


    // Debugging writes to file
    RF_DEBUG("tf_prefix is " << tfPrefix <<
             "\nmap_frame is " << mapFrameId_ <<
             "\nodom_frame is " << odomFrameId_ <<
             "\nbase_link_frame is " << baseLinkFrameId_ <<
             "\base_link_frame_output is " << baseLinkOutputFrameId_ <<
             "\nworld_frame is " << worldFrameId_ <<
             "\ntransform_time_offset is " << tfTimeOffset_.toSec() <<
             "\ntransform_timeout is " << tfTimeout_.toSec() <<
             "\nfrequency is " << frequency_ <<
             "\nsensor_timeout is " << filter_.getSensorTimeout() <<
             "\ntwo_d_mode is " << (twoDMode_ ? "true" : "false") <<
             "\nsilent_tf_failure is " << (silentTfFailure_ ? "true" : "false") <<
             "\nsmooth_lagged_data is " << (smoothLaggedData_ ? "true" : "false") <<
             "\nhistory_length is " << historyLength_ <<
             "\nuse_control is " << (useControl_ ? "true" : "false") <<
             "\nstamped_control is " << (stampedControl ? "true" : "false") <<
             "\ncontrol_config is " << controlUpdateVector <<
             "\ncontrol_timeout is " << controlTimeout <<
             "\nacceleration_limits are " << accelerationLimits <<
             "\nacceleration_gains are " << accelerationGains <<
             "\ndeceleration_limits are " << decelerationLimits <<
             "\ndeceleration_gains are " << decelerationGains <<
             "\ninitial state is " << filter_.getState() <<
             "\ndynamic_process_noise_covariance is " << (dynamicProcessNoiseCovariance ? "true" : "false") <<
             "\nprint_diagnostics is " << (printDiagnostics_ ? "true" : "false") << "\n");

    // Create a subscriber for manually setting/resetting pose
    setPoseSub_ = nh_.subscribe("set_pose",
                                1,
                                &RosFilter<T>::setPoseCallback,
                                this, ros::TransportHints().tcpNoDelay(false));

    // Create a service for manually setting/resetting pose
    setPoseSrv_ = nh_.advertiseService("set_pose", &RosFilter<T>::setPoseSrvCallback, this);

    // Create a service for manually enabling the filter
    enableFilterSrv_ = nhLocal_.advertiseService("enable", &RosFilter<T>::enableFilterSrvCallback, this);

    // Create a service for toggling processing new measurements while still publishing
    toggleFilterProcessingSrv_ =
      nhLocal_.advertiseService("toggle", &RosFilter<T>::toggleFilterProcessingCallback, this);

    // Init the last measurement time so we don't get a huge initial delta
    filter_.setLastMeasurementTime(ros::Time::now().toSec());
//根据传感器数量加载传感器配置以及确定每个传感器的可更新分量配置，
    // Now pull in each topic to which we want to subscribe.
    // Start with odom.
    size_t topicInd = 0;
    bool moreParams = false;
    do
    {
      // Build the string in the form of "odomX", where X is the odom topic number,
      // then check if we have any parameters with that value. Users need to make
      // sure they don't have gaps in their configs (e.g., odom0 and then odom2)
      std::stringstream ss;
      ss << "odom" << topicInd++;
      std::string odomTopicName = ss.str();
      moreParams = nhLocal_.hasParam(odomTopicName);

      if (moreParams)
      {
        // Determine if we want to integrate this sensor differentially
        bool differential;
        nhLocal_.param(odomTopicName + std::string("_differential"), differential, false);

        // Determine if we want to integrate this sensor relatively
        bool relative;
        nhLocal_.param(odomTopicName + std::string("_relative"), relative, false);

        if (relative && differential)
        {
          ROS_WARN_STREAM("Both " << odomTopicName << "_differential" << " and " << odomTopicName <<
                          "_relative were set to true. Using differential mode.");

          relative = false;
        }

        std::string odomTopic;
        nhLocal_.getParam(odomTopicName, odomTopic);

        // Check for pose rejection threshold
        double poseMahalanobisThresh;
        nhLocal_.param(odomTopicName + std::string("_pose_rejection_threshold"),
                       poseMahalanobisThresh,
                       std::numeric_limits<double>::max());

        // Check for twist rejection threshold
        double twistMahalanobisThresh;
        nhLocal_.param(odomTopicName + std::string("_twist_rejection_threshold"),
                       twistMahalanobisThresh,
                       std::numeric_limits<double>::max());

        // Now pull in its boolean update vector configuration. Create separate vectors for pose
        // and twist data, and then zero out the opposite values in each vector (no pose data in
        // the twist update vector and vice-versa).
        std::vector<int> updateVec = loadUpdateConfig(odomTopicName);
        std::vector<int> poseUpdateVec = updateVec;
        std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET, poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE, 0);
        std::vector<int> twistUpdateVec = updateVec;
        std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);

        int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
        int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
        int odomQueueSize = 1;
        nhLocal_.param(odomTopicName + "_queue_size", odomQueueSize, 1);

        const CallbackData poseCallbackData(odomTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
          relative, poseMahalanobisThresh);
        const CallbackData twistCallbackData(odomTopicName + "_twist", twistUpdateVec, twistUpdateSum, false, false,
          twistMahalanobisThresh);

        bool nodelayOdom = false;
        nhLocal_.param(odomTopicName + "_nodelay", nodelayOdom, false);

        // Store the odometry topic subscribers so they don't go out of scope.
        if (poseUpdateSum + twistUpdateSum > 0)
        {
          topicSubs_.push_back(
            nh_.subscribe<nav_msgs::Odometry>(odomTopic, odomQueueSize,
              boost::bind(&RosFilter::odometryCallback, this, _1, odomTopicName, poseCallbackData, twistCallbackData),
              ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayOdom)));
        }
        else
        {
          std::stringstream stream;
          stream << odomTopic << " is listed as an input topic, but all update variables are false";

          addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
                        odomTopic + "_configuration",
                        stream.str(),
                        true);
        }

        if (poseUpdateSum > 0)
        {
          if (differential)
          {
            twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX];
            twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY];
            twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ];
            twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
            twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
            twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
          }
          else
          {
            absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX];
            absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY];
            absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ];
            absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
            absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
            absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
          }
        }

        if (twistUpdateSum > 0)
        {
          twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx];
          twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVx];
          twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz];
          twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
          twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
          twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
        }

        RF_DEBUG("Subscribed to " << odomTopic << " (" << odomTopicName << ")\n\t" <<
                 odomTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
                 odomTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
                 odomTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
                 odomTopicName << "_queue_size is " << odomQueueSize << "\n\t" <<
                 odomTopicName << " pose update vector is " << poseUpdateVec << "\t"<<
                 odomTopicName << " twist update vector is " << twistUpdateVec);
      }
    }
    while (moreParams);

    // Repeat for pose
    topicInd = 0;
    moreParams = false;
    do
    {
      std::stringstream ss;
      ss << "pose" << topicInd++;
      std::string poseTopicName = ss.str();
      moreParams = nhLocal_.hasParam(poseTopicName);

      if (moreParams)
      {
        bool differential;
        nhLocal_.param(poseTopicName + std::string("_differential"), differential, false);

        // Determine if we want to integrate this sensor relatively
        bool relative;
        nhLocal_.param(poseTopicName + std::string("_relative"), relative, false);

        if (relative && differential)
        {
          ROS_WARN_STREAM("Both " << poseTopicName << "_differential" << " and " << poseTopicName <<
                          "_relative were set to true. Using differential mode.");

          relative = false;
        }

        std::string poseTopic;
        nhLocal_.getParam(poseTopicName, poseTopic);

        // Check for pose rejection threshold
        double poseMahalanobisThresh;
        nhLocal_.param(poseTopicName + std::string("_rejection_threshold"),
                       poseMahalanobisThresh,
                       std::numeric_limits<double>::max());

        int poseQueueSize = 1;
        nhLocal_.param(poseTopicName + "_queue_size", poseQueueSize, 1);

        bool nodelayPose = false;
        nhLocal_.param(poseTopicName + "_nodelay", nodelayPose, false);

        // Pull in the sensor's config, zero out values that are invalid for the pose type
        std::vector<int> poseUpdateVec = loadUpdateConfig(poseTopicName);
        std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET,
                  poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
                  0);
        std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET,
                  poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
                  0);

        int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);

        if (poseUpdateSum > 0)
        {
          const CallbackData callbackData(poseTopicName, poseUpdateVec, poseUpdateSum, differential, relative,
            poseMahalanobisThresh);

          topicSubs_.push_back(
            nh_.subscribe<geometry_msgs::PoseWithCovarianceStamped>(poseTopic, poseQueueSize,
              boost::bind(&RosFilter::poseCallback, this, _1, callbackData, worldFrameId_, false),
              ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayPose)));

          if (differential)
          {
            twistVarCounts[StateMemberVx] += poseUpdateVec[StateMemberX];
            twistVarCounts[StateMemberVy] += poseUpdateVec[StateMemberY];
            twistVarCounts[StateMemberVz] += poseUpdateVec[StateMemberZ];
            twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
            twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
            twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
          }
          else
          {
            absPoseVarCounts[StateMemberX] += poseUpdateVec[StateMemberX];
            absPoseVarCounts[StateMemberY] += poseUpdateVec[StateMemberY];
            absPoseVarCounts[StateMemberZ] += poseUpdateVec[StateMemberZ];
            absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
            absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
            absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
          }
        }
        else
        {
          ROS_WARN_STREAM("Warning: " << poseTopic << " is listed as an input topic, "
                          "but all pose update variables are false");
        }

        RF_DEBUG("Subscribed to " << poseTopic << " (" << poseTopicName << ")\n\t" <<
                 poseTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
                 poseTopicName << "_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
                 poseTopicName << "_queue_size is " << poseQueueSize << "\n\t" <<
                 poseTopicName << " update vector is " << poseUpdateVec);
      }
    }
    while (moreParams);

    // Repeat for twist
    topicInd = 0;
    moreParams = false;
    do
    {
      std::stringstream ss;
      ss << "twist" << topicInd++;
      std::string twistTopicName = ss.str();
      moreParams = nhLocal_.hasParam(twistTopicName);

      if (moreParams)
      {
        std::string twistTopic;
        nhLocal_.getParam(twistTopicName, twistTopic);

        // Check for twist rejection threshold
        double twistMahalanobisThresh;
        nhLocal_.param(twistTopicName + std::string("_rejection_threshold"),
                       twistMahalanobisThresh,
                       std::numeric_limits<double>::max());

        int twistQueueSize = 1;
        nhLocal_.param(twistTopicName + "_queue_size", twistQueueSize, 1);

        bool nodelayTwist = false;
        nhLocal_.param(twistTopicName + "_nodelay", nodelayTwist, false);

        // Pull in the sensor's config, zero out values that are invalid for the twist type
        std::vector<int> twistUpdateVec = loadUpdateConfig(twistTopicName);
        std::fill(twistUpdateVec.begin() + POSITION_OFFSET, twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE, 0);

        int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);

        if (twistUpdateSum > 0)
        {
          const CallbackData callbackData(twistTopicName, twistUpdateVec, twistUpdateSum, false, false,
            twistMahalanobisThresh);

          topicSubs_.push_back(
            nh_.subscribe<geometry_msgs::TwistWithCovarianceStamped>(twistTopic, twistQueueSize,
              boost::bind(&RosFilter<T>::twistCallback, this, _1, callbackData, baseLinkFrameId_),
              ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayTwist)));

          twistVarCounts[StateMemberVx] += twistUpdateVec[StateMemberVx];
          twistVarCounts[StateMemberVy] += twistUpdateVec[StateMemberVy];
          twistVarCounts[StateMemberVz] += twistUpdateVec[StateMemberVz];
          twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
          twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
          twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
        }
        else
        {
          ROS_WARN_STREAM("Warning: " << twistTopic << " is listed as an input topic, "
                          "but all twist update variables are false");
        }

        RF_DEBUG("Subscribed to " << twistTopic << " (" << twistTopicName << ")\n\t" <<
                 twistTopicName << "_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
                 twistTopicName << "_queue_size is " << twistQueueSize << "\n\t" <<
                 twistTopicName << " update vector is " << twistUpdateVec);
      }
    }
    while (moreParams);

    // Repeat for IMU
    topicInd = 0;
    moreParams = false;
    do
    {
      std::stringstream ss;
      ss << "imu" << topicInd++;
      std::string imuTopicName = ss.str();
      moreParams = nhLocal_.hasParam(imuTopicName);

      if (moreParams)
      {
        bool differential;
        nhLocal_.param(imuTopicName + std::string("_differential"), differential, false);

        // Determine if we want to integrate this sensor relatively
        bool relative;
        nhLocal_.param(imuTopicName + std::string("_relative"), relative, false);

        if (relative && differential)
        {
          ROS_WARN_STREAM("Both " << imuTopicName << "_differential" << " and " << imuTopicName <<
                          "_relative were set to true. Using differential mode.");

          relative = false;
        }

        std::string imuTopic;
        nhLocal_.getParam(imuTopicName, imuTopic);

        // Check for pose rejection threshold
        double poseMahalanobisThresh;
        nhLocal_.param(imuTopicName + std::string("_pose_rejection_threshold"),
                       poseMahalanobisThresh,
                       std::numeric_limits<double>::max());

        // Check for angular velocity rejection threshold
        double twistMahalanobisThresh;
        std::string imuTwistRejectionName =
          imuTopicName + std::string("_twist_rejection_threshold");
        nhLocal_.param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());

        // Check for acceleration rejection threshold
        double accelMahalanobisThresh;
        nhLocal_.param(imuTopicName + std::string("_linear_acceleration_rejection_threshold"),
                       accelMahalanobisThresh,
                       std::numeric_limits<double>::max());

        bool removeGravAcc = false;
        nhLocal_.param(imuTopicName + "_remove_gravitational_acceleration", removeGravAcc, false);
        removeGravitationalAcc_[imuTopicName + "_acceleration"] = removeGravAcc;

        // Now pull in its boolean update vector configuration and differential
        // update configuration (as this contains pose information)
        std::vector<int> updateVec = loadUpdateConfig(imuTopicName);

        // sanity checks for update config settings
        std::vector<int> positionUpdateVec(updateVec.begin() + POSITION_OFFSET,
                                           updateVec.begin() + POSITION_OFFSET + POSITION_SIZE);
        int positionUpdateSum = std::accumulate(positionUpdateVec.begin(), positionUpdateVec.end(), 0);
        if (positionUpdateSum > 0)
        {
          ROS_WARN_STREAM("Warning: Some position entries in parameter " << imuTopicName << "_config are listed true, "
                          "but sensor_msgs/Imu contains no information about position");
        }
        std::vector<int> linearVelocityUpdateVec(updateVec.begin() + POSITION_V_OFFSET,
                                                 updateVec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE);
        int linearVelocityUpdateSum = std::accumulate(linearVelocityUpdateVec.begin(),
                                                      linearVelocityUpdateVec.end(),
                                                      0);
        if (linearVelocityUpdateSum > 0)
        {
          ROS_WARN_STREAM("Warning: Some linear velocity entries in parameter " << imuTopicName << "_config are listed "
                          "true, but an sensor_msgs/Imu contains no information about linear velocities");
        }

        std::vector<int> poseUpdateVec = updateVec;
        // IMU message contains no information about position, filter everything except orientation
        std::fill(poseUpdateVec.begin() + POSITION_OFFSET,
                  poseUpdateVec.begin() + POSITION_OFFSET + POSITION_SIZE,
                  0);
        std::fill(poseUpdateVec.begin() + POSITION_V_OFFSET,
                  poseUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
                  0);
        std::fill(poseUpdateVec.begin() + POSITION_A_OFFSET,
                  poseUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
                  0);

        std::vector<int> twistUpdateVec = updateVec;
        // IMU message contains no information about linear speeds, filter everything except angular velocity
        std::fill(twistUpdateVec.begin() + POSITION_OFFSET,
                  twistUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE,
                  0);
        std::fill(twistUpdateVec.begin() + POSITION_V_OFFSET,
                  twistUpdateVec.begin() + POSITION_V_OFFSET + LINEAR_VELOCITY_SIZE,
                  0);
        std::fill(twistUpdateVec.begin() + POSITION_A_OFFSET,
                  twistUpdateVec.begin() + POSITION_A_OFFSET + ACCELERATION_SIZE,
                  0);

        std::vector<int> accelUpdateVec = updateVec;
        std::fill(accelUpdateVec.begin() + POSITION_OFFSET,
                  accelUpdateVec.begin() + POSITION_OFFSET + POSE_SIZE,
                  0);
        std::fill(accelUpdateVec.begin() + POSITION_V_OFFSET,
                  accelUpdateVec.begin() + POSITION_V_OFFSET + TWIST_SIZE,
                  0);

        int poseUpdateSum = std::accumulate(poseUpdateVec.begin(), poseUpdateVec.end(), 0);
        int twistUpdateSum = std::accumulate(twistUpdateVec.begin(), twistUpdateVec.end(), 0);
        int accelUpdateSum = std::accumulate(accelUpdateVec.begin(), accelUpdateVec.end(), 0);

        // Check if we're using control input for any of the acceleration variables; turn off if so
        if (static_cast<bool>(controlUpdateVector[ControlMemberVx]) && static_cast<bool>(accelUpdateVec[StateMemberAx]))
        {
          ROS_WARN_STREAM("X acceleration is being measured from IMU; X velocity control input is disabled");
          controlUpdateVector[ControlMemberVx] = 0;
        }
        if (static_cast<bool>(controlUpdateVector[ControlMemberVy]) && static_cast<bool>(accelUpdateVec[StateMemberAy]))
        {
          ROS_WARN_STREAM("Y acceleration is being measured from IMU; Y velocity control input is disabled");
          controlUpdateVector[ControlMemberVy] = 0;
        }
        if (static_cast<bool>(controlUpdateVector[ControlMemberVz]) && static_cast<bool>(accelUpdateVec[StateMemberAz]))
        {
          ROS_WARN_STREAM("Z acceleration is being measured from IMU; Z velocity control input is disabled");
          controlUpdateVector[ControlMemberVz] = 0;
        }

        int imuQueueSize = 1;
        nhLocal_.param(imuTopicName + "_queue_size", imuQueueSize, 1);

        bool nodelayImu = false;
        nhLocal_.param(imuTopicName + "_nodelay", nodelayImu, false);

        if (poseUpdateSum + twistUpdateSum + accelUpdateSum > 0)
        {
          const CallbackData poseCallbackData(imuTopicName + "_pose", poseUpdateVec, poseUpdateSum, differential,
            relative, poseMahalanobisThresh);
          const CallbackData twistCallbackData(imuTopicName + "_twist", twistUpdateVec, twistUpdateSum, differential,
            relative, twistMahalanobisThresh);
          const CallbackData accelCallbackData(imuTopicName + "_acceleration", accelUpdateVec, accelUpdateSum,
            differential, relative, accelMahalanobisThresh);

          topicSubs_.push_back(
            nh_.subscribe<sensor_msgs::Imu>(imuTopic, imuQueueSize,
              boost::bind(&RosFilter<T>::imuCallback, this, _1, imuTopicName, poseCallbackData, twistCallbackData,
                accelCallbackData), ros::VoidPtr(), ros::TransportHints().tcpNoDelay(nodelayImu)));
        }
        else
        {
          ROS_WARN_STREAM("Warning: " << imuTopic << " is listed as an input topic, "
                          "but all its update variables are false");
        }

        if (poseUpdateSum > 0)
        {
          if (differential)
          {
            twistVarCounts[StateMemberVroll] += poseUpdateVec[StateMemberRoll];
            twistVarCounts[StateMemberVpitch] += poseUpdateVec[StateMemberPitch];
            twistVarCounts[StateMemberVyaw] += poseUpdateVec[StateMemberYaw];
          }
          else
          {
            absPoseVarCounts[StateMemberRoll] += poseUpdateVec[StateMemberRoll];
            absPoseVarCounts[StateMemberPitch] += poseUpdateVec[StateMemberPitch];
            absPoseVarCounts[StateMemberYaw] += poseUpdateVec[StateMemberYaw];
          }
        }

        if (twistUpdateSum > 0)
        {
          twistVarCounts[StateMemberVroll] += twistUpdateVec[StateMemberVroll];
          twistVarCounts[StateMemberVpitch] += twistUpdateVec[StateMemberVpitch];
          twistVarCounts[StateMemberVyaw] += twistUpdateVec[StateMemberVyaw];
        }

        RF_DEBUG("Subscribed to " << imuTopic << " (" << imuTopicName << ")\n\t" <<
                 imuTopicName << "_differential is " << (differential ? "true" : "false") << "\n\t" <<
                 imuTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "\n\t" <<
                 imuTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "\n\t" <<
                 imuTopicName << "_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh << "\n\t" <<
                 imuTopicName << "_remove_gravitational_acceleration is " <<
                                 (removeGravAcc ? "true" : "false") << "\n\t" <<
                 imuTopicName << "_queue_size is " << imuQueueSize << "\n\t" <<
                 imuTopicName << " pose update vector is " << poseUpdateVec << "\t"<<
                 imuTopicName << " twist update vector is " << twistUpdateVec << "\t" <<
                 imuTopicName << " acceleration update vector is " << accelUpdateVec);
      }
    }
    while (moreParams);

    // Now that we've checked if IMU linear acceleration is being used, we can determine our final control parameters
    if (useControl_ && std::accumulate(controlUpdateVector.begin(), controlUpdateVector.end(), 0) == 0)
    {
      ROS_ERROR_STREAM("use_control is set to true, but control_config has only false values. No control term "
        "will be used.");
      useControl_ = false;
    }

    // If we're using control, set the parameters and create the necessary subscribers
    if (useControl_)
    {
      latestControl_.resize(TWIST_SIZE);
      latestControl_.setZero();

      filter_.setControlParams(controlUpdateVector, controlTimeout, accelerationLimits, accelerationGains,
        decelerationLimits, decelerationGains);

      if (stampedControl)
      {
        controlSub_ = nh_.subscribe<geometry_msgs::TwistStamped>("cmd_vel", 1, &RosFilter<T>::controlCallback, this);
      }
      else
      {
        controlSub_ = nh_.subscribe<geometry_msgs::Twist>("cmd_vel", 1, &RosFilter<T>::controlCallback, this);
      }
    }

    /* Warn users about:
    *    1. Multiple non-differential input sources
    *    2. No absolute *or* velocity measurements for pose variables
    */
    if (printDiagnostics_)
    {
      for (int stateVar = StateMemberX; stateVar <= StateMemberYaw; ++stateVar)
      {
        if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] > 1)
        {
          std::stringstream stream;
          stream <<  absPoseVarCounts[static_cast<StateMembers>(stateVar - POSITION_OFFSET)] <<
              " absolute pose inputs detected for " << stateVariableNames_[stateVar] <<
              ". This may result in oscillations. Please ensure that your variances for each "
              "measured variable are set appropriately.";

          addDiagnostic(diagnostic_msgs::DiagnosticStatus::WARN,
                        stateVariableNames_[stateVar] + "_configuration",
                        stream.str(),
                        true);
        }
        else if (absPoseVarCounts[static_cast<StateMembers>(stateVar)] == 0)
        {
          if ((static_cast<StateMembers>(stateVar) == StateMemberX &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVx)] == 0) ||
              (static_cast<StateMembers>(stateVar) == StateMemberY &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVy)] == 0) ||
              (static_cast<StateMembers>(stateVar) == StateMemberZ &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVz)] == 0 &&
               twoDMode_ == false) ||
              (static_cast<StateMembers>(stateVar) == StateMemberRoll &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVroll)] == 0 &&
               twoDMode_ == false) ||
              (static_cast<StateMembers>(stateVar) == StateMemberPitch &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVpitch)] == 0 &&
               twoDMode_ == false) ||
              (static_cast<StateMembers>(stateVar) == StateMemberYaw &&
               twistVarCounts[static_cast<StateMembers>(StateMemberVyaw)] == 0))
          {
            std::stringstream stream;
            stream << "Neither " << stateVariableNames_[stateVar] << " nor its "
                      "velocity is being measured. This will result in unbounded "
                      "error growth and erratic filter behavior.";

            addDiagnostic(diagnostic_msgs::DiagnosticStatus::ERROR,
                          stateVariableNames_[stateVar] + "_configuration",
                          stream.str(),
                          true);
          }
        }
      }
    }

    // Load up the process noise covariance (from the launch file/parameter server)
    Eigen::MatrixXd processNoiseCovariance(STATE_SIZE, STATE_SIZE);
    processNoiseCovariance.setZero();
    XmlRpc::XmlRpcValue processNoiseCovarConfig;

    if (nhLocal_.hasParam("process_noise_covariance"))
    {
      try
      {
        nhLocal_.getParam("process_noise_covariance", processNoiseCovarConfig);

        ROS_ASSERT(processNoiseCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);

        int matSize = processNoiseCovariance.rows();

        for (int i = 0; i < matSize; i++)
        {
          for (int j = 0; j < matSize; j++)
          {
            try
            {
              // These matrices can cause problems if all the types
              // aren't specified with decimal points. Handle that
              // using string streams.
              std::ostringstream ostr;
              ostr << processNoiseCovarConfig[matSize * i + j];
              std::istringstream istr(ostr.str());
              istr >> processNoiseCovariance(i, j);
            }
            catch(XmlRpc::XmlRpcException &e)
            {
              throw e;
            }
            catch(...)
            {
              throw;
            }
          }
        }

        RF_DEBUG("Process noise covariance is:\n" << processNoiseCovariance << "\n");
      }
      catch (XmlRpc::XmlRpcException &e)
      {
        ROS_ERROR_STREAM("ERROR reading sensor config: " <<
                         e.getMessage() <<
                         " for process_noise_covariance (type: " <<
                         processNoiseCovarConfig.getType() << ")");
      }

      filter_.setProcessNoiseCovariance(processNoiseCovariance);
    }

    // Load up the process noise covariance (from the launch file/parameter server)
    Eigen::MatrixXd initialEstimateErrorCovariance(STATE_SIZE, STATE_SIZE);
    initialEstimateErrorCovariance.setZero();
    XmlRpc::XmlRpcValue estimateErrorCovarConfig;

    if (nhLocal_.hasParam("initial_estimate_covariance"))
    {
      try
      {
        nhLocal_.getParam("initial_estimate_covariance", estimateErrorCovarConfig);

        ROS_ASSERT(estimateErrorCovarConfig.getType() == XmlRpc::XmlRpcValue::TypeArray);

        int matSize = initialEstimateErrorCovariance.rows();

        for (int i = 0; i < matSize; i++)
        {
          for (int j = 0; j < matSize; j++)
          {
            try
            {
              // These matrices can cause problems if all the types
              // aren't specified with decimal points. Handle that
              // using string streams.
              std::ostringstream ostr;
              ostr << estimateErrorCovarConfig[matSize * i + j];
              std::istringstream istr(ostr.str());
              istr >> initialEstimateErrorCovariance(i, j);
            }
            catch(XmlRpc::XmlRpcException &e)
            {
              throw e;
            }
            catch(...)
            {
              throw;
            }
          }
        }

        RF_DEBUG("Initial estimate error covariance is:\n" << initialEstimateErrorCovariance << "\n");
      }
      catch (XmlRpc::XmlRpcException &e)
      {
        ROS_ERROR_STREAM("ERROR reading initial_estimate_covariance (type: " <<
                         estimateErrorCovarConfig.getType() <<
                         "): " <<
                         e.getMessage());
      }
      catch(...)
      {
        ROS_ERROR_STREAM(
          "ERROR reading initial_estimate_covariance (type: " << estimateErrorCovarConfig.getType() << ")");
      }

      filter_.setEstimateErrorCovariance(initialEstimateErrorCovariance);
    }
  }

2.2 periodicUpdate() 循环处理函数

  template<typename T>
  void RosFilter<T>::periodicUpdate(const ros::TimerEvent& event)
  {
    // Warn the user if the update took too long (> 2 cycles)
    const double loop_elapsed = (event.current_real - event.last_expected).toSec();
    if (loop_elapsed > 2./frequency_)
    {
      ROS_WARN_STREAM("Failed to meet update rate! Took " << std::setprecision(20) << loop_elapsed);
    }

    // Wait for the filter to be enabled
    if (!enabled_)
    {
      ROS_INFO_STREAM_ONCE("Filter is disabled. To enable it call the " << enableFilterSrv_.getService() <<
        " service");
      return;
    }

    ros::Time curTime = ros::Time::now();

    if (toggledOn_)
    {
      // Now we'll integrate any measurements we've received if requested
      integrateMeasurements(curTime);
    }
    else
    {
      // clear out measurements since we're not currently processing new entries
      clearMeasurementQueue();

      // Reset last measurement time so we don't get a large time delta on toggle on
      if (filter_.getInitializedStatus())
      {
        filter_.setLastMeasurementTime(ros::Time::now().toSec());
      }
    }

    // Get latest state and publish it
    nav_msgs::Odometry filteredPosition;

    if (getFilteredOdometryMessage(filteredPosition))
    {
      worldBaseLinkTransMsg_.transform = tf2::toMsg(tf2::Transform::getIdentity());
      worldBaseLinkTransMsg_.header.stamp = filteredPosition.header.stamp + tfTimeOffset_;
      worldBaseLinkTransMsg_.header.frame_id = filteredPosition.header.frame_id;
      worldBaseLinkTransMsg_.child_frame_id = filteredPosition.child_frame_id;

      worldBaseLinkTransMsg_.transform.translation.x = filteredPosition.pose.pose.position.x;
      worldBaseLinkTransMsg_.transform.translation.y = filteredPosition.pose.pose.position.y;
      worldBaseLinkTransMsg_.transform.translation.z = filteredPosition.pose.pose.position.z;
      worldBaseLinkTransMsg_.transform.rotation = filteredPosition.pose.pose.orientation;

      // the filteredPosition is the message containing the state and covariances: nav_msgs Odometry

      if (!validateFilterOutput(filteredPosition))
      {
        ROS_ERROR_STREAM("Critical Error, NaNs were detected in the output state of the filter." <<
              " This was likely due to poorly coniditioned process, noise, or sensor covariances.");
      }

      // If the worldFrameId_ is the odomFrameId_ frame, then we can just send the transform. If the
      // worldFrameId_ is the mapFrameId_ frame, we'll have some work to do.
      if (publishTransform_)
      {
        if (filteredPosition.header.frame_id == odomFrameId_)
        {
          worldTransformBroadcaster_.sendTransform(worldBaseLinkTransMsg_);
        }
        else if (filteredPosition.header.frame_id == mapFrameId_)
        {
          try
          {
            tf2::Transform worldBaseLinkTrans;
            tf2::fromMsg(worldBaseLinkTransMsg_.transform, worldBaseLinkTrans);

            tf2::Transform baseLinkOdomTrans;
            tf2::fromMsg(tfBuffer_.lookupTransform(baseLinkFrameId_, odomFrameId_, ros::Time(0)).transform,
                         baseLinkOdomTrans);

            /*
             * First, see these two references:
             * http://wiki.ros.org/tf/Overview/Using%20Published%20Transforms#lookupTransform
             * http://wiki.ros.org/geometry/CoordinateFrameConventions#Transform_Direction
             * We have a transform from mapFrameId_->baseLinkFrameId_, but it would actually transform
             * a given pose from baseLinkFrameId_->mapFrameId_. We then used lookupTransform, whose
             * first two arguments are target frame and source frame, to get a transform from
             * baseLinkFrameId_->odomFrameId_. However, this transform would actually transform data
             * from odomFrameId_->baseLinkFrameId_. Now imagine that we have a position in the
             * mapFrameId_ frame. First, we multiply it by the inverse of the
             * mapFrameId_->baseLinkFrameId, which will transform that data from mapFrameId_ to
             * baseLinkFrameId_. Now we want to go from baseLinkFrameId_->odomFrameId_, but the
             * transform we have takes data from odomFrameId_->baseLinkFrameId_, so we need its
             * inverse as well. We have now transformed our data from mapFrameId_ to odomFrameId_.
             * However, if we want other users to be able to do the same, we need to broadcast
             * the inverse of that entire transform.
            */

            tf2::Transform mapOdomTrans;
            mapOdomTrans.mult(worldBaseLinkTrans, baseLinkOdomTrans);

            geometry_msgs::TransformStamped mapOdomTransMsg;
            mapOdomTransMsg.transform = tf2::toMsg(mapOdomTrans);
            mapOdomTransMsg.header.stamp = filteredPosition.header.stamp + tfTimeOffset_;
            mapOdomTransMsg.header.frame_id = mapFrameId_;
            mapOdomTransMsg.child_frame_id = odomFrameId_;

            worldTransformBroadcaster_.sendTransform(mapOdomTransMsg);
          }
          catch(...)
          {
            ROS_ERROR_STREAM_DELAYED_THROTTLE(5.0, "Could not obtain transform from "
                                              << odomFrameId_ << "->" << baseLinkFrameId_);
          }
        }
        else
        {
          ROS_ERROR_STREAM("Odometry message frame_id was " << filteredPosition.header.frame_id <<
                           ", expected " << mapFrameId_ << " or " << odomFrameId_);
        }
      }

      // Fire off the position and the transform
      positionPub_.publish(filteredPosition);

      if (printDiagnostics_)
      {
        freqDiag_->tick();
      }
    }

    // Publish the acceleration if desired and filter is initialized
    geometry_msgs::AccelWithCovarianceStamped filteredAcceleration;
    if (publishAcceleration_ && getFilteredAccelMessage(filteredAcceleration))
    {
      accelPub_.publish(filteredAcceleration);
    }

    /* Diagnostics can behave strangely when playing back from bag
     * files and using simulated time, so we have to check for
     * time suddenly moving backwards as well as the standard
     * timeout criterion before publishing. */
    double diagDuration = (curTime - lastDiagTime_).toSec();
    if (printDiagnostics_ && (diagDuration >= diagnosticUpdater_.getPeriod() || diagDuration < 0.0))
    {
      diagnosticUpdater_.force_update();
      lastDiagTime_ = curTime;
    }

    // Clear out expired history data
    if (smoothLaggedData_)
    {
      clearExpiredHistory(filter_.getLastMeasurementTime() - historyLength_);
    }
  }

2.2.1 Ekf::predict()

$$\begin{gathered} R=R_z{R}_y{R}_x \\ =\left(\begin{array}{ccc}cos{\theta }_{y}cos{\theta }_z& sin{\theta }_{x}sin{\theta }_{y}cos{\theta }_z-cos{\theta }_{x}sin{\theta }_z& cos{\theta }_{x}sin{\theta }_{y}cos{\theta }_z+sin{\theta }_{x}sin{\theta }_z\\ cos{\theta }_{y}sin{\theta }_z& sin{\theta }_{x}sin{\theta }_{y}sin{\theta }_z+cos{\theta }_{x}cos{\theta }_z& cos{\theta }_{x}sin{\theta }_{y}sin{\theta }_z-sin{\theta }_{x}cos{\theta }_z\\ -sin{\theta }_y& sin{\theta }_{x}cos{\theta }_y& cos{\theta }_{x}cos{\theta }_y\end{array}\right)（1） \end{gathered}$$
注意，状态向量中$（x,y,z,roll,pitch,yawl）$是在世界坐标系下，状态向量中$（\dot{x},\dot{y},\dot{z},\dot{{\theta }_x},\dot{{\theta }_y},\dot{{\theta }_z},\ddot{x},\ddot{y},\ddot{z}）$是在载体坐标系下的，因此在载体坐标系下进行运动估计后，还需要根据式（1）中的坐标转换矩阵将结果转换到世界坐标系下，于是就可以得到代码中的状态转移矩阵：

    //X是在世界坐标系下，X的一阶导和二阶导是在载体坐标系下，因此需要根据k-1时刻的姿态转换到世界坐标系下
    // Prepare the transfer function
    transferFunction_(StateMemberX, StateMemberVx) = cy * cp * delta;
    transferFunction_(StateMemberX, StateMemberVy) = (cy * sp * sr - sy * cr) * delta;
    transferFunction_(StateMemberX, StateMemberVz) = (cy * sp * cr + sy * sr) * delta;
    transferFunction_(StateMemberX, StateMemberAx) = 0.5 * transferFunction_(StateMemberX, StateMemberVx) * delta;
    transferFunction_(StateMemberX, StateMemberAy) = 0.5 * transferFunction_(StateMemberX, StateMemberVy) * delta;
    transferFunction_(StateMemberX, StateMemberAz) = 0.5 * transferFunction_(StateMemberX, StateMemberVz) * delta;
    transferFunction_(StateMemberY, StateMemberVx) = sy * cp * delta;
    transferFunction_(StateMemberY, StateMemberVy) = (sy * sp * sr + cy * cr) * delta;
    transferFunction_(StateMemberY, StateMemberVz) = (sy * sp * cr - cy * sr) * delta;
    transferFunction_(StateMemberY, StateMemberAx) = 0.5 * transferFunction_(StateMemberY, StateMemberVx) * delta;
    transferFunction_(StateMemberY, StateMemberAy) = 0.5 * transferFunction_(StateMemberY, StateMemberVy) * delta;
    transferFunction_(StateMemberY, StateMemberAz) = 0.5 * transferFunction_(StateMemberY, StateMemberVz) * delta;
    transferFunction_(StateMemberZ, StateMemberVx) = -sp * delta;
    transferFunction_(StateMemberZ, StateMemberVy) = cp * sr * delta;
    transferFunction_(StateMemberZ, StateMemberVz) = cp * cr * delta;
    transferFunction_(StateMemberZ, StateMemberAx) = 0.5 * transferFunction_(StateMemberZ, StateMemberVx) * delta;
    transferFunction_(StateMemberZ, StateMemberAy) = 0.5 * transferFunction_(StateMemberZ, StateMemberVy) * delta;
    transferFunction_(StateMemberZ, StateMemberAz) = 0.5 * transferFunction_(StateMemberZ, StateMemberVz) * delta;
    transferFunction_(StateMemberRoll, StateMemberVroll) = delta;
    transferFunction_(StateMemberRoll, StateMemberVpitch) = sr * tp * delta;
    transferFunction_(StateMemberRoll, StateMemberVyaw) = cr * tp * delta;
    transferFunction_(StateMemberPitch, StateMemberVpitch) = cr * delta;
    transferFunction_(StateMemberPitch, StateMemberVyaw) = -sr * delta;
    transferFunction_(StateMemberYaw, StateMemberVpitch) = sr * cpi * delta;
    transferFunction_(StateMemberYaw, StateMemberVyaw) = cr * cpi * delta;
    transferFunction_(StateMemberVx, StateMemberAx) = delta;
    transferFunction_(StateMemberVy, StateMemberAy) = delta;
    transferFunction_(StateMemberVz, StateMemberAz) = delta;

由于状态转移矩阵中存在这三角函数，因此这个状态转移矩阵是非线性的，因此必须求解与状态转移矩阵对应的雅克比矩阵transferFunctionJacobian_，在此以$\frac{\partial x}{\partial {\theta }_x}$为例记性推导：
由非线性状态转移矩阵transferFunction_可知：
$$\begin{gathered} x_k=x_{k-1}+cos{\theta }_{z}cos{\theta }_y\Delta t{v}_x+(cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x-sin{\theta }_{z}cos{\theta }_x)\Delta t{v}_y+ \\ (cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)\Delta t{v}_z+ \\ \frac{1}{2}cos{\theta }_{z}cos{\theta }_y\Delta t\Delta t{a}_x+ \\ \frac{1}{2}(cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x-sin{\theta }_{z}cos{\theta }_x)\Delta t\Delta t{a}_y+ \\ \frac{1}{2}(cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)\Delta t\Delta t{a}_z \end{gathered}$$
由上式对${\theta }_x$求偏导可以得到：
$$\begin{gathered} \frac{\partial x_k}{\partial {\theta }_x}=(cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)\Delta t{v}_y+ \\ (-cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x+sin{\theta }_{z}sin{\theta }_y)\Delta t{v}_z+ \\ \frac{1}{2}(cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)\Delta t\Delta t{a}_y+ \\ \frac{1}{2}(-cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x+sin{\theta }_{z}sin{\theta }_y\Delta t\Delta t{a}_z \\ =[(cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)\Delta t{v}_y+(-cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x+sin{\theta }_{z}sin{\theta }_y)\Delta t{v}_z]\Delta t+ \\ [(cos{\theta }_{z}sin{\theta }_{y}cos{\theta }_x+sin{\theta }_{z}sin{\theta }_x)a_y+(-cos{\theta }_{z}sin{\theta }_{y}sin{\theta }_x+sin{\theta }_{z}sin{\theta }_y)a_z]\frac{1}{2}\Delta t\Delta t \end{gathered}$$
于是可以得到如下所示的雅克比矩阵$F$：

  // Prepare the transfer function Jacobian. This function is analytically derived from the
    // transfer function.
    double xCoeff = 0.0;
    double yCoeff = 0.0;
    double zCoeff = 0.0;
    double oneHalfATSquared = 0.5 * delta * delta;

    yCoeff = cy * sp * cr + sy * sr;
    zCoeff = -cy * sp * sr + sy * cr;
    double dFx_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
                    (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
    double dFR_dR = 1.0 + (cr * tp * pitchVel - sr * tp * yawVel) * delta;

    xCoeff = -cy * sp;
    yCoeff = cy * cp * sr;
    zCoeff = cy * cp * cr;
    double dFx_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                    (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
    double dFR_dP = (cpi * cpi * sr * pitchVel + cpi * cpi * cr * yawVel) * delta;

    xCoeff = -sy * cp;
    yCoeff = -sy * sp * sr - cy * cr;
    zCoeff = -sy * sp * cr + cy * sr;
    double dFx_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                    (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;

    yCoeff = sy * sp * cr - cy * sr;
    zCoeff = -sy * sp * sr - cy * cr;
    double dFy_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
                    (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
    double dFP_dR = (-sr * pitchVel - cr * yawVel) * delta;

    xCoeff = -sy * sp;
    yCoeff = sy * cp * sr;
    zCoeff = sy * cp * cr;
    double dFy_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                    (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;

    xCoeff = cy * cp;
    yCoeff = cy * sp * sr - sy * cr;
    zCoeff = cy * sp * cr + sy * sr;
    double dFy_dY = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                    (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;

    yCoeff = cp * cr;
    zCoeff = -cp * sr;
    double dFz_dR = (yCoeff * yVel + zCoeff * zVel) * delta +
                    (yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
    double dFY_dR = (cr * cpi * pitchVel - sr * cpi * yawVel) * delta;

    xCoeff = -cp;
    yCoeff = -sp * sr;
    zCoeff = -sp * cr;
    double dFz_dP = (xCoeff * xVel + yCoeff * yVel + zCoeff * zVel) * delta +
                    (xCoeff * xAcc + yCoeff * yAcc + zCoeff * zAcc) * oneHalfATSquared;
    double dFY_dP = (sr * tp * cpi * pitchVel + cr * tp * cpi * yawVel) * delta;

    // Much of the transfer function Jacobian is identical to the transfer function
    transferFunctionJacobian_ = transferFunction_;
    transferFunctionJacobian_(StateMemberX, StateMemberRoll) = dFx_dR;
    transferFunctionJacobian_(StateMemberX, StateMemberPitch) = dFx_dP;
    transferFunctionJacobian_(StateMemberX, StateMemberYaw) = dFx_dY;
    transferFunctionJacobian_(StateMemberY, StateMemberRoll) = dFy_dR;
    transferFunctionJacobian_(StateMemberY, StateMemberPitch) = dFy_dP;
    transferFunctionJacobian_(StateMemberY, StateMemberYaw) = dFy_dY;
    transferFunctionJacobian_(StateMemberZ, StateMemberRoll) = dFz_dR;
    transferFunctionJacobian_(StateMemberZ, StateMemberPitch) = dFz_dP;
    transferFunctionJacobian_(StateMemberRoll, StateMemberRoll) = dFR_dR;
    transferFunctionJacobian_(StateMemberRoll, StateMemberPitch) = dFR_dP;
    transferFunctionJacobian_(StateMemberPitch, StateMemberRoll) = dFP_dR;
    transferFunctionJacobian_(StateMemberYaw, StateMemberRoll) = dFY_dR;
    transferFunctionJacobian_(StateMemberYaw, StateMemberPitch) = dFY_dP;

    FB_DEBUG("Transfer function is:\n" << transferFunction_ <<
             "\nTransfer function Jacobian is:\n" << transferFunctionJacobian_ <<
             "\nProcess noise covariance is:\n" << processNoiseCovariance_ <<
             "\nCurrent state is:\n" << state_ << "\n");

EKF公式和代码的对应关系为：

Eigen::VectorXd stateSubset(updateSize) -> 状态向量： $X$
Eigen::VectorXd measurementSubset(updateSize) -> 观测向量： $Z$
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); ->测量噪声： $R$
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); -> 测量矩阵：$H$
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); -> 卡尔曼增益：$K$
Eigen::VectorXd innovationSubset(updateSize); -> 观测误差：$Z-HX$
transferFunctionJacobian_ -> 线性化后的状态转移矩阵：$F$
于是可以得到：
预测状态向量的状态转移矩阵为：
$${\bar{X}}_k=f({\hat{X}}_{k-1},u_k)$$
即为：
$${\bar{X}}_k=A{\hat{X}}_{k-1}+u_k$$
对应的代码为：

    // (1) Apply control terms, which are actually accelerations
    state_(StateMemberVroll) += controlAcceleration_(ControlMemberVroll) * delta;
    state_(StateMemberVpitch) += controlAcceleration_(ControlMemberVpitch) * delta;
    state_(StateMemberVyaw) += controlAcceleration_(ControlMemberVyaw) * delta;

    state_(StateMemberAx) = (controlUpdateVector_[ControlMemberVx] ?
      controlAcceleration_(ControlMemberVx) : state_(StateMemberAx));
    state_(StateMemberAy) = (controlUpdateVector_[ControlMemberVy] ?
      controlAcceleration_(ControlMemberVy) : state_(StateMemberAy));
    state_(StateMemberAz) = (controlUpdateVector_[ControlMemberVz] ?
      controlAcceleration_(ControlMemberVz) : state_(StateMemberAz));

    // (2) Project the state forward: x = Ax + Bu (really, x = f(x, u))
    state_ = transferFunction_ * state_;

预测状态向量的噪声协方差矩阵为：
$${\bar{P}}_k=F{\hat{P}}_k{F}^T+R_k$$
对应的代码为：

    // (3) Project the error forward: P = J * P * J' + Q
    estimateErrorCovariance_ = (transferFunctionJacobian_ *
                                estimateErrorCovariance_ *
                                transferFunctionJacobian_.transpose());
    estimateErrorCovariance_.noalias() += delta * (*processNoiseCovariance);

    FB_DEBUG("Predicted estimate error covariance is:\n" << estimateErrorCovariance_ <<
             "\n\n--------------------- /Ekf::predict ----------------------\n");

2.2.2 Ekf::correct()

在校正过程中，首先计算Kalman增益：
$${K}_k=\frac{{\bar{P}}_k{H}^T}{H{\bar{P}}_k{H}^T+Q_k}$$
对应的代码为：

    // (1) Compute the Kalman gain: K = (PH') / (HPH' + R)
    Eigen::MatrixXd pht = estimateErrorCovariance_ * stateToMeasurementSubset.transpose();
    Eigen::MatrixXd hphrInv  = (stateToMeasurementSubset * pht + measurementCovarianceSubset).inverse();
    kalmanGainSubset.noalias() = pht * hphrInv;

然后利用当前观测和预测结果进行校正获得最终融合的结果：
$${\hat{X}}_k={\bar{X}}_k+K_k(Z_k-h({\bar{X}}_k))$$
对应的代码为：

    innovationSubset = (measurementSubset - stateSubset);

    // Wrap angles in the innovation
    for (size_t i = 0; i < updateSize; ++i)
    {
      if (updateIndices[i] == StateMemberRoll  ||
          updateIndices[i] == StateMemberPitch ||
          updateIndices[i] == StateMemberYaw)
      {
        while (innovationSubset(i) < -PI)
        {
          innovationSubset(i) += TAU;
        }

        while (innovationSubset(i) > PI)
        {
          innovationSubset(i) -= TAU;
        }
      }
    }

    // (2) Check Mahalanobis distance between mapped measurement and state.
    if (checkMahalanobisThreshold(innovationSubset, hphrInv, measurement.mahalanobisThresh_))
    {
      // (3) Apply the gain to the difference between the state and measurement: x = x + K(z - Hx)
      state_.noalias() += kalmanGainSubset * innovationSubset;

并获得最终状态的噪声协方差矩阵：
$${\hat{P}}_k=(I-K_{k}H){\bar{P}}_k(I-K_{k}H{)}^T+K_{k}R{K}_k^T$$
对应的代码为：

      // (4) Update the estimate error covariance using the Joseph form: (I - KH)P(I - KH)' + KRK'
      Eigen::MatrixXd gainResidual = identity_;
      gainResidual.noalias() -= kalmanGainSubset * stateToMeasurementSubset;
      estimateErrorCovariance_ = gainResidual * estimateErrorCovariance_ * gainResidual.transpose();
      estimateErrorCovariance_.noalias() += kalmanGainSubset *
                                            measurementCovarianceSubset *
                                            kalmanGainSubset.transpose();

未完，待续……