Covariance initialization issue when covariance is not specified (rolling)

[daisukes]

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.

robot_localization/src/filter_base.cpp

Lines 91 to 125 in 284c2ec

	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.

robot_localization/src/ros_filter.cpp

Lines 1819 to 1825 in a8b92e9

	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_);

[dsato80]

Hello @ayrton04

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

[SteveMacenski]

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