Is there any quality / degradation metric associated to the estimate in beluga_acml?

[agalbachicar]

For context, we were thinking of creating a covariance observer component to signal when the EKF fusing sensors and sending an odometry input to beluga_amcl is going bananas.
I wonder if the ROS 2 interface of beluga_amcl has some kind of metric as of know (jazzy branch in 2026-04) that we could hook up to signal the quality of the global pose estimate.

Additionally, wouldn't a time synchronized update together with kinematic constraints deal with jumps or out of bounds estimates?

I think it may be connected to #509

[glpuga]

There is not, for the time being, other than the pose covariance. #509 is in that direction but is pending.

A cheap solution is to think that a healthy filter operates as a balance between the uncertainty that the motion model adds and the uncertainty that the sensor model removes from the particle set. That being true, you can turn that around and assume that the motion model uncertainty and the expected localization performance can both help set a reasonable (if conservative) upper bound for what the covariance of the solution should be after any given update.

Therefore, a rule of thumb can be that if after any given update the cov ellipse is measurably larger along any dimension than the uncertainty "ellipse" of the motion model for that step + your nominal (expected) pose error (i.e the nominal particle spread in healthy conditions), then that may be an early symptom that the filter is diverging.

[agalbachicar]

Thank you @glpuga ! Interesting answer approach!
In case we would like to implement it, where and how it is the recommended place to insert it?

[glpuga]

I think it could be calculated after estimation. After calculating the actual solution covariance you would:

• First, you initialize a disposable particle set with the distribution you expect of a well behaved solution. That is, with the standard distributions in x, y, and yaw you're going for. We have no paramters for these values in beluga, so they would have to be added. Also notice we only care about the distribution, not the central value, so you can set the mean values to zero.
• Then you run that particle set it through the motion model with the same parameters we used for the propagation of the main filter. This way you simulate what the proposal distribution would look like if the filterd had started from a healthy state.
• Finally, you calculate the covariance of this simulation and compare the diagonals against those of the covariance of the main filter.

I think all of this can be done with the existing code in the repo.

[agalbachicar]

Thanks a ton!
Finally would a ticket in the repository with a design proposal expanding on your explanation would be enough to implement it?

[hidmic]

Go for it

[glpuga]

yes, but I think it's important we also come up with a criteria to evaluate if this is any good, either with hand labeled data or with a way to come up with synthetic cases that we know should trigger this.

[glpuga]

It's worth adding that theoretically, since Beluga/Nav2 use KLD sampling, you can also use the particle count as a measure of the particle spread. If you're near the top of the allowed range of particles, the filter is probably struggling to converge. This number is not published per-se, but it's available as the vector length in the message that carries the whole particle set for plotting in rviz. The number is also regularly logged to the terminal.

Sadly this criteria is rather soft, and does not provide clear thresholds. To some extent depends on the parameter values you give for the lower and limit particle counts. You can probably use it to know which areas of the map are better and which ones are worse, though.

[hidmic]

I'd stick to posterior distribution analysis (covariance or else). Relying on KLD sampling to correlate particle count with particle spread seems brittle to me.

[hidmic]

Related to this #563.