Issue with heading estimation

[FrankvVeelen]

We are developing a drone with the PX4 firmware, running on a holybro V5x FCU.

We have run into an issue which we can't understand within the team, so I am hoping for someone to help us. The issue has happened in multiple flights, but can be easiest seen in the following flight log: log

The heading estimate is very unstable (a jump of >90 degrees at the 1:12 mark), after which the drone is almost uncontrollable, since all orientations are messed up. This has started happening repeatedly, but we have also made more than 10 flights before where the issue did not arise.

I have noticed that the EKFs are also using the internal magnetometer, while it is set at a lower priority and the external magnetometer does produce data.

If anyone can assist (even if only giving some pointers on how to analyse this issue) that'd be greatly appreciated.

[dagar]

Was setting EKF2_MULTI_MAG 1 intentional? Multi-EKF is enabled across mags, but it's only going to use the first mag instance, which happens to be the internal. If you want to only use a single mag then I would re-enable the sensors level selection (by priority) by setting SENS_MAG_MODE 1.

Your first mag (internal) is quite bad.

[FrankvVeelen]

Setting EKF2_MULTI_MAG 1 was indeed not intentional and I will definitely fix this, but I don't think that's fully responsible for the problem.

I also have another flight log where navigation issues arose, which to the best of my analyzing abilities were because of the heading estimate instantly moving 90 degrees.

The compass in that log is also quite correlated with the thrust, which is why we tried doing the power compensation for the magnetometer (incorporated in the later log in which the wrong compass was used), but it was therefor not tested yet.

I will try to run a test with the correct magnetometer with power compensation.

[dagar]

@FrankvVeelen an easy thing to do for your next test is simply run multi-EKF across both mags by setting EKF2_MULTI_MAG 2 and SENS_MAG_MODE 0. Then in the actual flight (with good GPS) make sure there's a lot of lateral movement and yawing. In the ulg we'll then be able to see the estimated biases for each magnetometer.

[FrankvVeelen]

Hello again,

We have run some more tests. At first we tried just lots of moving and yawing, with EKF2_MULTI_MAG set to 0, this was recorded into flight log: log. Everything seemed fine in this log, with no heading jumps and the heading in QGC being consistent with the real life heading.

Therefor we went on to fly a survey mission in a grid pattern, which was recorded in flight log: log. As can be seen, issues with the heading resurfaced. What we noticed that at almost every corner taken is that the drone first yawed the wrong direction ~10 degrees, after which it "realized" the issue and started yawing the correct way. Another interesting thing we noticed in the log is that the drone was continuously yawing one way. We also noticed that the EKFs were continuously switching between the 2 magnetometers, presumably because they noticed issues in the primary magnetometer? After the flight the magnetometer calibration got messed up somehow and the heading shown in QGC was not consistent with the actual heading. Furthermore the drone rejected arming because of inconsistent magnetometer measurements.

After these tests the weather was not calm enough to still perform the test you suggested, but if you still think it would be helpful we'll try to run the test later this week.

EDIT: I've just noticed that the wrong magnetometer was used again in this flight, most likely due to us plugging the magnetometer in after booting the drone. Should this be handled or is it expected behaviour to not use the magnetometer if it was removed and reconnected while online? Is there an option to disallow arming if a certain magnetometer is not present?

[FrankvVeelen]

Hi,

The issues are still going on. After more testing we've found out that the issues seem to be related to the reset of the magnetometer after taking off. When we stay below 1.5m with the drone the heading stays stable and correct. Then as soon as we go to 1.5m there is a noticable change in motor sound and a jump in the attitude estimation.

At first we thought this'd be related to changing to 3D magnetometer fusion, but even disabling this (always using heading fusion) does not fix the issues. It now seems to us that it is related to the runInAirYawReset().

We know that the GPS position is way more reliable then the compass heading estimation, are there parameters we can tune to change the confidence, such that GPS position derivatives are used for heading estimation, with magnetometer heading estimation only being used if there is no movement?

[dagar]

@FrankvVeelen can you share that log?

[FrankvVeelen]

• log 1
• log 2 (During the stabilized mode flight we held the drone on the ground to test if the heading had issues from motor interference)
• log 3 (at the end of the log we walked with the drone to test some unrelated communication things)

[slgrobotics]

I am having the same problem on a ground vehicle (gas powered lawnmower) - something that hasn't been happening half a year ago. I have an 9250 IMU which I run on SPI with no magnetometer, and a separate RM3100 magnetometer on I2C. It runs on Raspberry Pi 3 under Ubuntu Server 20.04, I am using emlid_navio2 build. In my experiments the EKF2 heading abruptly changes 90 degrees and stays there. It happens only when the robot turns quickly (at its max turn rate) - either manually (R/C) or on a mission (e.g. following a zig-zag pattern). The heading resets to normal if I stop and restart ekf2, or just restart px4. To reproduce, try high-rate turns for 90 to 180 degrees - I believe any platform will do..

[ryanjAA]

@slgrobotics upload a log

[slgrobotics]

Here's a log with EKF2 heading jump at mark 480 sec

log11.ulg.zip

[slgrobotics]

BTW, I am using an RTK GPS, so when the vehicle is moving - GPS heading over ground is very precise.

[slgrobotics]

I tried the same in SIM (I5 desktop, Ubuntu 20.04), no matter how much I turn - the heading stays good.

[bresch]

For some reason I cannot see any file in the zip. Please share logs using http://logs.px4.io

[slgrobotics]

My bad, sorry. Here are two logs, log11 is as described above, log12 shows similar heading jump. Let me know if you need different data - what would be the SDLOG_PROFILE you want (I have it set to decimal 123). log11.ulg.zip log12.ulg.zip

[slgrobotics]

I tweaked EKF2_* parameters to disable fusing magnetometer data (as well as Ranger, EV, Barometer). The QGroundControl shows yaw 0 when px4 starts, and stays until the vehicle moves enough for the GPS east and north velocities to show up. Then yaw jumps without a good reason - and stays wrong. Here is the log: https://logs.px4.io/plot_app?log=3fd0d7cd-6027-480b-bb95-e8dcefdd3865 I drove the vehicle via manual control - it starts in the NNE direction, and moves in a loop. The "Yaw Angle" clearly shows a 90 degree jump at 193:55:24. Meanwhile GPS velocities stay steady as the screenshot shows:

[ryanjAA]

I'd try a different compass - it's rare but I've seen "flipped" mags that after a certain amount of time or exposure jump to inverse or an offset but present as normal when first powered on.

Swap it with anything you can find or have on hand to test.

[slgrobotics]

@ryanjAA : Fair enough - but didn't I actually disable compass by setting "param set EKF2_MAG_TYPE 5"? Before I did that - px4 started and immediately showed compass heading in QGroundControl, now it shows heading 0 until there's enough movement to produce valid GPS E and N velocity components.

EKF2 estimator: only consider heading from magnetometer (1) or use all three axisi (2): 0: Automatic (heading on ground, 3D in-flight) 1: Magnetic heading (2D) 2: 3-axis (3D) 5: none (i.e. use GPS instead, disable/ignore magnetometer) param set EKF2_MAG_TYPE 5

Let me reiterate the following question from [FrankvVeelen - commented on Jul 25]: "We know that the GPS position is way more reliable then the compass heading estimation, are there parameters we can tune to change the confidence, such that GPS position derivatives are used for heading estimation, with magnetometer heading estimation only being used if there is no movement?"

I am pretty sure that there's something in the EKF2 code that rotates GPS velocities in a wrong way, and that results in persistent error of 90 degrees, once all settles. With RTK GPS delivering reliable signal (NE velocities always available and true) - this effect shows up in my case.

I am tempted to make a stripped version of EKF2 code to just trust GPS at all times. No baro, EV, IMU. Maybe, no Kalman filter. This would be valuable for agricultural vehicles, having RTK and traveling on flat land. I'd use local_position_estimator, but it doesn't seem to work (does it?).

[slgrobotics]

I did more experimenting to expose the bug.

I have two platforms - a real gas powered lawnmower and a smaller robot (see the photo). Both run latest PX4 main software on Raspberry Pi 3B's, and I update/merge my fork with PX4 main every time I run them. Modifications in my branch are minimal - to support MCP3008 ADC and R/C receiver. Both robots have MPU9250 connected over SPI, I don't enable their magnetometer (no -M on startup). They differ with GPS and compass setup:

• Lawnmower has RM3100 magnetometer on I2C and U-Blox ZED-F9P RTK GPS from SparkFun on USB Serial. I do have another GPS set as a base station in the same location, and precision is solid around 2cm.
• Smaller robot ("Dragger") has BN-880 module - containing a U-Blox M8N GPS on USB Serial and hmc5883 magnetometer on I2C.

My goal is to achieve high precision movement of the lawnmower, both in turns and on straight runs - necessary to lay stripes on the lawn. I might use Dragger later for some vision-enabled navigation etc. It is obvious that exact heading (within 3-5 degrees) is essential for at least the lawnmower operation - so a bug-free EKF2 is important to me.

The EKF2 heading bug shows up on both platforms, but for this thread I am experimenting with Dragger, as it is easier for others to reproduce.

For the experiments I disabled magnetometer fusion by setting EKF2_MAG_TYPE to 5 (none), leaving GPS N-E velocity data to be fused into the heading (EKF2_GPS_CTRL=7). Then I drove Dragger in rectangular counter clock wise and clock wise patterns (around 8 meters between turns). I then re-enabled magnetometer (EKF2_MAG_TYPE=1) and disabled GPS 3D Velocity fusion (EKF2_GPS_CTRL=3). Each time the robot was facing true north on log start (when armed) and on finish (when disarmed). Plots show Yaw Angle offset at the end.

With GPS-only fusion the heading indicator on QGroundControl very quickly deviates from the true orientation of the robot (which I observe visually) - usually up to 50-90 degrees. Then it keeps that offset no matter how the robot moves around. The effect is more pronounced on the lawnmower with its RTK GPS, but is very easily reproduced on Dragger.

With compass-only fusion everytyhing works fine, especially on Dragger, where there is little magnetic mass around the compass, and the magnetometer is very precise and stable. Lawnmower has a bit more magnetic interference, but overall its magnetometer works well too.

Here are the logs.

• GPS-only fusion:

https://logs.px4.io/plot_app?log=1ecfc401-8a41-421f-937b-221b705f9e6c (Counter-clockwise 03-28-43.ulg) https://logs.px4.io/plot_app?log=a94123ee-5f0c-44bf-aa05-00af5f16aae0 (Clockwise 03-32-22.ulg)

• Magnetometer-only fusion:

https://logs.px4.io/plot_app?log=56979ada-8a7d-437c-865b-552940e8c56e (Counter-clockwise 03-44-20.ulg) https://logs.px4.io/plot_app?log=2c210d24-151d-4db9-94de-93400ddede74 (Clockwise 03-49-05.ulg)

[bresch]

@slgrobotics I had a look at the logs with and without mag.

It seems that the selected autopilot orientation is wrong:

• when in mag type "none" you say that there is a 50-90 degrees error in heading. From the log, the fusion works properly; the estimated heading is correct
• when using the mag, you say that the heading is correct. However I can see large measurement innovations: the heading gets pulled on one side by the GPS data during turns and by the mag on the other side during straight lines.

Altogether, this indicates that the rotation of the external mag is correct but that the rotation of the autopilot is incorrectly set. Do you have a picture showing how the autopilot is installed? You can then set SENS_BOARD_ROT to the appropriate value.

[slgrobotics]

@bresch - Thanks for responding. I am not sure why in the "Magnetometer-only fusion" log you concluded that "the heading gets pulled on one side by the GPS data during turns" - as GPS velocity data should be specifically excluded from fusing by setting EKF2_GPS_CTRL=3. Maybe I misinterpret that parameter's role or how EKF2 works. As Dragger was positioned facing true north at the beginning and at the end of the run, I can see no Yaw Angle offset in this case. But the offset is obvious in case of "GPS-only fusion" log.

As for orientations, details below.

My autopilot is a Raspberry Pi 3B with mpu9250 (accelerometer, gyro) and compass boards independently wired - and rotated as I find fit. A single-antenna GPS does not have a rotation, of course - it can be placed at the center of a robot in any direction.

So, I can set all my sensor and board rotations to 0 in parameters (ROT=0) and then manipulate sensors to match that setting - place them in relation to robot's body. Note that "board rotation" SENS_BOARD_ROT is essentially an IMU rotation - it doesn't matter how the RPi is oriented.

I run mpu9250 with "-R 0" and without "-M", like this: "mpu9250 -s -R0 -f 20000 start". Then, on setup, I tried different orientations until I see proper picture in QGroundControl artificial horizon (attitude indicator, AI) - I tilt the robot forward and see the green "earth" go up, in left bank - left side of the earth goes up - basically, all robot roll and pitch combinations must match the picture below (assuming the Western "aircraft view" style AI, not the reversed Russian "ground view" style). At that point I assume the MPU9250 board is rotated properly and I can attach it to robot's body in that orientation. Then I gently try the same with magnetometer attached to IMU, maybe restarting ekf2. I quickly find mag position corresponding to Earth's magnetic field.

I did repeat all the above with my robots, and discovered that the lawnmower indeed had its sensors rotated (thanks for the tip!). I corrected that. My sketch of proper orientations for MPU9250, RM3100 and BN-880 is attached.

With RM3100 magnetometer and ZED-F9P GPS (fix 6, 2cm accuracy) - here is lawnmower's new log running a mission. It performs OK, although L1 and other parameters need tuning:

https://logs.px4.io/plot_app?log=c7f20e13-bb1e-477a-a614-9620a0f84aeb (Lawnmower Mission loop, 23_23_24.ulg)

At this point I feel that there's a significant magnetic interference on the lawnmower, that can be dealt with only by introducing some kind of deviation table. The magnetometer heading is reliable, but somewhat offset in certain directions. I might either relocate the RM3100 even farther from the gas engine, or/and introduce a deviation table in the VehicleMagnetometer module. I think that BN-880 on the Dragger performs very well, and can be relied upon. The GPS-only offset in the second Dragger set still makes me think there's something unexplained in the way EKF2 fuses GPS velocities.

Anyway, thank you again for your help, it definitely explained some of my mistakes and pointed me in the right direction. I hope your team will have an opportunity to test EKF some more on a ground vehicle striving for precision.

[bresch]

I am not sure why in the "Magnetometer-only fusion" log you concluded that "the heading gets pulled on one side by the GPS data during turns" - as GPS velocity data should be specifically excluded from fusing by setting EKF2_GPS_CTRL=3.

The GPS position is weaker than the velocity fusion but is enough to make the heading observable during turns.

It performs OK, although L1 and other parameters need tuning

Good, yes the EKF2 output looks consistent now.

Anyway, thank you again for your help, it definitely explained some of my mistakes and pointed me in the right direction. I hope your team will have an opportunity to test EKF some more on a ground vehicle striving for precision.

I'm glad you're unblocked now! Unfortunately we don't have many rover testers, so it would be great if you can continue to share your experience as your project goes on.

Actually, for rovers we could add a sideslip constraint in order to make the heading observable at a constant velocity (currently, acceleration is needed). I could make a prototype of this when I have a bit of time if you can then do some testing.

[slgrobotics]

@bresch - Thanks for explanation, it helps.

I will be happy to test whatever needs testing on my rovers, feel free to ask. There's a lot of potential in agricultural robotics, security and delivery - and your framework is very well suited to these fields.

Meanwhile I opened https://github.com/PX4/PX4-Autopilot/issues/20791 - which might be related to this discussion.