# IMU + GPS fusion and change of reference frame

I started working with GPS + IMU fusion using Kalman Filter. For this, I'm using Python, with Madgwick filter from a library (https://github.com/morgil/madgwick_py) and Kalman filter also from an library (http://github.com/rlabbe/filterpy), i developed a model as below, where C(east or north) is the coordinate in east or north, v is the velocity and a is the acceleration.

$$$$C_{north}(k) = C_{north}(k-1) + v_{north}(k)\Delta t + a_{north}(k) \frac{\Delta t^2}{2}$$$$

$$$$C_{east}(k) = C_{east}(k-1) + v_{east}(k)\Delta t + a_{east}(k) \frac{\Delta t^2}{2}$$$$

$$$$v_{north}(k) = v_{north}(k-1) + a_{north}(k) \Delta t$$$$

$$$$v_{east}(k) = v_{east}(k-1) + a_{east}(k) \Delta t$$$$ Before i make an hardware implementation, im simulating my model with a dataset that provides GPS latitude,longitude and speed and IMU (MPU 9250) data (accel + gyro + mag). But as the dataset dont provides me heading or course over the ground information, im using madgwick filter to get yaw and use this as my heading (with magnetic declination subtracted). And with this i decompose the velocity vector in east and north component. Like the picture below:

About the GPS, im transforming the latitude and longitude from geodetic to NED frame of reference, and taking the velocity vector and decomposing it, as i described above with yaw angle. And with the GPS, no doubts, i think thats correct.

My doubts arise from IMU fusion with madgwick to get the acceleration in NED frame and heading, the y axis of the accelerometer is pointing to the north of the car and the x axis to the east of the car, how should i pass the IMU data to the madgwick filter, and its correct i get the yaw as my heading information? With the yaw as my heading will i get the correct angle to decompose my velocity that is coming from the GPS? Now, the madgwick requires accel, gyro and mag data in (x,y,z), but as the axis of the accelerometer in this dataset are inverted, im passing accel and gyro in (y,x,z) order and mag(y,x,-z). Just to test what the model with this logic is getting, I put 0 in all the components of the covariance matrix Q and a large number in the components of the covariance matrix R, so in the figure below is what the model and only the model is predicting

In blue is the model prediction without updating phase, and in black is the "truth displacement". I known that without the updating process with GPS the model should diverge, but it seems horrible. The last picture is the Kalman Filter with R and Q with the values that i think that should be.

As we can see, the model follows the truth displacement but not finish the path, so any help with the Madgwick filter will help me a lot. Other problems like R or Q covariance matrix i already found good matrix. I think that the IMU fusion itself is the problem here.