Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Covariance initialization issue when covariance is not specified (rolling) #809

Open
daisukes opened this issue May 3, 2023 · 2 comments

Comments

@daisukes
Copy link
Contributor

daisukes commented May 3, 2023

I happened to find this issue with my latest humble image for my project.
ros/rosdistro#37126

My EKF node could not output odometry well with 3.5.0 (rolling), but it works well with 3.3.1 (humble).
I checked the differences between them and found the issue in this commit.
a8b92e9#diff-376c7ff2a0637ccaaa018b18e572c3775d84bde2c782eb4defc27f1359b72a8d

The filter is initialized with default covariance values here.

estimate_error_covariance_.setIdentity();
estimate_error_covariance_ *= 1e-9;
// We need the identity for the update equations
identity_.setIdentity();
// Set the epsilon matrix to be a matrix with small values on the diagonal
// It is used to maintain the positive-definite property of the covariance
covariance_epsilon_.setIdentity();
covariance_epsilon_ *= 0.001;
// Assume 30Hz from sensor data (configurable)
sensor_timeout_ = rclcpp::Duration::from_seconds(0.033333333);
// Initialize our last update and measurement times
last_measurement_time_ = rclcpp::Time(0, 0, RCL_ROS_TIME);
// These can be overridden via the launch parameters,
// but we need default values.
process_noise_covariance_.setZero();
process_noise_covariance_(StateMemberX, StateMemberX) = 0.05;
process_noise_covariance_(StateMemberY, StateMemberY) = 0.05;
process_noise_covariance_(StateMemberZ, StateMemberZ) = 0.06;
process_noise_covariance_(StateMemberRoll, StateMemberRoll) = 0.03;
process_noise_covariance_(StateMemberPitch, StateMemberPitch) = 0.03;
process_noise_covariance_(StateMemberYaw, StateMemberYaw) = 0.06;
process_noise_covariance_(StateMemberVx, StateMemberVx) = 0.025;
process_noise_covariance_(StateMemberVy, StateMemberVy) = 0.025;
process_noise_covariance_(StateMemberVz, StateMemberVz) = 0.04;
process_noise_covariance_(StateMemberVroll, StateMemberVroll) = 0.01;
process_noise_covariance_(StateMemberVpitch, StateMemberVpitch) = 0.01;
process_noise_covariance_(StateMemberVyaw, StateMemberVyaw) = 0.02;
process_noise_covariance_(StateMemberAx, StateMemberAx) = 0.01;
process_noise_covariance_(StateMemberAy, StateMemberAy) = 0.01;
process_noise_covariance_(StateMemberAz, StateMemberAz) = 0.015;

So if the node does not have process_noise_covariance or initial_estimate_covariance, covariance should not be overwritten by zeros.

load_covariance("process_noise_covariance", process_noise_covariance_);
RF_DEBUG("Process noise covariance is:\n" << process_noise_covariance_ << "\n");
filter_.setProcessNoiseCovariance(process_noise_covariance_);
load_covariance("initial_estimate_covariance", initial_estimate_error_covariance_);
RF_DEBUG("Initial estimate covariance is:\n" << initial_estimate_error_covariance_ << "\n");
filter_.setEstimateErrorCovariance(initial_estimate_error_covariance_);

daisukes added a commit to daisukes/robot_localization that referenced this issue May 3, 2023
@dsato80
Copy link

dsato80 commented May 15, 2023

Hello @ayrton04

This is related to your change a8b92e9 .
Could you check my fix #810?
Thank you.

@SteveMacenski
Copy link
Collaborator

@ayrton04 merged it so he should take a look over it, but I will as well today

ayrton04 pushed a commit that referenced this issue Jun 5, 2023
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

3 participants