EKF, MMC5883, AK09915 Coherence

[sietse]

Hey Folks!

I’m having some issues with the compasses on our ROV. Everything seemed to work fine with our Waterlinked A50, but after testing outside our tank, we discovered that our map position wasn’t quite similar to the actual ROV position.

We’ve tried everything with the DVL, including using the dead-reckoning functionality and calibrating its IMU/gyro, but the map position still appears unrealistic. Position Hold worked flawlessly, so I don’t believe drift is causing the issue.

If I understand correctly: the map position is determined by the DVL’s velocity/speed in combination with the ROV’s heading, right? Since Position Hold is working seamlessly, I’m starting to doubt the accuracy of the compass. To investigate, I checked the Compass data in the Vehicle Setup tab in BlueOS (all compasses had a green smiling emoji status).

In the Compass tab, there are three vectors displayed: the MMC5883 (Navigator), the AK09915 (Navigator), and the EKF. With previous calibrations (done inside a building), the AK09915 was slightly off, which makes sense as it’s a magnetometer that can be affected by large buildings and steel structures. After calibrating the compasses outdoors, the AK09915 and MMC5883 worked together much better! (We calibrated the compasses in both BlueOS and QGC.)

Now my question is: what exactly is the EKF? Is it a calculated output from the MMC5883 and AK09915 values? While operating in open water, we received some “Bad EKF” messages in QGC. We also saw messages indicating that the ROV was switching between Compass 0 and Compass 1. When these messages appear, is the ROV switching between the MMC5883 and the AK09915 as its primary compass source? And if so, how does it decide when one of the compasses is providing faulty data?

I know these are a lot of questions haha! I’m excited to hear from you!

Cheers!

~ Sietse

[tony-white]

Hi @sietse -
You may want to calibrate your gyro as well - from BlueOS Vehicle setup, you should have the vehicle stationary (on land) and once calibrated, the offsets should all be below ~10 to 15…

The EKF is the Extended Kalman filter that combines the sensor’s data to provide an estimate of orientation - it is calculated with all healthy sensors. Making sure you don’t run any additional power wires near the Navigator, and calibrate your system well is key!

[sietse]

Hey @tony-white!
Thank you so much! I tried it out and the offsets are now -4.70, -3.70, 6.60 after calibration. Before calibration they were -10.60, -25.90, 6.20.

@tony-white

Making sure you don’t run any additional power wires near the Navigator, and calibrate your system well is key!

This might be something to check for us, since we are running T500’s in our setup. We limited our system to draw a 90A current max.

@sietse

If I understand correctly: the map position is determined by the DVL’s velocity/speed in combination with the ROV’s heading, right?

Is this something you could confirm? Would the compass and gyro calibration within BlueOS be enough to get rid of the ‘Bad EKF’ Messages and the Map Position drift?

@sietse

we received some “Bad EKF” messages in QGC. We also saw messages indicating that the ROV was switching between Compass 0 and Compass 1. When these messages appear, is the ROV switching between the MMC5883 and the AK09915 as its primary compass source? And if so, how does it decide when one of the compasses is providing faulty data?

I tried looking into the ArduSub documentation to see how and why the compass sources are switched after a ‘faulty’ compass is detected, but I couldn’t find anything about the switching compass messages.

We’ll test our ROV once again with the calibrated gyro, and let you know how it went!

Thank you so much!

[sietse]

Hey @tony-white!

Yesterday we tested our ROV and DVL after calibrating the compass by the waterside, calibrating the gyroscope, and calibrating the DVL’s gyroscope. However, we were still getting EKF Bad messages. We struggled with a few things:

While calibrating the compass within BlueOS, we weren’t able to get the calibrations into the green calibration area. After trying to calibrate the compass in BlueOS multiple times, we decided to try calibrating it within QGC. This gave us one green and one almost green/yellow calibration.

After the compass calibration, we calibrated the gyroscope within BlueOS and also calibrated the DVL’s gyroscope.

Initial Results:

Position Hold worked seamlessly in the beginning. When we tried to navigate back to our launch point using the map, we surfaced about 5 meters away from our original launch point. This was already a big improvement compared to what we’d experienced before.

When we returned to our launch point, we decided to go back up the river to test Position Hold again, perform a small circular dive, and return to the launch point on the map. While heading back up the river, we got a “Bad EKF” message followed by a “Yaw realigned” message. We saw the QGC compass immediately jump to a different angle after the yaw realignment (which seems logical).

When we tried Position Hold again, it worked well for about a minute before the ROV started drifting sideways. We were monitoring the battery voltage and current and noticed that the thrusters weren’t responding to the sideways drift and weren’t pushing the ROV back to its intended position. When the drifting paused for a few seconds, we decided to return to our launch point on the map. This time, the ROV surfaced about 25 meters away from the launch point, and its front wasn’t pointing in the correct direction. This was probably caused by the yaw realignment messing with the compass angle.

Compass Observations Post-Dive:

After our dive, we opened the BlueOS Compass screen to monitor the compass inputs and noticed some strange data:

The EKF (which should estimate orientation using the MMC5883 and AK09915 data) showed a heading that was 180 degrees off from the actual compass values. At the time of the screenshot, the MMC5883 and AK09915 values seemed correct, but the EKF heading was completely wrong. Since we used the EKF heading in QGC to navigate, it’s no surprise we surfaced 25 meters away from our original launch point!

Questions:

1. How do the compasses work together in the ROV?
   We also saw messages indicating that the ROV was switching between Compass 0 and Compass 1. Does this mean the system alternates between the MMC5883 and AK09915 as the primary compass source? If so, how does it decide when one compass is providing faulty data?
2. How is the map position determined?
   Is the map position calculated using the DVL’s velocity data in combination with the ROV’s heading (based on EKF data from the MMC5883 and AK09915)? If this is the case, why isn’t the DVL’s yaw angle used as an additional heading input? The DVL A50 has a built-in AHRS, right?

[tony-white]

Hi @sietse -
Can you please share the .BIN logs from your dives? You can find these in the Log Browser menu, as well as the BlueOS system logs (found in the lower left menu of BlueOS, the gear icon.) You should be able to drag and drop them into your forum post reply.

Please confirm what version of ArduSub and BlueOS you have running on the system.

What approximate depth and bottom type was present during your testing? What navigational gain setting were you operating at primarily?

We’ll b better equipped to determine whats going on with that information. Thanks!

[sietse]

Hey @tony-white !

Here’s the .BIN log file from yesterday:
00000024.BIN (10.5 MB)

We’re currently using BlueOS 1.3.1 together with ArduSub 4.1.2 Stable!
The reason we haven’t updated to 4.5.0 yet is that we’re using a custom thruster configuration (7 thrusters). To use ArduSub 4.5.0, we would need to recompile our custom thruster configuration.

Our test environment was approximately 6.5 meters deep, and we conducted dives at depths between 1.5 meters and 6 meters. The bottom type was mostly sandy, with some grit here and there, but no large pebbles or rocks.

As for our Joystick Input Gain levels, during manual mode they were set to 80%.

Regarding the PID Gain settings, here’s what we are currently using:
PSC_POSXY_P = 2.0
PSC_POSZ_P = 1.0
PSC_VELXY_D = 0.8
PSC_VELXY_I = 0.5
PSC_VELXY_P = 5
PSC_VELZ_P = 5

Here’s also the rest of our parameter list:
Submarine-4.1.2-STABLE-20241119-132352.params (18.2 KB)

While downloading the log file and parameter list, I checked the Compass tab in the vehicle setup to review how the compasses were performing. I noticed that the MMC5883 displayed a warning indicating that “quick calibration” was used. However, we fully calibrated the compass by rotating it around its axis, rather than using the quick calibration option.

Could it be that BlueOS registers the standard QGC compass calibration method as a “quick calibration”? If so, might this affect the calibration quality or cause inconsistencies in how the compass data is interpreted?

Thank you so much in advance!

[williangalvani]

Hi! that likely means the last step of the calibration got skipped. The Onboard calibration consists of 3 different steps.
The last on “fix_radius()” uses Ardupilot’s built-in World Magnetic Model (a table of declination/inclination per world location) to scale the compass readings so they match the expected field at a given location.

This “Full calibration” is also required in order for the Ekf to use “3d compass fusion” which should improve performance overall, specially when in unusual attitudes.

The gotcha is that this step cannot run without a global position. In Sub 4.5 we introduced two related changes:

• ORIGIN_LAT/ORIGIN_LON parameters that can be used when there’s no GPS/DVL connected.
• The calibration process, when done in BlueOS, will now allow setting the vehicle position so this last step can run, too.

So I would recommend rebuilding Ardusub 4.5 and using BlueOS for calibrating.

[williangalvani]

Looking into your log, it looks like you are getting a lot of magnetic interference on the compasses:

It seems to be directly correlated to the total system current draw:

This is causing the compasses to steer away from real north, which causes the ekf to increase the estimated Gyro bias.

Decreasing EK3_GBIAS_P_NSE could help. but the real fix would be to either get rid of the magnetic interference, or calibrate the system to ignore it.

For the calibration, I recommend using this tool:
https://firmware.ardupilot.org/Tools/WebTools/MAGFit/