// constructor
odom_estimation::odom_estimation():
_prior(NULL),
_filter(NULL),
_filter_initialized(false),
_odom_initialized(false),
_imu_initialized(false),
_vo_initialized(false)
{
// create SYSTEM MODEL
ColumnVector sysNoise_Mu(6); sysNoise_Mu = 0;
SymmetricMatrix sysNoise_Cov(6); sysNoise_Cov = 0;
sysNoise_Cov(1,1) = pow(1000.0,2);
sysNoise_Cov(2,2) = pow(1000.0,2);
sysNoise_Cov(3,3) = pow(1000.0,2);
sysNoise_Cov(4,4) = pow(1000.0,2);
sysNoise_Cov(5,5) = pow(1000.0,2);
sysNoise_Cov(6,6) = pow(1000.0,2);
Gaussian system_Uncertainty(sysNoise_Mu, sysNoise_Cov);
_sys_pdf = new NonLinearAnalyticConditionalGaussianOdo(system_Uncertainty);
_sys_model = new AnalyticSystemModelGaussianUncertainty(_sys_pdf);
// create MEASUREMENT MODEL ODOM
ColumnVector measNoiseOdom_Mu(3); measNoiseOdom_Mu = 0;
SymmetricMatrix measNoiseOdom_Cov(3); measNoiseOdom_Cov = 0;
measNoiseOdom_Cov(1,1) = pow(0.001,2);
measNoiseOdom_Cov(2,2) = pow(0.001,2);
measNoiseOdom_Cov(3,3) = pow(0.01,2);
Gaussian measurement_Uncertainty_Odom(measNoiseOdom_Mu, measNoiseOdom_Cov);
Matrix Hodom(3,6); Hodom = 0;
Hodom(1,1) = 1; Hodom(2,2) = 1; Hodom(3,6) = 1;
_odom_meas_pdf = new LinearAnalyticConditionalGaussian(Hodom, measurement_Uncertainty_Odom);
_odom_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_odom_meas_pdf);
// create MEASUREMENT MODEL IMU
ColumnVector measNoiseImu_Mu(3); measNoiseImu_Mu = 0;
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.001,2);
measNoiseImu_Cov(2,2) = pow(0.001,2);
measNoiseImu_Cov(3,3) = pow(0.001,2);
Gaussian measurement_Uncertainty_Imu(measNoiseImu_Mu, measNoiseImu_Cov);
Matrix Himu(3,6); Himu = 0;
Himu(1,4) = 1; Himu(2,5) = 1; Himu(3,6) = 1;
_imu_meas_pdf = new LinearAnalyticConditionalGaussian(Himu, measurement_Uncertainty_Imu);
_imu_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_imu_meas_pdf);
// create MEASUREMENT MODEL VO
ColumnVector measNoiseVo_Mu(3); measNoiseVo_Mu = 0;
SymmetricMatrix measNoiseVo_Cov(3); measNoiseVo_Cov = 0;
measNoiseVo_Cov(1,1) = pow(0.001,2);
measNoiseVo_Cov(2,2) = pow(0.001,2);
measNoiseVo_Cov(3,3) = pow(0.001,2);
Gaussian measurement_Uncertainty_Vo(measNoiseVo_Mu, measNoiseVo_Cov);
Matrix Hvo(3,6); Hvo = 0;
Hvo(1,4) = 1; Hvo(2,5) = 1; Hvo(3,6) = 1;
_vo_meas_pdf = new LinearAnalyticConditionalGaussian(Hvo, measurement_Uncertainty_Vo);
_vo_meas_model = new LinearAnalyticMeasurementModelGaussianUncertainty(_vo_meas_pdf);
};
void MatFromIMU::imuCallback(const ImuConstPtr& imu)
{
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
//orientation_covariance = imu->orientation_covariance;
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
/*/ Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
ROS_ERROR("Could not transform imu message from %s to %s", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
ROS_DEBUG("Could not transform imu message from %s to %s. Imu will not be activated yet.", imu->header.frame_id.c_str(), base_footprint_frame_.c_str());
return;
}
tf::StampedTransform base_imu_offset;
robot_state_.lookupTransform(base_footprint_frame_, imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
*/
// imu_time_ = ros::Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
MatrixWrapper::SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
robot_broadcaster_.sendTransform(tf::StampedTransform(imu_meas_.inverse(), imu_stamp_, base_footprint_frame_, "imuTF"));
};
// callback function for imu data
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_callback_counter_++;
assert(imu_used_);
// receive data
imu_stamp_ = imu->header.stamp;
tf::Quaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = tf::Transform(orientation, tf::Vector3(0,0,0));
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
// Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform("base_footprint", imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
// warn when imu was already activated, not when imu is not active yet
if (imu_active_)
ROS_ERROR("Could not transform imu message from %s to base_footprint", imu->header.frame_id.c_str());
else if (my_filter_.isInitialized())
ROS_WARN("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
else
ROS_DEBUG("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
return;
}
StampedTransform base_imu_offset;
robot_state_.lookupTransform("base_footprint", imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_);
// activate imu
if (!imu_active_) {
if (!imu_initializing_){
imu_initializing_ = true;
imu_init_stamp_ = imu_stamp_;
ROS_INFO("Initializing Imu sensor");
}
if ( filter_stamp_ >= imu_init_stamp_){
imu_active_ = true;
imu_initializing_ = false;
ROS_INFO("Imu sensor activated");
}
else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.",
(imu_init_stamp_ - filter_stamp_).toSec());
}
if (debug_){
// write to file
double tmp, yaw;
imu_meas_.getBasis().getEulerYPR(yaw, tmp, tmp);
imu_file_ << yaw << endl;
}
};