The math behind the heading calculation

ArduRover
1

I know that this question can be out of topic, but I’m very interested in understanding the math which is behind the calculation for the heading and the motor commands used by ArduPilot Rover. In particular, I’m very curious to understand how and if the Kalman filter is used when, for example, the compass is disabled and only two GPS modules are used to calculate the vehicle’s yaw.

I tried to search for this documentation, but I only found this link Extended Kalman Filter Navigation Overview and Tuning — Dev documentation which doesn’t consider the dual GPS use case.

Thank you!

2

Here’s some more reading for you:

https://ardupilot.org/rover/docs/common-compassless.html

I can’t give you the detailed computations involved, but effectively, accelerometer data is used to “fill the gaps” between GPS heading updates, and it does so fairly well, even when the updates are interrupted for several seconds at a time.

1 Like
3

Thank you! So, when the compass is disabled, are the angular velocities and the accelerations from the IMU always considered together with the DUAL GPS for the heading?
I was thinking that only the GPS values were used for the heading calculation.

If this is correct, I could use my XSENS Mti-300 instead of the standard pixhawk IMU in order to improve the heading calculation with the DUAL GPS configuration. Am I right? (The Mti-300 is a very accurate sensor compared to the standard one used by pixhawk.)

Is there a Matlab code or some equations that explain how the heading is calculated or the used algorithm is proprietary and so we can only guess how it work?
I recently compiled on Ubuntu the Ardurover firmware, is it possible to find the equations there?

4

The code is all open source, and the specific algorithm you want is called GSF - Gaussian sum filter.

I’m not sure if your IMU is supported, but if it is, nothing is stopping you from using it.

By default, GSF is enabled for fallback. There are parameters to disable it, but you probably should not do so.

5

I was investigating further about how the algorithm of Ardurover works.
I found out this document (https://px4.io/wp-content/uploads/2020/07/Paul-Riseborough-PX4-state-estimation-update.pdf) about the GSF and the EKF used together to output the yaw estimations.

If I got it, the heading and the position is calculated without using the compass (I disabled the compass) and they only rely on accelerations and angular velocities from the IMU plus the coordinates coming from the two GPS modules.
I’m trying to import the imu and gps data in Matlab in order to reproduce the calculations because I’m interested in being able to test the data in a more convenient way and to plot some graphs.

The article showed this explanation about the use of GSF:

ekf-gsf
ekf-gsf786×468 106 KB

So, five instances of EKF are run by using the same input coming from the IMU and then the GSF use their estimation to further calculate the yaw? If yes, how the GPS modules are handled?

I also found out this interesting document: PX4-ECL/Process and Observation Models.pdf at master · PX4/PX4-ECL · GitHub but it considers also the magnetometer data and only a GPS source so it is not about the dual GPS configuration.