Pseudo code or mathematical description of a filter to fusion signals from two IMU sensor for 3-axis gimbal

Hello! I am working on 3-axis stabilizer which is using 3 motors and two IMUs. The IMU has got a 3-axis accelerometer and a 3-axis gyroscope. I am looking for someone who prepares a pseudo-code or mathematical description of how to do signals fusion from both IMU sensors. The first IMU is placed near the camera - this place can be moved by any of the motors. The second IMU is placed on the top of the gimbal and it is not moved by any of the motors. Now I have implemented the Madgwick filter which gets a 3-axis accelerometer and 3-axis gyroscope data as an input and outputs quaternion. Then I make a conversion from quaternion to Euler angles - pitch, roll, yaw. That calculated angles then I push to the PID regulator and calculate outputs for each of the motors. It works, but has some limitations and does not work in full ranges - to fully understand look at this video [login to view URL] from 4:30 to 5:05. What I want, you can also check in the same video, but from 5:10 to 6:10. To sum up, I want from you an algorithm (in pseudo-code, mathematical description or C code, something that allows me to implement it in my gimbal, the code is written in C, but if you are not a programmer or you do not know a C - no problem) that fully use benefits of second IMU like: - significantly improved accuracy, i.e., the camera is held in position with a much smaller error bandwidth, - drastically larger range of operation, essentially any pitch and yaw angle is possible (for a more detailed discussion of the possibilities and limitations see video above), - stable operation of the 2nd IMU above the yaw axis without deterioration over time due to gyro drift. If your only experience is that you implemented a ready-to-use filter, like mentioned Madgwick or Kalman filters and you do not have any experience related with quaternion math in the academic level - please do not waste our time. I have read many scientific papers related with data processing from IMU sensors, but my math is insufficient, so I will be able to send them to you and work in close cooperation with you, do not worry. :) Write to me only after you watch the video.