Complexities on improving the utmost basic IMU AHRS guidence sketch

[clywedog]

Im in the process of making an autopilot to fly a small plane, and I've used basic calculus and control theory to gather my pitch/roll angle based on integrating a gyroscope and using it in a combination filter with an accelerometer. Surprisingly, after many crashes, I can get it to sorta work. Here's a video of a successful test.

Heres the information-gathering code that makes this 2d interpretation of my orientation:

  prevTime = currentTime;
  currentTime = millis();
  timeDif = (currentTime - prevTime) / 1000; 
 
  float pitchDeriv = (gyevent.gyro.y * 0.8);
  float rollDeriv = (gyevent.gyro.x * 0); // roll derivitive because asymmetry except nevermind and its still here
  rawaccelY = constrain((aevent.acceleration.y / 9.8), -1, 1); //prepares the accelerometer data  ROLL
  rawaccelX = constrain((aevent.acceleration.x / 9.8), -1, 1); //prepares the accelerometer data PITCH
  rawgyroX = (gyevent.gyro.x - 0.0035); //ROLL
  rawgyroY = (gyevent.gyro.y + 0.0015); //PITCH
  rollAccelRad = asin(rawaccelY); //converts acceleration to radians ROLL
  pitchAccelRad = asin(rawaccelX); //converts acceleration to radians PITCH
  rollEstimated = (rollAccelRad * alpha) + (1 - alpha) * (rollEstimated + timeDif * rawgyroX * -1); //Complimentary FILTERS
  pitchEstimated = (pitchAccelRad * alpha) + (1 - alpha) * (pitchEstimated + timeDif * rawgyroY);
 
  float Aroll = rollEstimated * 57296 / 1000;
  float Apitch = pitchEstimated * 57296 / 1000; // converts to degrees
 
  Oroll = computeRollPID(Aroll);
  Opitch = computePitchPID(Apitch) - (gyevent.gyro.y * 20);
  mapServoR = constrain((Oroll + Opitch + rollDeriv + pitchDeriv + 90), 90 - servoConst , 90 + servoConst);
  mapServoL = constrain((Oroll - Opitch + rollDeriv - pitchDeriv + 90), 90 - servoConst , 90 + servoConst);
 
  servoL.write(mapServoL);
  servoR.write(mapServoR);

Its a lot but what's doing most of the work is rollEstimated and pitchEstimated

Here's the problem in an example; Lets say the plane has a pitch of 0 and a roll of pi/8, as the plane yaws, its roll is exchanged for its pitch. At a yaw angle of pi/2 radians, the roll has completely turned to pitch, and roll is zero. And at a rotation of pi, pitch has turned entirely back into roll, except reversed. My sketch currently doesn't account for this and struggles immensely due to believing yaw has zero effect on pitch and roll.
So I've looked around online desperate for the equation or function to integrate these but now I'm faced with hugely unintuitive and computationally intensive stuff like Euler rotations and quaternions.
I've done some research but I don't know if I'm going in the right direction due to the way quaternions are all about measuring an absolute orientation, but I cant know my exact yaw rotation, I can only estimate it with a gyroscope. Plus my chip only runs at 8Mhz and is already using up 80% of flash, is there some easier way I can modify my current integration setup as to not need my absolute yaw, just the rate, and integrate pitch and roll accordingly.
So much appreciation for any ideas, I really don't have much other help.

[jremington]

Euler angles are useful for nearly level flight, but not much else, precisely because of the problems you have already encountered.

Furthermore, there are 12 different systems of Euler angles, with no obvious way to identify which is being used, so the angles you report don't necessarily mean much to other people.

This is why nearly everyone has moved on to the use of quaternions to store orientation information. They are easy to convert to Euler angles if you need that information, but again, this is useful only for nearly level flight.

The two most popular AHRS IMU algorithms that update the quaternion are the Madgwick and Mahony filters, with plenty of open source examples to study. I prefer the latter because of its simplicity and speed, but the theory takes some time to absorb.

A decent reference comparing various algorithms is OlliW's Bastelseiten » IMU Data Fusing: Complementary, Kalman, and Mahony Filter

For handling quaternions, take a look at the various video game graphics tutorials, or this blog: Quaternions