Using EKF-GSF yaw estimator for ground rover

[lukegluke]

I'm playing with EKFGSF_yaw for ground rover. Logs recorded with dual antenna GPS RTK and now ekf2 replayed with different parameters and modifications using recorded GPS heading only as reference that I expect to achieve.

I've temporary removed "in_air" check so yaw estimator can always works (#902) and removed default 15 m/s airspeed so no corrections to measured accel for centripetal acceleration (in fact it has little effect on result).

Replayed and saw this for estimator status:

On start moving there is convergence - yaw estimator variance get reduced. It will remain lower than 0.01 (in most even lower 0.005) till the end. But individual filters are not converging, instead only one filter gets and remains weight 1.0. Is this a wrong behavior? I've seen a different characteristic in @priseborough video from PX4 Developer Summit Virtual 2020.

Overall result (EKF2_AID_MASK 1) is quite bad: yaw_composite is bad, but yaw from attitude is worse (see third graph for difference with reference heading)

In parallel as one the test I tried to substitute a heading data with course over ground here and replayed with EKF2_AID_MASK 129:

Now yaw is much closer to heading: on the straight movement segments residual with heading is lower that 1°, increasing to about ~5° usually (max up to 10°) on turns.

Of course cog is noisy and bad on small speeds and is jumping on stops, and there will be a problem on moving backward, and of course it could working only for rovers/planes, but at least this test shows that good results could be achieved in some way - results of gps_yaw_fusion on cog better than with EKF-GSF yaw estimator.

It's just a simple first research, I've not so well got into understanding EKF-GSF yaw estimator and will be very appreciated for any thoughts what I could change/correct/try. Big thanks in advance!

[bresch]

Hi @lukegluke ,

We could have a quick look at the data if you can share the replayed log.

[lukegluke]

Hi @bresch , I would like to not share it widely publicly, could I send you a link for ulogs to your email, will it be ok for you? I will send you an original ulog and two replays I pictured: EKF2_AID_MASK 1 and cog instead of heading+EKF2_AID_MASK 129.

Logs were recorded on custom board, project based on px4, closed px4 commit https://github.com/PX4/PX4-Autopilot/commit/16776ff6fbcc7ab21f2dc16db1cf8c756d7abbe9, but due to several modifications (the critical modification in gps orb msg: lat, lon are double in deg, alt - float in m, also has dgps_age field) it would be not so easy to replay elsewhere, but of course I could try any suggestions.

[lukegluke]

I've notice this Paul's https://github.com/PX4/PX4-ECL/pull/831#issuecomment-635165383:

Using the bias too long after takeoff runs the risk of a bad yaw and loss of nav causing the EKF to have a bad gyro bias estimate.

Doesn't this means that EKF-GSF yaw estimator is not suppose to work constantly as the only aiding source of yaw? But I saw that it is positioned not only as fallback, but also for operations without mag.

[bresch]

@lukegluke Quickly looking at your log, I can see that the weights of almost all the yaw estimators are collapsing to 0. Once at 0 they are in a numerical lock and cannot recover. This was fixed in https://github.com/PX4/PX4-ECL/pull/914, you probably don't have it in your branch.

[lukegluke]

You are right, thanks! Data was recorded several month ago so I'm using old branch for replaying. I missed that EKF-GSF yaw estimator had improvements after this. I will try with lattest ecl, hope it will be quite compatible with old px4.

[lukegluke]

I've cherry-picked #914 commits. Now weights are not exactly zero, but still very small 1e-6. Picture is little better, but in general is the same, except in the middle yaw weights is jumped from one filter to the other:

I've sent you the link to the new replayed ulog. Also latter I'm going to record new logs using latest px4 master.

[bresch]

Interesting, the GSF finds out that the estimator # 4 is correct, but the other ones are not converging immediately. If you make a log with master that you can share, we could replay it on our side for debugging.

[lukegluke]

Hi @bresch

It takes a while, but I've recorded new logs with incorporated almost latest px4 master branch. I've sent them to your mail. Hope you find them useful.

[lukegluke]

I've only recently notice that EKF-GSF yaw estimator is used only for yaw alignment (or yaw reset if something wrong). So in my case yaw is aligned and then EKF estimates yaw only by IMU, while EKF-GSF yaw estimator result is quite better due to gps velocity incorporation. I wonder how correctly always use EKF-GSF yaw estimator output as yaw for the main filter?

[bresch]

Yes, the yaw emergency filter is only used for initial alignment and a reset because it has nicer convergence properties (It will converge to any yaw angle while EKF2 will struggle if the current yaw is too far from the truth) but using it continuously as a measurement would severely violate one of the main assumptions done in a Kalman filter: the independence of the measurements. The yaw estimate is not just slightly correlated to some other measurements but created using the same sensors already fused in EKF2.

EKF2 already incorporates gyro, acceleration and GNSS (velocity + position) measurements and is able to constrain and correct the yaw estimate when there is enough acceleration (i.e.: as for the yaw estimator, the state is dynamically observable and when the estimate is close to the truth, EKF2 won't have convergence issues).

If EKF2 isn't converging to the true value while the yaw estimator is, it means that something is wrong.

[lukegluke]

Thanks for taking a look and explanations, I see.

If EKF2 isn't converging to the true value while the yaw estimator is, it means that something is wrong.

This is what I see on my last picture, value from yaw estimator is quite better then yaw in EKF2.

[lukegluke]

value from yaw estimator is quite better then yaw in EKF2.

Especially taking into account that, as I see, yaw estimator uses simple complementary filter for attitude solution and doesn't correct gps velocity relative to IMU using offsets gps_pos_body and imu_pos_body like EKF2 does. And maybe this is the reason, because this offsets are quite huge in my case related to usual drones case?

[lukegluke]

I would like to share another log. Now directly here. https://review.px4.io/plot_app?log=57c2ff2e-11f2-435e-8144-c0ca53c7003c

Log is recorded on a regular car with rtk gps with two antenna heading (EKF2_AID_MASK 129).

To replay this log for ekf-gsf test, original px4 with several modification is needed: https://github.com/lukegluke/PX4-Autopilot/tree/px4_for_ekfgsf_test

I'm playing with ekf replay with different parameters trying to achieve best result for yaw without using heading from 2 antenna gps in filter (EKF2_AID_MASK 1). One of the attempts.

But for now, after alignment EKF2 yaw get diverge from real heading. In parallel yaw estimator composite is also diverge, but latter get back closer to heading (see last picture above. It was for this log in fact). I tried to replay with and without offsets gps_pos_body and imu_pos_body - it didn't influence much on yaw results.

Also here prepared layout for PlotJuggler 2.8.4 for ease of analysis (just correct path to ulg file in previouslyLoaded_Datafiles section) 2021-02-21-19-55-53-894_replayed.zip

[lukegluke]

I've noticed that while working without magnetometer (my case) the controlMagFusion() calls fuseHeading() https://github.com/PX4/PX4-ECL/blob/4df005487379436dcdde10402d3cea7b78d76c19/EKF/mag_control.cpp#L49-L59 and without any additional sources of yaw fuseHeading() fuses the predicted_hdg (possibly with zero innovation) or last static yaw if vehicle at rest.

I commented out controlMagFusion() and got this result: https://review.px4.io/plot_app?log=ccdbb7e1-2807-4fa8-bf2b-0640f6130091 Now yaw from ekf2 get closer to true heading and follows gps more.

I understand that there were reasons and comments say about them (and it does prevent the heading from drifting while in rest), but in motion it seems like constantly fusing prediction heading value behaves like quite a smoothing filter. @bresch could you please be so kind to explain more such behavior.

[bresch]

yaw estimator uses simple complementary filter for attitude solution and doesn't correct gps velocity relative to IMU using offsets gps_pos_body and imu_pos_body like EKF2 does

This isn't true, the measurements are corrected here and then transferred to the yaw estimator here (because controlFusionModes runs before runYawEstimator https://github.com/PX4/PX4-ECL/blob/cd38621dd6a4ef0f42eb29a2b4145f58023e7910/EKF/ekf.cpp#L120-L128)

[lukegluke]

Yes, you are right, I've already found this, but forgot to correct my comment. Sorry for bothering.

[lukegluke]

Interesting, the GSF finds out that the estimator # 4 is correct, but the other ones are not converging immediately.

In fact it choose nearest estimator, but estimator itself doesn't converge to cog. All estimators behave the same - move in parallel being almost evenly spaced as on initiation.

Velocity innovations for all filters are small and looks the same - different yaw is not an obstacle for them. As I see in code model weights are calculated based on gaussian density - higher weight would be for filters with lower innovations, right? But innovations are almost the same, why then all weights get down to zero except one?

I understand my logs are custom and it is not the main case for px4, but I would be very appreciated if somebody (maybe you, @bresch), could give me a direction where to look of source of not converging. Thanks in advanced.

[bresch]

As I see in code model weights are calculated based on gaussian density - higher weight would be for filters with lower innovations, right? But innovations are almost the same, why then all weights get down to zero except one?

The weight is based on the state variance, not the innovation directly. So a filter that converged has more weight in the combined solution ("composite") than a filter with a large uncertainty.

[priseborough]

The bank of EKF's rely on horizontal acceleration to make the yaw observable and the acceleration needs to be at a sufficient magnitude to be distinguishable from acceleration due to INS drift. This works well for a post takeoff flyaway event or during launch or turning flight of planes, but with a rover there may not be enough horizontal acceleration. Also the yaw estimator doesn't like sitting stationary with GPS errors creating apparent acceleration, so for flight vehicles we turn it off when on ground. For rovers addition of a kinematic constraint with respect to lateral slip would be beneficial. In the short term, customising when the filter is enabled based on movement and tuning on replay may be required.

[lukegluke]

Big thanks for the feedbacks!

Yes you right, looking on data I have, it's hard to distinguish starting acceleration. I close this issue for now.