Add a GPS + IMU Fusion Part by Phill310 · Pull Request #1237 · autorope/donkeycar

Phill310 · 2026-06-12T07:56:12Z

This is the addition mentioned in #1236.

adds the BNO08x IMU into IMU.py
creates a fusion part in gps_imu_fusion.py
This fusion part uses a 5 state Kalman filter to predict your position based on your velocity and acceleration data. The 5th variable is an acceleration bias to prevent drift when the car isn't moving. There is also a low pass EMA filter to clean out noise. Finally we added a friction factor to slowly bleed velocity.

We tested with a SparkFun NEO-F10N GPS which had a frequency of 10Hz. I believe the BNO08x IMU has a frequency of 50 Hz so in theory the fusion part can go up to 50 Hz. The main limiting factor is the drive loop frequency since the fusion part recalculates position once every drive loop.

This should support any GPS since the fusion part only takes the x and y position data from the gps.

Config Options added:

USE_FUSION
FUSION_DEBUG

I hope this explains everything we did. If not please ask questions so I can expand on our thinking.
Should I also make a pr in the docs repo to add a section for the fusion part and update the IMU section?

TCIII · 2026-06-12T22:58:58Z

@Phill310,

Are the x and y GPS positions in degrees or in meters from the Greenwich Meridian and the Equator?

Regards,
TCIII

Phill310 · 2026-06-13T05:38:55Z

@Phill310,

Are the x and y GPS positions in degrees or in meters from the Greenwich Meridian and the Equator?

Regards, TCIII

I believe that the Fusion part outputs the position in UTM coordinates (same as the gps part as far as I know). So yes it give the distance from the equator in meters but for easting it outputs the number of meters from its specific zone's central meridian. Is this something we should describe in the fusion class?

DocGarbanzo

Thank you @Phill310 - this is good PR that we would like to merge. I have two general feedback points:

We should not have to call pip install some_package to make use of the BNO085 or other sensors. The relevant python packages should be added to setup.cfg so everything works out of the box.
The IMU interfaces use scalars instead of vectors for acceleration, gyro, quaternions, etc. That is bad, but it is of course not your fault as such code was there before. It would be great if instead of making this even worse, we would clean this up as part of this PR as for the moment, no-one is really using IMU data yet. - Let me know if this exceeds your intended contribution and we can do a refactoring PR afterwards.

DocGarbanzo · 2026-06-13T18:45:39Z

+        cosy_cosp = 1 - 2 * (qj * qj + qk * qk)
+        return math.atan2(siny_cosp, cosy_cosp)
+
+    def run(self, gps_x, gps_y, ax, ay, az, gx, gy, gz, qi, qj, qk, qr):


Can you change the interface to pass vectors instead of single coordinates:
[gps_x, gpy_y] -> gps
[ax, ay, az] -> a or accel?
[gx, gy, gz] -> g or gyro?
[qi, qj, qk, qr] -> q or quarternion?

DocGarbanzo · 2026-06-13T18:48:07Z

-    
+
    - MPU9250
    pip install mpu9250-jmdev


These install commands are partially wrong (sudo...). can you add the python dependencies into setup.cfg so they get installed as part of the standard donkey install?

DocGarbanzo · 2026-06-13T18:50:08Z

-        self.gyro = { 'x' : 0., 'y' : 0., 'z' : 0. }
+        self.accel = {'x': 0., 'y': 0., 'z': 0.}
+        self.gyro = {'x': 0., 'y': 0., 'z': 0.}
        self.mag = {'x': 0., 'y': 0., 'z': 0.}


Dictionaries are unnecessary here, please just use vectors (or lists, if you prefer python native over numpy. Both types would be fine).

DocGarbanzo · 2026-06-13T18:50:41Z

-                self.mag = { 'x' : ret[13], 'y' : ret[14], 'z' : ret[15] }
+                self.accel = {'x': ret[1] * GRAVITY, 'y': ret[2] * GRAVITY, 'z': ret[3] * GRAVITY}
+                self.gyro = {'x': ret[4], 'y': ret[5], 'z': ret[6]}
+                self.mag = {'x': ret[13], 'y': ret[14], 'z': ret[15]}


Same here, vectors are better than dicts.

DocGarbanzo · 2026-06-13T18:55:01Z

-
+
+    def run_threaded(self):
+        return self.accel['x'], self.accel['y'], self.accel['z'], self.gyro['x'], self.gyro['y'], self.gyro[


It would also be much cleaner to return a tuple of two vectors (accel and gyro) and a float (temp), instead of unravelling the vectors.

DocGarbanzo · 2026-06-13T18:58:10Z

        if cfg.SIM_RECORD_LOCATION:
            inputs += ['pos/pos_x', 'pos/pos_y', 'pos/pos_z', 'pos/speed', 'pos/cte']
-            types  += ['float', 'float', 'float', 'float', 'float']
+            types += ['float', 'float', 'float', 'float', 'float']


And this...

DocGarbanzo · 2026-06-13T18:58:18Z

-            types  += ['float', 'float', 'float', 'float', 'float']
+            types += ['float', 'float', 'float', 'float', 'float']
        if cfg.SIM_RECORD_GYROACCEL:
            inputs += ['gyro/gyro_x', 'gyro/gyro_y', 'gyro/gyro_z', 'accel/accel_x', 'accel/accel_y', 'accel/accel_z']


And this...

DocGarbanzo · 2026-06-13T18:58:29Z

-            types  += ['float', 'float', 'float', 'float', 'float', 'float']
+            types += ['float', 'float', 'float', 'float', 'float', 'float']
        if cfg.SIM_RECORD_VELOCITY:
            inputs += ['vel/vel_x', 'vel/vel_y', 'vel/vel_z']


... and this as well.

DocGarbanzo · 2026-06-13T19:02:42Z

@@ -877,7 +873,7 @@ def add_camera(V, cfg, camera_type):
                       'imu/acl_x', 'imu/acl_y', 'imu/acl_z',


If you don't want to touch this class, but want to work with vectors, then you can write a one-liner class that combines x, y, z values into a vector, like:

class Vectoriser: def run(x, y, z): return [x, y, z]

DocGarbanzo · 2026-06-13T19:03:43Z

+                V.add(fusion,
+                      inputs=[
+                          'gps/utm/longitude', 'gps/utm/latitude',  # From GPS
+                          'imu/acl_x', 'imu/acl_y', 'imu/acl_z',  # From IMU


Also, use vectors here.

…calars

aokhader · 2026-06-14T10:54:52Z

We are working on refactoring our code to use vectors instead of scalars, and we added the package install to setup.cfg. For the refactoring of the IMU code in general and for the code that wasn't directly from our fusion module, we left it as-is since we thought it was the base setup for the code. We are going to try to refactor the the code for all the IMU modules, but I'm not sure if we can verify if everything works the way it should for all the components including the camera since it would be out of our scope. Right now it is a PR in our repo if it helps.

Refactored IMU code and all its dependencies to use vectors, in the context of our fusion module

aokhader · 2026-06-17T01:20:18Z

I've refactored the IMU code to use vectors and changed all the dependencies for the IMU to do the same, as well as verified that it works properly on path following. Is there anything else that needs fixing in this code?
Just to reiterate, the IMU fusion module is GPS-agnostic, so it can be used with a GPS with RTK corrections.

Phill310 and others added 4 commits June 4, 2026 19:13

Initial setup for gps + imu fusion

f4055d9

Reuse more of the old gps setup code

581ce4a

Added friction and velocity decay to acceleration; fixed loop

5316f14

Check if imu is enabled before activating fusion

dff5f34

DocGarbanzo requested changes Jun 13, 2026

View reviewed changes

aokhader added 2 commits June 14, 2026 03:14

Added package to setup and made IMU interface use vectors intead of s…

aa146c3

…calars

Refactored GPS and IMU to use vectors

f3944b6

aokhader added 4 commits June 14, 2026 04:01

Fixed gps input to fusion to be a tuple

848bfb4

Properly combined the groupings of sensor values

3576766

Merge branch 'main' into PR-donkeycar-requests

b0a3003

Refactored IMU code to use vectors

5f8a84e

Refactored IMU code and all its dependencies to use vectors, in the context of our fusion module



		def run_threaded(self):
		return self.accel['x'], self.accel['y'], self.accel['z'], self.gyro['x'], self.gyro['y'], self.gyro[

		@@ -877,7 +873,7 @@ def add_camera(V, cfg, camera_type):
		'imu/acl_x', 'imu/acl_y', 'imu/acl_z',

Uh oh!

Conversation

Phill310 commented Jun 12, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

TCIII commented Jun 12, 2026

Uh oh!

Phill310 commented Jun 13, 2026

Uh oh!

DocGarbanzo left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

aokhader commented Jun 14, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

aokhader commented Jun 17, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

Phill310 commented Jun 12, 2026 •

edited

Loading

aokhader commented Jun 14, 2026 •

edited

Loading

aokhader commented Jun 17, 2026 •

edited

Loading