Gyros, Tilt, and Quaternions are not your friend

The Rocketry Forum

Help Support The Rocketry Forum:

This site may earn a commission from merchant affiliate links, including eBay, Amazon, and others.

AllDigital

Well-Known Member
TRF Supporter
Joined
Jun 14, 2013
Messages
616
Reaction score
751
Location
SoCal
About a month ago, my son and I decided to ditch the packaged EM7180 board we were using for tilt angle monitoring and roll our own, since most of the IMUs are a black box and/or they integrate accelerometer data, which doesn't work reliably for very long on rockets. The solution we had been using also integrated accelerometer data and the output was unreliable about ten seconds after launch.

With a fresh new ST LSM6DSO32 6dof accel/gyro we mastered the basics with the gyro. We got stationary drift offset to almost nothing over five minutes and we were reading rotations and integrating them accurately into degrees at 500 hz on the bench. Then we hit a brick wall. If the rocket didn't spin, calculating tilt would be easy peasy, but with high rates of spin (or low rates of spin) you have no idea how to integrate your pitch and yaw data (e.g. if you pitched +5 degrees, then rolled/spun 180d, then pitched -5d, you are now sitting at 10d pitch - not -0d pitch). At first, we tried to use solutions solely based on integrating Euler Angles. Some seemed to work, but when stress tested the results were ugly. We tried everything we could, before jumping in the deep end of the pool with quaternions.

I spent a week reading every previous post on the board related to tilt, quaternions, and I sifted through thousands of lines of code. At some point in 2012, @UhClem said that "quaternions are your friend" in solving the tilt problem. I'm eternally grateful for David, and the dozens of others that have weighed in over the years (@jderimig @keithp @JimJarvis50 etc), but I'd also like to declare that while they are great at solving this problem, quaternions are not your friend. I spent about 20 hours tracing @keithp 's AltusMetrum code (thank you!!), but couldn't quite get things to work using subsets of code. That was probably a blessing, as it forced us to decompose each step and fully understand what was going on. (side note, HUGE props to Keith for making his code open source).

So, the punchline is that we finally got quaternions to work for us with our gyro and using the accelerometer to baseline tilt angles before launch. We haven't flown it yet, but I am amazed that I can put my board in a football, launch it up high, spin it, tumble it end-over-end, and when I bring it back to zero it has almost no drift. That is the amazing power of quaternions. Here is our summary of what we are doing and some sample code for future travelers trying to solve the same challenge with rockets. The code is franken-code, hacked together from many sources, and not optimized, but the quat code runs in less than 1.8 microseconds for me on a teensy 4.0, so I don't have much incentive to hone it. That said, we would appreciate any feedback, ideas, or advice that could make it better for others.

Basic logic flow:

1. On the pad (pre launch) sample the accelerometer, calculate tilt angles, and prepopulate the "master Quaternion" with the starting tilt (an over-simplified version in code below)
2. After launch, start integrating gyro rotations into the master quaternion with the following steps (for us sampling at 500x per second):
- Sample the gyro and convert raw values to degrees per second, proportionalized for the tiny amount of time that has gone by
- Convert the rotations into a half Euler angle and load it into a temporary quaternion
- Combine the temporary quaternion rotation into the master quaternion by multiplying the two together
- Normalize the master quaternion back below 1, as the integrations over time cause it to get too big
- last, convert the master quaternion back into angles that humans can understand.
- The resulting angX, angY, angZ are your roll, pitch, yaw. Watch pitch (AngY) and yaw (AngZ) values for max tilt lockout.

< Code in the next post >
 
Code example....

C-like:
// Gyro Libraries
#include "Mike_LSM6DS.h"
#include "Arduino.h"
#include "Wire.h"
#include "I2Cdev.h"   

I2Cdev*   i2c_bus;

#define pinInt1 33
#define pinInt2 32

  LSM6DS LSM6;
  int16_t accelRead[3];
  int16_t gyroRead[3];
 
  float Ax, Ay, Az;
  float Gx, Gy, Gz;

  float AxOffset = -3.79444;
  float AyOffset = .3354;
  float AzOffset = -22.3125;

  float GxOffset = -.131247;
  float GyOffset = .4083;
  float GzOffset = .127094;
 
  unsigned long td;
  unsigned long lastMicros;
  float dt = 0.0;


// QUATERNION STUFF
   #include "math.h"
   struct quaternion {
     float r;    /* real bit */
     float x, y, z;  /* imaginary bits */
    };
   quaternion masterQuaternion;
   quaternion tempQuaternion;
   float angX, angY, angZ;


void setup() {
 
  Serial.begin(115200);
  while (!Serial) delay(10);

  pinMode(pinInt1, INPUT);
  pinMode(pinInt2, INPUT); 
 
  Wire.begin();
  delay(100);
  Wire.setClock(300000); // 300kHz I2C clock.
  delay(1000);

  LSM6.setAccelRateScale(B01100100); // 01100100 :  0110 = 416hz, 01 = 32g, 0 = no LPF, 0 = default
  LSM6.setGyroRateScale(B01101100); // 01101100 :  0110 = 416hz, 11 = 2000 dps, 0 = FS, 0 = default

  //Quaternion stuff
  // set up the master quaternion using the accelerometer ground values before launching
  gyroPrelaunch();

  Serial.println("Starting....");
  lastMicros = micros();
}



void loop() {  // This is the main loop

  unsigned long starttime = millis() + 200;  // output to monitor every 200ms

  while(starttime > millis()) {

    if(digitalRead(pinInt1) == LOW) {  //get an interupt from the gyro at 500hz
      
      LSM6.readGyroData(gyroRead);   
      td = micros() - lastMicros;
      lastMicros = micros();
      dt = (float) td / (float) 1000000.0;  // this is the time interval in microseconds

      //now convert the gyro readings into degrees per second, based on the lsb per degree from the datasheet
      Gx = (float)gyroRead[0] * (float) .070;  // 4000=140, 2000=70, 1000=35, 500=17.5, 250=8.75, 125 = 4.375 (datasheet pg 25)
      Gy = (float)gyroRead[1] * (float) .070;
      Gz = (float)gyroRead[2] * (float) .070;

      //adjust for still calibration offsets
      Gx += GxOffset;
      Gy += GyOffset;
      Gz += GzOffset;
      // now factor for fraction of a second
      Gx = Gx * dt;
      Gy = Gy * dt;
      Gz = Gz * dt;

      // done getting what we need from the Gyro. Got Gx, Gy, Gz represented in degrees moved since last sampled

      // now convert hardware orientation to our quaternion orientation. Quaternion functions expects x = roll, y = pitch, z = yaw
      float qX, qY, qZ;
      qX = Gy;
      qY = Gz;
      qZ = Gx;

      gyroRotate(qX, qY, qZ); // does the four quaternion functions and updates angX, angY, angZ
    }
  }
 
   //output to the monitor every 200ms so we can see it working
   Serial.print("Roll = ");Serial.print(angX,1);
   Serial.print("   Pitch = ");Serial.print(angY,1);
   Serial.print("   Yaw = ");Serial.println(angZ,1);
 
}  // end main loop



void gyroPrelaunch() {
 
  // set the baseline quaternion tilt values using accel data before launch 
  masterQuaternion = { .r = 1, .x = 0, .y = 0, .z = 0 };

  LSM6.readAccelData(accelRead); //populate accelRead[3] array with raw sample data from accelerometer
  Ax = accelRead[0] + AxOffset;
  Ay = accelRead[1] + AyOffset;
  Az = accelRead[2] + AzOffset;
                  
  double a1 = 0.00, a2 = 0.00;       //Roll & Pitch are the angles which rotate by the axis X and y   
  double x_Buff = float(Az);
  double y_Buff = float(Ax);
  double z_Buff = float(Ay);
  //set angles for pitch and yaw
  a1 = atan2(y_Buff , z_Buff) * 57.3; //pitch
  a2 = atan2((- x_Buff) , sqrt(y_Buff * y_Buff + z_Buff * z_Buff)) * 57.3; //yaw
    
  //now integrate the angles into our freshly minted master quaternion (ignoring roll)
  gyroRotate(0,a1,a2);

}


void gyroRotate(float gRoll, float gPitch, float gYaw) {

  // R, P, Y inputs should be degrees factored for time interval

  //first convert the gyro tiny rotation into a half euler and store it in a temporary quaternion
  quaternionInitHalfEuler(gRoll, gPitch, gYaw);

  //now combine it with the masterQuaternion to get an integrated rotation
  quaternionMultiply();

  //now normalize the masterQuaternion in case it went out of bounds
  quaternionNormalize();

  //now we need to translate the masterQuaternion back to angles that humans can read
  quaternionAxisAngles();   
}


void quaternionAxisAngles() {
 
  quaternion p = masterQuaternion;

  // Compute the X (roll) angle in radians
  if((1-2*(p.x*p.x+p.y*p.y)) != 0) {
     angX = atan(2*(p.r*p.x+p.y*p.z)/(1-2*(p.x*p.x+p.y*p.y)));
   } else {
    if((p.r*p.x+p.y*p.z) > 0) {
      angX = M_PI / 2.0f;
    } else {
      if((p.r*p.x+p.y*p.z) < 0) {
        angX = -1.0f * M_PI / 2.0f;
      } else {
        angX = 9999999;
        // SIGNAL ERROR DIVIDE BY ZERO!!!
      }
    }
  }
  // Convert x (roll) from radian to degrees
  angX = angX * 180.0f / M_PI;


  //Compute the Y (pitch) angle in radians
  if((2*(p.x*p.y-p.z*p.x)) <= -1) {
    angY = -1.0f * M_PI / 2.0f;
  } else {
    if((2*(p.r*p.y-p.z*p.x)) >= 1) {
      angY = M_PI / 2.0f;
    } else {
      angY = asin(2*(p.r*p.y-p.z*p.x));
    }
  }
  // Convert y (pitch) from radian to degrees
  angY = angY * 180.0f / M_PI; 


  // Compute the Z (Yaw) angle in radians
  if((1-2*(p.x*p.x+p.y*p.y)) != 0) {
     angZ = atan(2*(p.r*p.z+p.x*p.y)/(1-2*(p.y*p.y+p.z*p.z)));
   } else {
    if((p.r*p.z+p.x*p.y) > 0) {
      angZ = M_PI / 2.0f;
    } else {
      if((p.r*p.z+p.x*p.y) < 0) {
        angZ = -1.0f * M_PI / 2.0f;
      } else {
        angZ = 9999999;
        // SIGNAL ERROR DIVIDE BY ZERO!!!
      }
    }
  }
  // Convert z (Yaw) from radian to degrees
  angZ = angZ * 180.0f / M_PI;
 
}

void quaternionNormalize() {

  quaternion p = masterQuaternion;
  float testP = 0.0;

  testP = (p.r * p.r) + (p.x * p.x) + (p.y * p.y) + (p.z * p.z);
  if(testP > 1.0) {
     p.r = p.r * (float)(1.0f / sqrtf(testP));
     p.x = p.x * (float)(1.0f / sqrtf(testP));
     p.y = p.y * (float)(1.0f / sqrtf(testP));
     p.z = p.z * (float)(1.0f / sqrtf(testP));
  }
  masterQuaternion = p; 
}

void quaternionMultiply() {
 
  // combine t with the masterQuaternion to get an integrated rotation
  quaternion p = masterQuaternion;
  quaternion t = tempQuaternion;

  masterQuaternion.r = (p.r * t.r) + (-p.x * t.x) + (-p.y * t.y) + (-p.z * t.z);
  masterQuaternion.x = (p.r * t.x) + (p.x * t.r) + (p.y * t.z) + (-p.z * t.y);
  masterQuaternion.y = (p.r * t.y) + (-p.x * t.z) + (p.y * t.r) + (p.z * t.x);
  masterQuaternion.z = (p.r * t.z) + (p.x * t.y) + (-p.y * t.x) + (p.z * t.r);

}


void quaternionInitHalfEuler(float gRoll, float gPitch, float gYaw) {

  // store the tiny rotation from the gyro in a new temporary quaternion
  // roll, pitch, yaw input is in degrees

  float s_x, c_x;
  float s_y, c_y;
  float s_z, c_z;

  float x = (gRoll / 2.0f) * M_PI/180.0f;  // half the degree and convert it to radians
  float y = (gPitch / 2.0f) * M_PI/180.0f;
  float z = (gYaw / 2.0f) * M_PI/180.0f;

  s_x = sin(x); c_x = cos(x);
  s_y = sin(y); c_y = cos(y);
  s_z = sin(z); c_z = cos(z);

  // This is our quaternion that represents the rotation difference
  tempQuaternion.r = c_x * c_y * c_z + s_x * s_y * s_z;
  tempQuaternion.x = s_x * c_y * c_z - c_x * s_y * s_z;
  tempQuaternion.y = c_x * s_y * c_z + s_x * c_y * s_z;
  tempQuaternion.z = c_x * c_y * s_z - s_x * s_y * c_z;

}
 
That seems like too much work.
Code:
      // Use small angle approximation for cos() and sin()
      q.r = 1;
      q.i = gx * dT / 2.0;
      q.j = gy * dT / 2.0;
      q.k = gz * dT / 2.0;

      Q = qmult(&Q, &q);

      /*
        Renormalizing the quaternion at each step is excessive but
        it should be done periodically.
       */
      if(i%100 == 0)
        qnorm(&Q);
 
Looks good, for your next iteration you do not need to calculate Euler angles:

qdot = w*q/2 (w = gyro rate quaternion)
q(i+1) = q(i) + t*qdot

You can do this in one line of code but you need to get the commutation order right.
 
Last edited:
Well done. Using a MEMS gyro with quaternion differentials is not an easy task. The problems you ran into are very common.

Teensy4.0 is a great board to use for this, as it has a 64-bit FPU that handles the math very quickly. I use a Teensy3.5 and actually lose precision if I integrate too quickly because of 32-bit rounding. Quaternions aren't the only way to do it, there are a few other algorithms out there that work well for our purposes. Great job overall!
 
If you have a quaternion, you can take the vector that represents your vertical, rotate it with the quaternion and then calculate the angle between the two vectors. If you store the tilt limit as the cosine of the angle you don't need to do any trig at all. Quaternions are your friend!
 
ts not a cheap sensor but the Adafruit BNO055
I seem to recall another thread where this sort of sensor fusion was discussed. The problem is that the system model assumes flight dynamics (!G level flight, with some maneuvers) that simply doesn't work with a rocket.
 
I seem to recall another thread where this sort of sensor fusion was discussed. The problem is that the system model assumes flight dynamics (!G level flight, with some maneuvers) that simply doesn't work with a rocket.
If you can find that thread, I'd be interested in reading it. I know it's used by a few rocket related projects and hadn't heard of issues with it.
 
Thanks @UhClem and @jderimig for the improvements. The code above is nowhere close to being optimized. It has the extra steps to convert the gyro data into angles and then into the quat and extra steps to get it back out of the quat into angles for all three axis (unnecessary), but it gave us a good starting point to understand and improve. The overall approach (steps) is based on what Keith is using in his Altus Metrum, but I think he is converting into angles for input, since he is only sampling at 100hz.

I also recall seeing a few posts that reference an IMU where you can disable the accelerometer input into the quaternion engine, but I was worried we'd run into other "black box" issues. Also, most of the packaged IMUs come with a 16G accelerometer, which will give you your ground tilt, but then is pretty much useless for most flights.

Switching over to the teensy 4.0 has been amazing. I was using an ESP32 before and the T4.0 with an ARM M7 is 5000x faster (actually!). I am swimming in CPU cycles and now only constrained by I/O (I2C and SPI and serial comms). I'm tempted to jack my I2C bus up to 1M, but I'm leaving it at 400K for now.

Anyone else that comes along and wants to jump in the deep end with quaternions, I would highly suggest the visualization tutorial by Ben Eater and the guys at 3blue1brown. The interactive model really helped us understand how they worked and test some of our inputs/outputs.

https://eater.net/quaternions
 
I have found a number of articles on the web. One that is pertinent to this discussion is sensors-15-19302.pdf entitled "Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs"

In the paper you linked beware that Valenti's algorithm is not appropriate for a rocket flight because the accelerometer input is not valid in a rocket flight. And his filter uses the magnetic field sensor to correct the accelerometer based attitude which is invalid. So the mag loop is not useful. After you lose those two elements it is reduced to a simple 3dof gyro based system.

Valenti's IMU algorithm is only useful on the pad to determine the initial orientation before launch.
 
If you have a quaternion, you can take the vector that represents your vertical, rotate it with the quaternion and then calculate the angle between the two vectors. If you store the tilt limit as the cosine of the angle you don't need to do any trig at all. Quaternions are your friend!

This!

I had a hard time with quaternions, and at first it felt like I was just plugging and playing algorithms without actually understanding the reasoning behind the computations. I saw a paper, which had visualizations, and links to longer YouTube videos that really gave me the Aha! moment. https://eater.net/quaternions . Kind of reminded me when calculus first clicked, and you could see the real beauty of not just the computation, but the concepts behind it.
 
Wow, very nice Mike . many thanks for sharing this information. I dabble a bit with IMU's but always ran into the math issue, short term results where ok but no success on long term results and being anywhere near accurate. A new direction to pursue.

David
 
Love this thread! I've been on the edge of playing with this stuff for a while, but the big stumbling block for me has been coming up with a good way to understand what the board is "seeing" when the code is running...

So, the punchline is that we finally got quaternions to work for us with our gyro and using the accelerometer to baseline tilt angles before launch. We haven't flown it yet, but I am amazed that I can put my board in a football, launch it up high, spin it, tumble it end-over-end, and when I bring it back to zero it has almost no drift.

Am I reading this too litterally? How did you tell what the drift was afterwards? Did you add a display of some kind that was dumping out angle measurements in realtime? Did you "toss" the board and then plug it back into a laptop to read out where it thought zero was? Inquiring minds want to know!

KMc
 
How did you tell what the drift was afterwards?
Kevin, for most of my testing I'm using a Teensy 4.1 board that has a built in SD card. It can log to SD about 7000 Hz, but I usually just have it update to SD at 100hz. A simple sketch would beep after power cycle then log to SD for twenty seconds. I then pop out the SD and open the file in a computer to "see" what was happening. I've used slo mo video from an iPhone to sync the data to the real-life "events".
My full board is a flight computer with radio, so I can see things in real-time, but the package is bigger. In the past I've also used a tiny TFT display for experimenting, but you can't really see the important stuff if you throw an accelerometer in the air. Better to log to an SD card.
 
This!

I had a hard time with quaternions, and at first it felt like I was just plugging and playing algorithms without actually understanding the reasoning behind the computations. I saw a paper, which had visualizations, and links to longer YouTube videos that really gave me the Aha! moment. https://eater.net/quaternions . Kind of reminded me when calculus first clicked, and you could see the real beauty of not just the computation, but the concepts behind it.

THIS IS ROCKET SCIENCE! :D
(Sorry to bust in like that, but I've been lurking the thread and that just inspired me...)
 
Thanks for that. I am known as a nag about that - one of our old signal analyzers (we use them for vibration) has this lower-bit DAC and I'm always nagging to the youngsters that they must pay attention to their signal and set their full-scale voltage range to suit so they don't lose dynamic resolution. Do they have anything out there with 24 bits?

I put a watch on this thread because I hope to get into this aspect of rocketry some day and this is the real stuff, I believe. I've had some experience long ago with microprocessor control, so I hope it'd come back to me when I have the chance. But for LPR, I'm not sure of the possible applications. You can't put a whole lot of stuff in LP, nor is there much need to. Maybe if it's laying still on the ground after flight, wire a speaker up to squawk "PUT ME DOWN!" when it senses motion again?
 
Kevin, for most of my testing I'm using a Teensy 4.1 board that has a built in SD card. It can log to SD about 7000 Hz, but I usually just have it update to SD at 100hz. A simple sketch would beep after power cycle then log to SD for twenty seconds. I then pop out the SD and open the file in a computer to "see" what was happening. I've used slo mo video from an iPhone to sync the data to the real-life "events".
My full board is a flight computer with radio, so I can see things in real-time, but the package is bigger. In the past I've also used a tiny TFT display for experimenting, but you can't really see the important stuff if you throw an accelerometer in the air. Better to log to an SD card.
I’m assembling a similar nosecone-mountable Arduino “Blue Pill” STM32F rocket data logging and telemetry package with 3-axis accelerometer, gyroscope, and magnetometer, along with altimeter, GPS, SD, and RF transmitter.

Hoping to see what actual high resolution launch data looks like for L1 and L2 launches.
 
Hoping to see what actual high resolution launch data looks like for L1 and L2 launches.
I found that the magnetometer included with the MPU9250 was a pain to work with. It wasn't integrated but was in fact a third party device grafted on and hanging off an internal I2C bus. Worse is that the sample rate was quite limited and using it put sharp limits on the other sample rates.

As a result I ignored it. The old Honeywell sensors without integrated ADC's were in some ways better.
 
I'll have to see. It could be as accurate as I need it to be for determining the general orientation of the rocket without spin, but without much precision. My desktop tests of the MPU9250 show it generally working and I'm not sure how much precision I need. I was thinking that I would only use it to initialize the orientation of the gyroscope on the launch pad.

If I use the Arduino Nano 33 IoT board, it has a built-in LSM6DS3 3-axis accelerometer and gyroscope and a built-in low power bluetooth and WiFi interface for wireless communications of the data I'll be saving on the SD card I'll have connected to it. I'm planning on also having an Arduino-based ground station for collecting and saving telemetry data.

I'm having to be careful because my electronics and firmware development are threatening to swallow and overwhelm my rocketry projects.
 
Side comment.

You guys are stupendously smarter than me when it comes to electronics!:clapping: Kurt Savegnago
Thanks. But not real smarts. The Arduino hardware connecting modules, and firmware is free hobby stuff that high school kids can assemble and use. Its much easier and more advanced than I was in the late 60s!

There are published example circuit hookups and example firmware to use, so it just requires curiosity and care.
 
Thanks,
I was on the bleeding edge when I was 13 and built a Foxmitter and Minimitter tracking transmitters. The Foxmitter had different modules like for tracking or G meter or biological (aghast!) telemetry. Found an old catalog at this link: https://www.spacemodeling.org/jimz/publications/astrocomm_1971_catalog.pdf
The plans for the trackers came out before this concern kitted it and I had to source all the parts from Allied Radio back in the day.
I still had them and they worked when I was in my 30's but somehow lost the box with all my old rocket electronics during a move! Stinks but the old 22.5V Burgess batteries for the Foxmitter are no longer made! The Minimitter I think worked at lower voltage. Look at the size of these things and this was from the late 60's early 70's and it's impressive even today using discrete components.
Kurt
 
Back
Top