robot_localization 源码解析（1）ekf_localization

robot_localization 源码解析（1）ekf_localization_node

1. 简介
- 1.1 坐标系定义
2. 实例化RosEkf
2. 初始化RosEkf
- 2.1 loadParams()加载参数
- 2.2 periodicUpdate() 循环处理函数
- - 2.2.1 Ekf::predict()
  - 2.2.2 Ekf::correct()

1. 简介

1.1 坐标系定义

状态向量为：
X=（x,y,z,θx,θy,θz,x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,z¨)TX = （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=（x,y,z,θx,θy,θz,x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,z¨)T
状态向量中（x,y,z,θx,θy,θz)（x,y,z,\theta_x,\theta_y,\theta_z)（x,y,z,θx,θy,θz)为世界坐标系下的绝对姿态，（x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,z¨)（\dot{x},\dot{y},\dot{z},\dot{\theta_x},\dot{\theta_y},\dot{\theta_z},\ddot{x},\ddot{y},\ddot{z})（x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,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);//实例化RosEkfekf.initialize();//初始化RosEkfros::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 diagnosticminFrequency_ = frequency_ - 2;maxFrequency_ = frequency_ + 2;freqDiag_ = std::make_unique<diagnostic_updater::HeaderlessTopicDiagnostic>("odometry/filtered",diagnosticUpdater_,diagnostic_updater::FrequencyStatusParam(&minFrequency_,&maxFrequency_,0.1,10));// PublisherpositionPub_ = nh_.advertise<nav_msgs::Odometry>("odometry/filtered", 20);// Optional acceleration publisherif (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 variablesstd::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 informationnhLocal_.param("print_diagnostics", printDiagnostics_, true);// Check for custom gravitational acceleration valuenhLocal_.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 succeededif (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_prefixstd::string tfPrefix = "";std::string tfPrefixPath = "";if (nhLocal_.searchParam("tf_prefix", tfPrefixPath)){nhLocal_.getParam(tfPrefixPath, tfPrefix);}// Append the tf prefix in a tf2-friendly mannerFilterUtilities::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 transformnhLocal_.param("publish_tf", publishTransform_, true);// Whether we're publishing the acceleration state transformnhLocal_.param("publish_acceleration", publishAcceleration_, false);// Transform future datingdouble offsetTmp;nhLocal_.param("transform_time_offset", offsetTmp, 0.0);tfTimeOffset_.fromSec(offsetTmp);// Transform timeoutdouble timeoutTmp;nhLocal_.param("transform_timeout", timeoutTmp, 0.0);tfTimeout_.fromSec(timeoutTmp);// Update frequency and sensor timeoutdouble sensorTimeout;nhLocal_.param("frequency", frequency_, 30.0);nhLocal_.param("sensor_timeout", sensorTimeout, 1.0 / frequency_);filter_.setSensorTimeout(sensorTimeout);// Determine if we're in 2D modenhLocal_.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 parametersnh_.param("/silent_tf_failure", silentTfFailure_, false);// Smoothing window sizenhLocal_.param("smooth_lagged_data", smoothLaggedData_, false);nhLocal_.param("history_length", historyLength_, 0.0);// Wether we reset filter on jump back in timenhLocal_.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 termbool 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 notnhLocal_.param("disabled_at_startup", disabledAtStartup_, false);enabled_ = !disabledAtStartup_;// Debugging writes to fileRF_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 posesetPoseSub_ = nh_.subscribe("set_pose",1,&RosFilter<T>::setPoseCallback,this, ros::TransportHints().tcpNoDelay(false));// Create a service for manually setting/resetting posesetPoseSrv_ = nh_.advertiseService("set_pose", &RosFilter<T>::setPoseSrvCallback, this);// Create a service for manually enabling the filterenableFilterSrv_ = nhLocal_.advertiseService("enable", &RosFilter<T>::enableFilterSrvCallback, this);// Create a service for toggling processing new measurements while still publishingtoggleFilterProcessingSrv_ =nhLocal_.advertiseService("toggle", &RosFilter<T>::toggleFilterProcessingCallback, this);// Init the last measurement time so we don't get a huge initial deltafilter_.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 differentiallybool differential;nhLocal_.param(odomTopicName + std::string("_differential"), differential, false);// Determine if we want to integrate this sensor relativelybool 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 thresholddouble poseMahalanobisThresh;nhLocal_.param(odomTopicName + std::string("_pose_rejection_threshold"),poseMahalanobisThresh,std::numeric_limits<double>::max());// Check for twist rejection thresholddouble 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 posetopicInd = 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 relativelybool 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 thresholddouble 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 typestd::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 twisttopicInd = 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 thresholddouble 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 typestd::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 IMUtopicInd = 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 relativelybool 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 thresholddouble poseMahalanobisThresh;nhLocal_.param(imuTopicName + std::string("_pose_rejection_threshold"),poseMahalanobisThresh,std::numeric_limits<double>::max());// Check for angular velocity rejection thresholddouble twistMahalanobisThresh;std::string imuTwistRejectionName =imuTopicName + std::string("_twist_rejection_threshold");nhLocal_.param(imuTwistRejectionName, twistMahalanobisThresh, std::numeric_limits<double>::max());// Check for acceleration rejection thresholddouble 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 settingsstd::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 orientationstd::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 velocitystd::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 soif (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 parametersif (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 subscribersif (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 enabledif (!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 requestedintegrateMeasurements(curTime);}else{// clear out measurements since we're not currently processing new entriesclearMeasurementQueue();// Reset last measurement time so we don't get a large time delta on toggle onif (filter_.getInitializedStatus()){filter_.setLastMeasurementTime(ros::Time::now().toSec());}}// Get latest state and publish itnav_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 Odometryif (!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 transformpositionPub_.publish(filteredPosition);if (printDiagnostics_){freqDiag_->tick();}}// Publish the acceleration if desired and filter is initializedgeometry_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 dataif (smoothLaggedData_){clearExpiredHistory(filter_.getLastMeasurementTime() - historyLength_);}}

2.2.1 Ekf::predict()

R=RzRyRx=(cosθycosθzsinθxsinθycosθz−cosθxsinθzcosθxsinθycosθz+sinθxsinθzcosθysinθzsinθxsinθysinθz+cosθxcosθzcosθxsinθysinθz−sinθxcosθz−sinθysinθxcosθycosθxcosθy)（1）R = R_zR_yR_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） R=RzRyRx=⎝⎛cosθycosθzcosθysinθz−sinθysinθxsinθycosθz−cosθxsinθzsinθxsinθysinθz+cosθxcosθzsinθxcosθycosθxsinθycosθz+sinθxsinθzcosθxsinθysinθz−sinθxcosθzcosθxcosθy⎠⎞（1）
注意，状态向量中（x,y,z,roll,pitch,yawl）（x,y,z,roll,pitch,yawl）（x,y,z,roll,pitch,yawl）是在世界坐标系下，状态向量中（x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,z¨）（\dot{x},\dot{y},\dot{z},\dot{\theta_x},\dot{\theta_y},\dot{\theta_z},\ddot{x},\ddot{y},\ddot{z}）（x˙,y˙,z˙,θx˙,θy˙,θz˙,x¨,y¨,z¨）是在载体坐标系下的，因此在载体坐标系下进行运动估计后，还需要根据式（1）中的坐标转换矩阵将结果转换到世界坐标系下，于是就可以得到代码中的状态转移矩阵：

    //X是在世界坐标系下，X的一阶导和二阶导是在载体坐标系下，因此需要根据k-1时刻的姿态转换到世界坐标系下// Prepare the transfer functiontransferFunction_(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_，在此以∂x∂θx\frac {\partial x}{\partial \theta_x}∂θx∂x为例记性推导：
由非线性状态转移矩阵transferFunction_可知：
xk=xk−1+cosθzcosθyΔtvx+(cosθzsinθysinθx−sinθzcosθx)Δtvy+(cosθzsinθycosθx+sinθzsinθx)Δtvz+12cosθzcosθyΔtΔtax+12(cosθzsinθysinθx−sinθzcosθx)ΔtΔtay+12(cosθzsinθycosθx+sinθzsinθx)ΔtΔtazx_k = x_{k-1} + cos\theta_zcos\theta_y\Delta tv_x + (cos\theta_zsin\theta_ysin\theta_x-sin\theta_zcos\theta_x)\Delta tv_y +\\ (cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)\Delta tv_z + \\ \frac{1}2cos\theta_zcos\theta_y\Delta t\Delta ta_x+\\ \frac{1}2(cos\theta_zsin\theta_ysin\theta_x-sin\theta_zcos\theta_x)\Delta t\Delta ta_y + \\ \frac{1}2 (cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)\Delta t\Delta ta_z xk=xk−1+cosθzcosθyΔtvx+(cosθzsinθysinθx−sinθzcosθx)Δtvy+(cosθzsinθycosθx+sinθzsinθx)Δtvz+21cosθzcosθyΔtΔtax+21(cosθzsinθysinθx−sinθzcosθx)ΔtΔtay+21(cosθzsinθycosθx+sinθzsinθx)ΔtΔtaz
由上式对θx\theta_xθx求偏导可以得到：
∂xk∂θx=(cosθzsinθycosθx+sinθzsinθx)Δtvy+(−cosθzsinθysinθx+sinθzsinθy)Δtvz+12(cosθzsinθycosθx+sinθzsinθx)ΔtΔtay+12(−cosθzsinθysinθx+sinθzsinθyΔtΔtaz=[(cosθzsinθycosθx+sinθzsinθx)Δtvy+(−cosθzsinθysinθx+sinθzsinθy)Δtvz]Δt+[(cosθzsinθycosθx+sinθzsinθx)ay+(−cosθzsinθysinθx+sinθzsinθy)az]12ΔtΔt\frac{\partial x_k}{\partial\theta_x} = (cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)\Delta tv_y +\\ (-cos\theta_zsin\theta_ysin\theta_x + sin\theta_zsin\theta_y)\Delta tv_z+\\ \frac{1}2(cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)\Delta t\Delta ta_y +\\ \frac{1}2(-cos\theta_zsin\theta_ysin\theta_x + sin\theta_zsin\theta_y\Delta t\Delta ta_z\\ \\ =[(cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)\Delta tv_y + (-cos\theta_zsin\theta_ysin\theta_x + sin\theta_zsin\theta_y)\Delta tv_z]\Delta t+\\ [(cos\theta_zsin\theta_ycos\theta_x + sin\theta_zsin\theta_x)a_y + (-cos\theta_zsin\theta_ysin\theta_x + sin\theta_zsin\theta_y)a_z]\frac{1}2\Delta t\Delta t ∂θx∂xk=(cosθzsinθycosθx+sinθzsinθx)Δtvy+(−cosθzsinθysinθx+sinθzsinθy)Δtvz+21(cosθzsinθycosθx+sinθzsinθx)ΔtΔtay+21(−cosθzsinθysinθx+sinθzsinθyΔtΔtaz=[(cosθzsinθycosθx+sinθzsinθx)Δtvy+(−cosθzsinθysinθx+sinθzsinθy)Δtvz]Δt+[(cosθzsinθycosθx+sinθzsinθx)ay+(−cosθzsinθysinθx+sinθzsinθy)az]21ΔtΔt
于是可以得到如下所示的雅克比矩阵FFF：

  // 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 functiontransferFunctionJacobian_ = 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) -> 状态向量： XXX
Eigen::VectorXd measurementSubset(updateSize) -> 观测向量： ZZZ
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); ->测量噪声： RRR
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); -> 测量矩阵：HHH
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); -> 卡尔曼增益：KKK
Eigen::VectorXd innovationSubset(updateSize); -> 观测误差：Z−HXZ-HXZ−HX
transferFunctionJacobian_ -> 线性化后的状态转移矩阵：FFF
于是可以得到：
预测状态向量的状态转移矩阵为：
Xˉk=f(X^k−1,uk)\bar{X}_k = f(\hat{X}_{k-1},u_k) Xˉk=f(X^k−1,uk)
即为：
Xˉk=AX^k−1+uk\bar{X}_k = A\hat{X}_{k-1} + u_k Xˉk=AX^k−1+uk
对应的代码为：

    // (1) Apply control terms, which are actually accelerationsstate_(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_;

预测状态向量的噪声协方差矩阵为：
Pˉk=FP^kFT+Rk\bar P_k = F\hat P_kF^T + R_k Pˉk=FP^kFT+Rk
对应的代码为：

    // (3) Project the error forward: P = J * P * J' + QestimateErrorCovariance_ = (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增益：
Kk=PˉkHTHPˉkHT+Qk\\ K_k = \frac{\bar P_kH^T}{H\bar P_kH^T + Q_k} Kk=HPˉkHT+QkPˉkHT
对应的代码为：

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

然后利用当前观测和预测结果进行校正获得最终融合的结果：
X^k=Xˉk+Kk(Zk−h(Xˉk))\hat X_k = \bar X_k + K_k(Z_k - h(\bar X_k)) X^k=Xˉk+Kk(Zk−h(Xˉk))
对应的代码为：

    innovationSubset = (measurementSubset - stateSubset);// Wrap angles in the innovationfor (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;

并获得最终状态的噪声协方差矩阵：
P^k=(I−KkH)Pˉk(I−KkH)T+KkRKkT\hat P_k =(I - K_kH)\bar P_k(I - K_kH)^T+K_kRK_k^T P^k=(I−KkH)Pˉk(I−KkH)T+KkRKkT
对应的代码为：

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

未完，待续……