概述
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
¨
)
T
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=（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);//实例化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 << ")nt" <<
odomTopicName << "_differential is " << (differential ? "true" : "false") << "nt" <<
odomTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "nt" <<
odomTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "nt" <<
odomTopicName << "_queue_size is " << odomQueueSize << "nt" <<
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 << ")nt" <<
poseTopicName << "_differential is " << (differential ? "true" : "false") << "nt" <<
poseTopicName << "_rejection_threshold is " << poseMahalanobisThresh << "nt" <<
poseTopicName << "_queue_size is " << poseQueueSize << "nt" <<
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 << ")nt" <<
twistTopicName << "_rejection_threshold is " << twistMahalanobisThresh << "nt" <<
twistTopicName << "_queue_size is " << twistQueueSize << "nt" <<
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 << ")nt" <<
imuTopicName << "_differential is " << (differential ? "true" : "false") << "nt" <<
imuTopicName << "_pose_rejection_threshold is " << poseMahalanobisThresh << "nt" <<
imuTopicName << "_twist_rejection_threshold is " << twistMahalanobisThresh << "nt" <<
imuTopicName << "_linear_acceleration_rejection_threshold is " << accelMahalanobisThresh << "nt" <<
imuTopicName << "_remove_gravitational_acceleration is " <<
(removeGravAcc ? "true" : "false") << "nt" <<
imuTopicName << "_queue_size is " << imuQueueSize << "nt" <<
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()
R
=
R
z
R
y
R
x
=
(
c
o
s
θ
y
c
o
s
θ
z
s
i
n
θ
x
s
i
n
θ
y
c
o
s
θ
z
−
c
o
s
θ
x
s
i
n
θ
z
c
o
s
θ
x
s
i
n
θ
y
c
o
s
θ
z
+
s
i
n
θ
x
s
i
n
θ
z
c
o
s
θ
y
s
i
n
θ
z
s
i
n
θ
x
s
i
n
θ
y
s
i
n
θ
z
+
c
o
s
θ
x
c
o
s
θ
z
c
o
s
θ
x
s
i
n
θ
y
s
i
n
θ
z
−
s
i
n
θ
x
c
o
s
θ
z
−
s
i
n
θ
y
s
i
n
θ
x
c
o
s
θ
y
c
o
s
θ
x
c
o
s
θ
y
)
（
1
）
R = R_zR_yR_x \= left( begin{array}{ccc} costheta_y costheta_z & sintheta_x sintheta_y costheta_z - costheta_x sintheta_z& costheta_x sintheta_y costheta_z + sintheta_x sintheta_z\ costheta_y sintheta_z & sintheta_x sintheta_y sintheta_z + costheta_x costheta_z& costheta_x sintheta_y sintheta_z - sintheta_x costheta_z\ -sintheta_y &sintheta_x costheta_y &costheta_x costheta_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
,
r
o
l
l
,
p
i
t
c
h
,
y
a
w
l
）
（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 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_，在此以
∂
x
∂
θ
x
frac {partial x}{partial theta_x}
∂θx∂x为例记性推导：
由非线性状态转移矩阵transferFunction_可知：
x
k
=
x
k
−
1
+
c
o
s
θ
z
c
o
s
θ
y
Δ
t
v
x
+
(
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
−
s
i
n
θ
z
c
o
s
θ
x
)
Δ
t
v
y
+
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
Δ
t
v
z
+
1
2
c
o
s
θ
z
c
o
s
θ
y
Δ
t
Δ
t
a
x
+
1
2
(
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
−
s
i
n
θ
z
c
o
s
θ
x
)
Δ
t
Δ
t
a
y
+
1
2
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
Δ
t
Δ
t
a
z
x_k = x_{k-1} + costheta_zcostheta_yDelta tv_x + (costheta_zsintheta_ysintheta_x-sintheta_zcostheta_x)Delta tv_y +\ (costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)Delta tv_z + \ frac{1}2costheta_zcostheta_yDelta tDelta ta_x+\ frac{1}2(costheta_zsintheta_ysintheta_x-sintheta_zcostheta_x)Delta tDelta ta_y + \ frac{1}2 (costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)Delta tDelta 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求偏导可以得到：
∂
x
k
∂
θ
x
=
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
Δ
t
v
y
+
(
−
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
+
s
i
n
θ
z
s
i
n
θ
y
)
Δ
t
v
z
+
1
2
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
Δ
t
Δ
t
a
y
+
1
2
(
−
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
+
s
i
n
θ
z
s
i
n
θ
y
Δ
t
Δ
t
a
z
=
[
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
Δ
t
v
y
+
(
−
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
+
s
i
n
θ
z
s
i
n
θ
y
)
Δ
t
v
z
]
Δ
t
+
[
(
c
o
s
θ
z
s
i
n
θ
y
c
o
s
θ
x
+
s
i
n
θ
z
s
i
n
θ
x
)
a
y
+
(
−
c
o
s
θ
z
s
i
n
θ
y
s
i
n
θ
x
+
s
i
n
θ
z
s
i
n
θ
y
)
a
z
]
1
2
Δ
t
Δ
t
frac{partial x_k}{partialtheta_x} = (costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)Delta tv_y +\ (-costheta_zsintheta_ysintheta_x + sintheta_zsintheta_y)Delta tv_z+\ frac{1}2(costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)Delta tDelta ta_y +\ frac{1}2(-costheta_zsintheta_ysintheta_x + sintheta_zsintheta_yDelta tDelta ta_z\ \ =[(costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)Delta tv_y + (-costheta_zsintheta_ysintheta_x + sintheta_zsintheta_y)Delta tv_z]Delta t+\ [(costheta_zsintheta_ycostheta_x + sintheta_zsintheta_x)a_y + (-costheta_zsintheta_ysintheta_x + sintheta_zsintheta_y)a_z]frac{1}2Delta tDelta 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
于是可以得到如下所示的雅克比矩阵
F
F
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
X
X
Eigen::VectorXd measurementSubset(updateSize) -> 观测向量：
Z
Z
Z
Eigen::MatrixXd measurementCovarianceSubset(updateSize, updateSize); ->测量噪声：
R
R
R
Eigen::MatrixXd stateToMeasurementSubset(updateSize, state_.rows()); -> 测量矩阵：
H
H
H
Eigen::MatrixXd kalmanGainSubset(state_.rows(), updateSize); -> 卡尔曼增益：
K
K
K
Eigen::VectorXd innovationSubset(updateSize); -> 观测误差：
Z
−
H
X
Z-HX
Z−HX
transferFunctionJacobian_ -> 线性化后的状态转移矩阵：
F
F
F
于是可以得到：
预测状态向量的状态转移矩阵为：
X
ˉ
k
=
f
(
X
^
k
−
1
,
u
k
)
bar{X}_k = f(hat{X}_{k-1},u_k)
Xˉk=f(X^k−1,uk)
即为：
X
ˉ
k
=
A
X
^
k
−
1
+
u
k
bar{X}_k = Ahat{X}_{k-1} + u_k
Xˉk=AX^k−1+uk
对应的代码为：
// (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_;
预测状态向量的噪声协方差矩阵为：
P
ˉ
k
=
F
P
^
k
F
T
+
R
k
bar P_k = Fhat P_kF^T + R_k
Pˉk=FP^kFT+Rk
对应的代码为：
// (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_ <<
"nn--------------------- /Ekf::predict ----------------------n");
2.2.2 Ekf::correct()
在校正过程中，首先计算Kalman增益：
K
k
=
P
ˉ
k
H
T
H
P
ˉ
k
H
T
+
Q
k
\ K_k = frac{bar P_kH^T}{Hbar 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
+
K
k
(
Z
k
−
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 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;
并获得最终状态的噪声协方差矩阵：
P
^
k
=
(
I
−
K
k
H
)
P
ˉ
k
(
I
−
K
k
H
)
T
+
K
k
R
K
k
T
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();
未完，待续……
最后
以上就是疯狂心锁为你收集整理的robot_localization 源码解析（1）ekf_localization_node的全部内容，希望文章能够帮你解决robot_localization 源码解析（1）ekf_localization_node所遇到的程序开发问题。
如果觉得靠谱客网站的内容还不错，欢迎将靠谱客网站推荐给程序员好友。
发表评论 取消回复