Quaternions, Euler Angles and CH Robotics UM6

Hi,

I have interfaced a CH Robotics UM6 to an Arduino. The UM6 is a 9 DoF Orientation Sensor (Accel/Gyro/Magno) with an on board ARM Cortex processor running an Extended Kalman Filter so the whole thing outputs its stable orientation via a UART.

Product info here:
http://www.chrobotics.com/index.php?main_page=product_info&products_id=9

Datasheet here :
http://www.chrobotics.com/docs/UM6_datasheet.pdf

Simples. Or so I thought. I thought buying a device with a built in Kalman Filter would protect me from having to work out some complex maths. However, out of the frying pan, in to the fire. The UM6 outputs its orientation as Quaternions or Euler Angles !

I have read what I can find on Quaternions and I’m lost. IIUIC Quaternions are 4 real numbers that represent 3 imaginary numbers and a scalar that represent a 3D position in space. But that’s where my understanding ends. Does anyone understand Quaternions and could point me at a good site that explains it easily ? How do I get from the 4 Quaternion numbers to a 3D orientation? Ideally I want pitch, roll & yaw represented by three numbers 0-360 degrees. Can anyone point me at some code that used Quaternions for an Arduino ?

Secondly, the device also outputs Euler Angles, which again, IIUIC represents pitch, roll & yaw as three angles, however there’s a limitation of Euler Angles that they only represent a 180 deg semi-sphere, so you don’t know if you’re upside down ? Is this true ? Can I use Euler Angles and the raw Z axis accelerometer to work out if I’m upside down ? Again, any pointers to example Euler Angles code on Arduino, would be very helpful.

If anyone’s interested, here is my code so far. I have only implemented one Packet Type from the UM6 (the Quaternion) FTTB, as it’s the only one I need. But the state machine will easily expand to further packets. I’m sure there are lots of bugs in the code. This was written last night. Any constructive criticism gratefully accepted. It outputs 4 changing numbers. I have no idea if they are the right numbers ! :slight_smile:

At 19200 baud the 16Mhz Arduino seems to be keeping up with the rather fast data stream from the UM6, even with software serial ports. The unit’s slowest update rate is 20Hz !

Thanks for any comments or help.

Jon.

#include <NewSoftSerial.h>

NewSoftSerial UM6Serial(2,3);

int nState = 0;
#define STATE_ZERO         0
#define STATE_S            1
#define STATE_SN           2
#define STATE_SNP          3
#define STATE_PT           4
#define STATE_READ_DATA    5
#define STATE_CHK1         6
#define STATE_CHK0         7
#define STATE_DONE         8 

#define UM6_GET_DATA 0xAE
#define UM6_QUAT_AB  0x64
#define UM6_QUAT_CD  0x65
#define PT_HAS_DATA  0b10000000
#define PT_IS_BATCH  0b01000000
#define PT_COMM_FAIL 0b00000001

#define DATA_BUFF_LEN  16

byte aPacketData[DATA_BUFF_LEN];
int n = 0;
byte c = 0;
int nDataByteCount = 0;

typedef struct {
  boolean HasData;
  boolean IsBatch;
  byte BatchLength;
  boolean CommFail;
  byte Address;
  byte Checksum1;
  byte Checksum0;
  byte DataLength;
} UM6_PacketStruct ;

UM6_PacketStruct UM6_Packet;

void setup(){
  UM6Serial.begin(19200);
  Serial.begin(19200);
}

void loop(){

  n = UM6Serial.available();
  if (n > 0){
    c = UM6Serial.read();
    switch(nState){
      case STATE_ZERO : // Begin. Look for 's'.
        Reset();
        if (c == 's'){ //0x73 = 's'
          nState = STATE_S;
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_S : // Have 's'. Look for 'n'.
        if (c == 'n'){ //0x6E = 'n'
          nState = STATE_SN; 
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SN : // Have 'sn'. Look for 'p'.
        if (c == 'p'){ //0x70 = 'p'
          nState = STATE_SNP; 
        } else {
          nState = STATE_ZERO;
        }
        break;
      case STATE_SNP : // Have 'snp'. Read PacketType and calculate DataLength.
        UM6_Packet.HasData = 1 && (c & PT_HAS_DATA);
        UM6_Packet.IsBatch = 1 && (c & PT_IS_BATCH);
        UM6_Packet.BatchLength = ((c >> 2) & 0b00001111);
        UM6_Packet.CommFail = 1 && (c & PT_COMM_FAIL);
        nState = STATE_PT;
        if (UM6_Packet.IsBatch){
          UM6_Packet.DataLength = UM6_Packet.BatchLength * 4;
        } else {
          UM6_Packet.DataLength = 4;
        }
        break;
      case STATE_PT : // Have PacketType. Read Address.
        UM6_Packet.Address = c;
        nDataByteCount = 0;
        nState = STATE_READ_DATA; 
        break;
      case STATE_READ_DATA : // Read Data. (UM6_PT.BatchLength * 4) bytes.
        aPacketData[nDataByteCount] = c;
        nDataByteCount++;
        if (nDataByteCount >= UM6_Packet.DataLength){
          nState = STATE_CHK1;
        }
        break;
      case STATE_CHK1 : // Read Checksum 1
        UM6_Packet.Checksum1 = c;
        nState = STATE_CHK0;
        break;
      case STATE_CHK0 : // Read Checksum 0
        UM6_Packet.Checksum0 = c;
        nState = STATE_DONE;
        break;
      case STATE_DONE : // Entire packet consumed. Process packet
        ProcessPacket();
        nState = STATE_ZERO;
        break;
      }
    }
  }

void ProcessPacket(){
int DataA = 0;
int DataB = 0;
int DataC = 0;
int DataD = 0;

  PrintDebug();
  switch(UM6_Packet.Address){
    case UM6_QUAT_AB :
      if (UM6_Packet.HasData && !UM6_Packet.CommFail){
        DataA = (aPacketData[0] << 8) | aPacketData[1];
        DataB = (aPacketData[2] << 8) | aPacketData[3];
        if (UM6_Packet.DataLength > 4){
          DataC = (aPacketData[4] << 8) | aPacketData[5];
          DataD = (aPacketData[6] << 8) | aPacketData[7];
        }
      }
      Serial.print("N = ");
      Serial.print(n,DEC);
      Serial.print(" A = ");
      Serial.print(DataA,DEC);
      Serial.print(" B = ");
      Serial.print(DataB,DEC);
      Serial.print(" C = ");
      Serial.print(DataC,DEC);
      Serial.print(" D = ");
      Serial.print(DataD,DEC);
      Serial.println(".");
      break;
    Serial.println("unknown packet");
  }
}

void Reset(){
  UM6_Packet.HasData = false;
  UM6_Packet.IsBatch = false;
  UM6_Packet.BatchLength = 0;
  UM6_Packet.CommFail = false;
  UM6_Packet.Address = 0;
  UM6_Packet.Checksum1 = 0;
  UM6_Packet.Checksum0 = 0;
  UM6_Packet.DataLength = 0;
}

void PrintDebug(){
// PT = has_data=1 is_batch=1 batch=0010 (2) res=0 fail=0 
// AD = 0x64
  Serial.print("N = ");
  Serial.print(n,DEC);
  Serial.print(" HD = ");
  Serial.print(UM6_Packet.HasData,BIN);
  Serial.print(" IB = ");
  Serial.print(UM6_Packet.IsBatch,BIN);
  Serial.print(" BL = ");
  Serial.print(UM6_Packet.BatchLength,DEC);
  Serial.print(" CF = ");
  Serial.print(UM6_Packet.CommFail,BIN);
  Serial.print(" AD = 0x");
  Serial.print(UM6_Packet.Address,HEX);
  Serial.print(" CS1 = 0x");
  Serial.print(UM6_Packet.Checksum1,HEX);
  Serial.print(" CS0 = 0x");
  Serial.print(UM6_Packet.Checksum0,HEX);
  Serial.print(" DL = ");
  Serial.print(UM6_Packet.DataLength,DEC);
  Serial.println(".");
}

Quaternions are 4 real numbers that represent 3 imaginary numbers and a scalar that represent a 3D position in space.

Nope. A Quaternion is 4 numbers, 3 of which represent a vector, and the 4th represents a rotation around that vector.
The 3 vector values (x, y, and z components) have an imaginary component to them.
So the Quaternion is in the form Q = w + xi + yj + zk. W being the rotation about the vector. w, x, y, and z being real numbers, and i, j, and k being the imaginary components.
For the purpose of programming, you only need to be concerned with the real components, ie w, x, y, and z, the imaginary components can be ignored. I'll explain that more later.

however there’s a limitation of Euler Angles that they only represent a 180 deg semi-sphere, so you don’t know if you’re upside down ? Is this true ?

Not true (unless it's a specific limitation of the sensor you are using, or some arbitrary implementation of an orientation tracking system). The primary limitation of using Euler Angles for 3D positioning/orientation is what's called gimbal lock.

Euler Angles utilize three values that represent rotations about each of the reference axis, x, y, and z. So, if you think about the 'airplane' model of pitch, roll, and yaw, as the airplane pitches up and approaches 90º, the roll and yaw axis get closer and closer to being parallel, because the angles are always measured from the reference frame. At 90º of pitch, roll and yaw are parallel, and thus represent the same rotation, and you've degenerated from a 3D system to a 2D system.

It can be tough to visualize this gimbal lock, but the analogy that really helped it click for me was to think of the Longitude/Latitude system of coordinates we use for geographic locations. Think of yourself standing at some point on earth, Pitch represents Latitude, Yaw represents Longitude, and roll represents the direction you are facing at that location. As you approach the North or South Pole, your longitude and your facing gradually approach each other in value. At the North or South Pole, the Longitude value no longer has any meaning to your location/orientation. Euler Angles work in the same way.

WARNING I'm by no means a Quaternions expert, and it has been years since I've actually worked with them. The following is to the best of my knowledge, and should be mostly correct, but there may be some inaccuracies. Even with the inaccuracies, I'm confident this will get you much closer to a full understanding of Quaternions than you're currently at. Additional research (or corrections from more knowledgeable members) may show some errors in the following statements.

So, how do Quaternions solve this problem? They solve it basically by making each rotation about a defined axis. The vector portion of the Quaternion defines the axis about which the rotation is applied. And since this is an arbitrarily defined axis, the rotation will always be perfectly perpendicular to that axis.

So, back to our airplane example. With Euler Angles, the orientation of your airplane was always defined as a series of angular rotations around the axis of the reference frame. With Quaternions, your airplane orientation can be defined as a series of vectors (an orthonormal local frame), and a rotation can be made around any one of those vectors in the form of a Quaternion as opposed to being restricted to just rotations about the reference axis. The reference axis is still there (and the orientation and vector component of your Quaternions are all with respect to this reference frame), but it doesn't restrict how you rotate your object. Quaternions are actually the more intuitive way to think about orientation and how to change it. Back to the plane example, when you want to roll the plane 10º to the left, you don't have to think about your specific orientation. You push the flight stick to the left and the plane banks, or rolls, in that direction. The rotation is with respect to the orientation of the plane. You'd do the same with Quaternions. The vector component of the Quaternion would represent the orientation of your plane, and the w component would represent the 10º rotation about that vector. whether it was on a level flight heading North, or in a 5º climb heading SW, the rotation will remain the same. Only the vector changes.

Don't let the imaginary components of Quaternions intimidate you. When writing code to deal with Quaternions, you'll never actually deal with the imaginary numbers. They don't matter to the real world usage of Quaternions. They're use is primarily from the theoretical perspective, proving out the mathematics of Quaternions themselves, and perhaps understanding why the various Quaternion operations actually work from a mathematically point of view. If you don't care about understanding how the math works but only how you apply it, you can still write the code that applies the math and utilizes Quaternions for gimbal lock free 3D orientation tracking.

I'll also mention that you can also scale and translate with Quaternions, with the same benefits that are provided rotations, but there are plenty of resources out there covering all the details of utilizing Quaternions in 3D coordinate systems.

Thanks ! That makes a lot of sense, if I’ve understood it correctly. Thanks for the simple explanation.

Is the x,y,z component simply the x,y,z of the vector, or does it need to processed first ? So if (w,x,y,z) = (10,1,0,0) does it represents a rotation of 10 around a vector pointing down the x axis ? If so, what are units of the rotation ? Radian ? Degrees ? Also, where is the rotation “from” ? Is the origin always parallel from the x/y plain ? and clockwise ?

So, IIUIC :
(0,1,0,0) would be a plane flying forward (“north”), straight and level. (I know “north” is a bad analogy, as north if relative, but you know what I mean).
(0,-1,0,0) is the plane flying “south” ?
(180,0,1,0) is a the plane flying “east”, upside down ?

Thanks.

Jon.