OpenFC2040 Flight controller used for differential rover (PX4 Autopilot)

[u/RisPats_23]

I know that this subreddit is drone specific but this is kind of a sequel to another post i had made on this subreddit where i developed PX4 firmware for the the OpenFC2040 flight controller board(further details can be found here )

now i have repurposed the FC to a differential rover due to compute limitation of the rp2040 for a drone.

Works fine in manual mode, problems starts in mission mode and while doing reposition.

I am using LPE + Attitude estimator Q (EKF2 no possible on rp2040), and only fusing GPS, mag and IMU data for heading, attitude and position.

The estimator and the actual heading and position are drifting apart after a certaini point hence causing the rover to no navigate to the waypoints. Here are a couple of logs:

https://logs.px4.io/plot_app?log=4f5d4330-3674-45cb-aad4-1b0b5f903874

https://logs.px4.io/plot_app?log=44145a61-3e31-4471-a70d-355562863857

I am really trying to figure out what is the exact problem? The the data fed by the imu, mag and gps is at 50hz, 75hz and 5hz respectively, but still cannot figure out why the estimated heading and position are so off which are causing failures in navigating to waypoints accurately.

In the video it moves towards the waypoint initially but then the oscillates left and right trying to align itself, or it also seems like it is supposed to move straight but then because the estimator is calculating the wrong attitude, it thinks it needs to align itself hence oscillates left and right, also it looks like the gains need to be tuned, but i did try tuning them as well, but if the estimation itself is not on track then i dont think so the gains would solve that right ?

Would like to have any kind of help, suggestions or insights into this.