How to improve the estimation (how to minimize the loss?)

[meteoricfarm]

Dear Contributors,

Thanks to your work, I could estimate the Euler angle from IMU sensors. But, there is a gap between estimated the Euler angle from EKF and Magwick filter and ardupilot simulation. Here is how I used your work.

Flight: ArduCopter (ardupilot SITL) Data collection frequency: 100ms (10 rows in every second.)

class EKFCalc():
    def __init__(self):
        # set initial quaternion and instantiate ekf
        q = Quaternion()
        q0 = q.from_angles(np.array((-0.0026674278, -0.2326069474, 0.0621483289))) # convert initial roll, pitch, yaw to quaternion
        self.ekf = EKF(frequency=10, frame='NED', q0=q0) # 100 millisecond frequency, NED(North, East, Down)
        
        # other member variables
        self.prev_quaternion = q0

    def run(self, acc_data:np.array, gyro_data:np.array, mag_data:np.array = None, angle:tuple = None):
        # estimate quaternion from IMU
        if mag_data is not None:
            # gyr = (xgyro, ygyro, zgyro)  // acc = (xacc, yacc, zacc)  // mag = (xmag, ymag, zmag) 
            curr_quaternion = self.ekf.update(self.prev_quaternion, gyr=gyro_data, acc=acc_data, mag=mag_data)
        else:
            curr_quaternion = self.ekf.update(self.prev_quaternion, gyr=gyro_data, acc=acc_data) # w, x, y, z
        
        # save current quaternion for next iteration
        if angle is not None:
            q = Quaternion()
            self.prev_quaternion = q.from_angles(np.array(angle))
        else:
            self.prev_quaternion = curr_quaternion
        
        # instantiate quaternion object
        q = Quaternion(curr_quaternion)
        # convert quaternion to euler angle with degree
        # to_angles return yaw, pitch, roll respectively
        angles = q.to_angles()

        return {"roll": angles[2], "pitch": angles[1], "yaw": angles[0]} # degree

1. Take the initial Euler angle from SITL and use it as the first quaternion to instantiate the EKF filter.
2. Update the EKF filter as soon as the IMU sensor data is collected.
3. Set the previous quaternion depending on the conditions
4. the conditions:

• no reference Euler angles from SITL: use previous quaternion data from the estimated value of the EKF filter.
• reference every update from SITL: use previous quaternion data from SITL Euler in every update.
• reference every 5th update from SITL: use previous quaternion data from SITL Euler in every 5th update.

Are there ways to improve the accuracy of estimation?

Sincerely,