Hi Amir,
It looks like you are dealing with sensor fusion and estimation problems. You can try the following things.
Ensure that all sensor data is transformed into a consistent coordinate frame before being used in the filter. Verify that the sensors are properly calibrated. If the insEKF expects data in a specific frame (e.g., a body frame or an inertial frame), you'll need to apply the necessary rotations and translations to the X-plane data to match this expectation.
The tuning of the filter involves setting the process noise and measurement noise covariance matrices. These matrices should accurately reflect the expected noise levels of your sensors and the dynamic model. The initial state estimate and initial state covariance can also impact the performance of the filter. The dynamic model used in the filter should accurately represent the system's behavior. Any discrepancies between the model and the actual system dynamics can lead to estimation errors.
Check the quality of the sensor data. Preprocessing the sensor data with low-pass filtering may help remove high-frequency noise that could be affecting the estimation. The way GPS data is integrated into the filter can significantly affect the estimation. Different sensors might have different update rates. Make sure the filter can handle asynchronous measurements properly, or resample the data to a common rate if necessary.
To debug properly, test the filter with individual sensors (just accelerometers, just gyroscopes, just GPS) to see if one sensor's data is causing more issues than others. Try running the filter with simulated sensor data where you know the true state. Visualize the raw sensor data, the estimated state, and the true state (if available) to identify any patterns or discrepancies.
I hope this helps!
