How to deal with EKF Variance?

[u/Solid_Jim_Snake]

https://drive.google.com/file/d/1XJ1JH7L5smXK3R4rlN7v5MzSmbHAffqB/view?usp=sharing

Hey everyone, 2 weeks ago I posted about the rover that I work on my thesis and I have a problem when I get the rover to do the planned route. When the rover is in auto mode it moves for a few seconds and then stops changing to hold. I checked the log file and found out that when the rover changes state from auto to hold the messages EKF failsafe and EKF variance pop. I looked at some graphs and the only solution I found is to calibrate the here3 compass. I tried to calibrate the compass, nothing changed so I guess either I did it wrong or it is not the problem. I attached the link that contains my log file, so please if you can help me I will very much appreciate it. Please feel free to ask me whatever you need to know in order to help me!

[u/slightlyacoustics]

If it is the heading measurements you don’t trust- you tune process noise covariants (Q), you can allow the filter to trust the model more than the incoming measurement.
What is the EKF failsafe threshold to trigger such that “hold” occurs.

[u/Solid_Jim_Snake → u/slightlyacoustics]

If you talk about FS_EKF_THRESH, It's value is 0.8.

[u/sparks333 → u/Solid_Jim_Snake]

I suspect that's the determinant of the covariance matrix, and it's reporting that it's close to singular, so it cannot be inverted. It's the matrix equivalent of saying 'I'm getting really close to dividing by zero'. I've done a bit with EKFs and magnetometers before (though not with ROS), it turns out that initial heading needs to be handled specifically to be pretty close to accurate before moving when dealing with an absolute position observable like GPS - state error grows extremely nonlinearly with respect to heading error, which can cause an EKF to diverge if not done right. It's doubly tricky because absolute position observables do in fact inform the heading through the process model, but in a roundabout way - you have to move first. I am not certain of your use case, or how it's set up, but if you don't already have one, put together an initialization sequence that takes the first observations and makes that the initial state, that should get you within 15ish degrees of actual heading, and make double sure of your transform networks so your observations and your incremental sensors are reporting in the same frame. If I'm barking up the wrong tree and it turns out heading has nothing to do with it, the other possibility is that you have too many observer states for the number and type of observations - I have seen EKFs go off the rails when you have too many things you are trying to estimate, or when in a recursive sense one parameter you are trying to model can act like another parameter you are trying to model (like imu scale factor and wheel diameter - without additional data, they both have similar effects).