Posted by JibbSmartJibbSmart on 26 Jul 2021 02:43

Whether for leaning, steering, absolute orientation calculations, or for world/player-space gyro controls, it can be useful to know which way gravity is pointing. Given that standard modern controllers all have 3-axis gyroscopes and 3-axis accelerometers (we see this basically everywhere except Xbox, which has fallen behind), we will use these sensor inputs to figure out which way is "down".

Here's a quick explanation of why we want both these sensors working together.

As you can see in that example, just using the accelerometer alone often gives a decent approximation of the gravity direction. But it's also very prone to interference from other movement during regular play. Here, we'll start with a super simple sensor fusion solution that any game should be able to use easily — it's the same four-liner I shared in Player Space Gyro (and Alternatives) Explained. It's not perfect, but it's surprisingly robust given how simple it is. Then we'll explore a more fancy solution. As we do, it's a good idea to have this working alongside it to compare. Our fancy solution should be at least this good.

Something Simple

Vec3 GravityVector;
 
Vec3 CalculateGravitySimple(Vec3 gyro, Vec3 accel) {
   // convert gyro input to reverse rotation
   Quat reverseRotation = AngleAxis(gyro.Length() * DeltaSeconds, -gyro.X, -gyro.Y, -gyro.Z);
 
   // rotate gravity vector
   GravityVector *= reverseRotation;
 
   // nudge towards gravity according to current acceleration
   Vec3 newGravity = -accel;
   GravityVector += (newGravity - GravityVector) * 0.02;
}

We're going to work our way up from this simple solution to the one I have in GamepadMotionHelpers — a free and open source library to help with gyro controls, calibration, and gravity calculation. It's used in JoyShockMapper, and I separated it from JSM and JSL so that I could access the same functionality in multiple projects, regardless of how I'm getting controller info (SDL, platform-specific SDKs, etc).

There's probably no one-size-fits-all gravity solution. Different applications will have different needs. But here were my needs when I implemented this for JoyShockMapper:

  • The gravity vector should be steady when the controller is held steady;
  • The gravity vector should update quickly, without smoothing, so it can be used for responsive lean and motion stick inputs;
  • Corrections should be applied gradually, without sudden jumps;
  • But we should apply corrections quickly enough that errors are kept very small.

That sounds pretty reasonable, right? Now, let's break down our simple sensor fusion example line by line:

Quat reverseRotation = AngleAxis(gyro.Length() * DeltaSeconds, -gyro.X, -gyro.Y, -gyro.Z);

Convert gyro input into an angular displacement. This quaternion represents how much and in what direction the controller has rotated since the last update. But we invert the axis, because we're going to use the inverse of this rotation.

GravityVector *= reverseRotation;

Apply that reverse rotation to our last calculated GravityVector. This means we're re-using the previous update's gravity direction and accounting for any rotating the controller did in the meantime. To visualise this, if I were to point directly North and then rotate my whole body 35° to the right, I'd have to rotate my arm 35° in the opposite direction for it to still be pointing North. The gyro input gives us how much the controller has rotated, and so we rotate our gravity direction the same amount in the opposite direction to keep it pointing the same way.

Vec3 newGravity = -accel;

If we didn't have the previous gravity vector to work with, our best guess at the current gravity direction would be the opposite of the acceleration detected by the accelerometer. This is because when the controller is held perfectly still, there's still an upwards acceleration applied to the controller counteracting gravity. The accelerometer detects that upwards acceleration.

GravityVector += (newGravity - GravityVector) * 0.02;

The accelerometer doesn't just detect gravity. It'll also detect other linear acceleration as well (shaking the controller, for example). Here we try to filter out those linear shakes. We're guessing that the actual gravity direction is somewhere between our reverse-rotated previous best guess at the gravity direction and the current acceleration detected by the accelerometer. That's what the magic 0.02 is for. You can set it to anything between 0 and 1.

  • 0 means the accelerometer input is ignored, and we're just relying on the gyro input to rotate our gravity vector correctly. Noise and rounding errors will accumulate with nothing to counter them.
  • 1 means the gyro input and the previous frame's gravity vector are ignored, and we're just assuming the accelerometer input represents the gravity direction. It'll often be right, but even subtle bumps and shakes of the controller will mess with that.
  • 0.02 means we are mostly trusting the gyro input, but we're always pushing it a little towards the accelerometer to avoid accumulating too much error. Increasing this number means less error accumulates over time from rounding errors, but we get more interference from moment-to-moment bumps and shakes.

0.02 isn't particularly special. In fact, it's unsatisfying to do it like this — it means the error correction rate is tied to the update rate (possibly your game's frame-rate, if that's how often you're doing these updates). But in practice this quick and dirty solution works pretty darn well, and it'll be good to keep this function on hand as we try to do something fancier. We can compare results between this Simple function and our new Fancy function and see if our Fancy function is actually doing a good job.

Something Fancy

The last line of our simple solution is where we decide how quickly we move from the gravity direction we're just remembering (and un-rotating) from previous updates to just using the latest accelerometer vector (inverted). Our correction rate is just a constant 0.02 * the distance between our remembered gravity direction and our inverted acceleration.

For our fancy solution, all we're going to do is try and be smarter about choosing that correction rate. Everything else remains the same. So what do we want to take into consideration when choosing a good correction rate?

  1. If the accelerometer input is changing a lot over time, the controller is probably shaking or being bumped. Trust the accelerometer less and decrease the correction rate.
  2. If the accelerometer input is very steady, it's probably giving us our gravity vector very accurately. Use a high correction rate to accept that new vector quickly.
  3. Seeing the gravity vector change when the controller isn't turning is jarring. We can hide corrections by keeping them in small proportion to the controller's turn rate.
  4. If there's a big difference between our remembered gravity vector and a trusted accelerometer gravity vector, we need to make corrections quickly.

Sure, sometimes these considerations will be in tension with each other. For example, points 3 and 4 can be in tension when the controller is being held still but our remembered gravity vector is clearly very wrong. But it's not hard to make good decisions about how to resolve these. In this case, number 4 is the more important issue, and will override number 3.

So, before we start implementing these considerations, let's make room for them like so:

Vec3 GravityVector;
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // convert gyro input to reverse rotation
   Quat reverseRotation = AngleAxis(gyro.Length() * DeltaSeconds, -gyro.X, -gyro.Y, -gyro.Z);
 
   // rotate gravity vector
   GravityVector *= reverseRotation;
 
   // nudge towards gravity according to current acceleration
   Vec3 newGravity = -accel;
   Vec3 gravityDelta = newGravity - GravityVector;
 
   float correctionRate = ??
 
   GravityVector += gravityDelta.Normalized() * correctionRate;
}

Now we just need to fill in the ??.

Let's start with considerations 1 and 2 from above. We need to know how shaky the accelerometer input has been. Here's an easy way to do this. Keep record of a smoothed out version of the accelerometer input. The shakier the accelerometer input is, the more the smoothed and unsmoothed inputs will differ. But sometimes a shaky input will happen to be near the smoothed input, so we keep track of the biggest shakiness we've had so far and slowly reduce it over time.

For example, if we were to rapidly shake the controller, we might get the following accelerometer inputs in one axis: 2, 0, -2, 0, 2, 0, -2, 0… and so on (unrealistically consistent values chosen for simplicity). The smoothed accelerometer input will stay close to 0, because that's the rolling average here. When we get a 2 or -2 raw accelerometer input, these differ quite a bit from the smoothed acceleration, and we know the controller is being shaken. Our current "shakiness" value must be at least 2. But between every 2 and -2 in this example is a 0. That 0 is very close to the smoothed value, but not because the controller is being held still. It's just on its way from one extreme to another, passing the smoothed value along the way. If we remember that our most recent "shakiness" value is 2, we know this momentary 0 doesn't mean we're not shaking anymore. We can decrease our "shakiness" value a little just in case we continue to get more 0s or near-0s in a row. But if the next value is another 2 or -2, we will bump up the "shakiness" to 2 again.

So we want an easy, low-effort way to track the smoothed acceleration input and a way to pull our "shakiness" towards 0 over time.

One way to do smoothing is to interpolate between our last smoothed value and our latest raw value. In fact that's exactly what the last line of our super simple gravity function is doing — interpolating between our old GravityVector and our newly calculated newGravity where the interpolation factor is 0.02. But we want a frame-rate independent version. We want it to behave basically the same whether we're updating at super high rates (500 Hz or more), super low rates (30 Hz or less), or anything in between.

This is where the exp2 function comes in. Its value in this context is in figuring out how far to move from one value to another at the same rate regardless of our frame-rate. See it properly explained in this blog post on Gamasutra.

We can say, "Hey, I want to move value A to value B such that the gap between the two values is halved every second." That will look like this:

A = Lerp(B, A, exp2(-DeltaSeconds));

Too slow? "Let's have the gap close by half every quarter second."

A = Lerp(B, A, exp2(-DeltaSeconds / 0.25));

Having done that, we can use the same interpolation factor for our smoothing function and for reducing our "shakiness" value:

float Shakiness;
Vec3 SmoothAccel;
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // ...
 
   SmoothAccel *= reverseRotation;
   float smoothInterpolator = exp2(-DeltaSeconds / 0.25);
   Shakiness *= smoothInterpolator;
   Shakiness = max(Shakiness, (accel - SmoothAccel).Length());
   SmoothAccel = lerp(accel, SmoothAccel, smoothInterpolator);
 
   // ...
}

SmoothAccel is our smoothed acceleration value. When the player shakes or bumps the controller, the raw accel input will be very different from the smoothed acceleration. But before adding to our smoothed acceleration, we first update its direction, reverse-rotating it by however much the controller rotated since the last update (just like we do with the GravityVector). If we didn't do that, rotating the controller would look like shakiness, because the direction of the raw acceleration would differ from the direction of the smoothed acceleration.

We're putting our exp2(-DeltaSeconds…) in a local variable smoothInterpolator so we can smooth our acceleration input and pull back our max shakiness at the same rate — in this case, halving the value every quarter of a second. Why are we pulling Shakiness back to 0 with interpolation and not just decreasing it at a fixed rate? Well, if we get an unexpectedly large input for one frame for whatever reason, bringing that down at a fixed rate could take a long time. Interpolating towards 0 like this means bigger values decrease very quickly, so we never have to wait long to recover from an unexpectedly large bump. Interpolating towards 0 is just done by multiplying our most recent value by smoothInterpolator. Easy!

Now that we've calculated our Shakiness, we're just about ready to figure out how it should affect our correction rate. But first, we're going to need to introduce some settings that we can tune.

// the time it takes in our acceleration smoothing for 'A' to get halfway to 'B'
float SmoothingHalfTime = 0.25;
 
// thresholds of trust for accel shakiness. less shakiness = more trust
float ShakinessMaxThreshold = 0.4;
float ShakinessMinThreshold = 0.01;
 
// when we trust the accel a lot (the controller is "still"), how quickly do we correct our gravity vector?
float CorrectionStillRate = 1;
// when we don't trust the accel (the controller is "shaky"), how quickly do we correct our gravity vector?
float CorrectionShakyRate = 0.1;

We've moved that magic "/ 0.25" that we're using when we call exp2() for our frame-rate independent interpolation factor into a setting here: SmoothingHalfTime.

I got most of these other values by a bit of trial and error. Feel free to experiment. But for ShakinessMinThreshold and ShakinessMaxThreshold, I just printed the Shakiness value to the screen every update and watched how it changed when holding the controller as still as I could or started moving it around. ShakinessMinThreshold is intended to be the shakiness we get when holding the controller perfectly still. ShakinessMaxThreshold is the shakiness we get when shaking the controller around just enough that the detected acceleration can't be trusted to give us a decent gravity vector. That's much more open to interpretation, but I've found this value to work well.

My values will probably work well for you if you're using the same units that I am. Some controllers / platforms will give you acceleration in metres per second squared, so you'd expect GravityVector to usually be around 9.8. Others will give you acceleration in g units, which is what the datasheets for most IMUs use. That's what I use. What that means is that your GravityVector will usually have a length of roughly 1, which has useful consequences we can use later. But if you're using different units, that's fine. You can probably adjust most of these settings accordingly (multiply them by 9.8).

Anyway, let's use our shakiness thresholds to get a number from 0-1 rating the controller's shakiness — where 0 means "still" and 1 means "shaky". Then we'll choose our correctionRate to be somewhere between CorrectionStillRate and CorrectionShakyRate depending on that 0-1 number. This is how that looks:

// settings and state...
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // ...
 
   float smoothInterpolator = exp2(-DeltaSeconds / SmoothingHalfTime);
 
   // ...
 
   Vec3 gravityDelta = -accel - GravityVector;
   Vec3 gravityDirection = gravityDelta.Normalized();
   float correctionRate;
   if (ShakinessMaxThreshold > ShakinessMinThreshold) {
      float stillOrShaky = clamp((Shakiness - ShakinessMinThreshold) / (ShakinessMaxThreshold - ShakinessMinThreshold), 0, 1);
      correctionRate = CorrectionStillRate + (CorrectionShakyRate - CorrectionStillRate) * stillOrShaky;
   } else if (Shakiness > ShakinessMaxThreshold) {
      correctionRate = CorrectionShakyRate;
   } else {
      correctionRate = CorrectionStillRate;
   }
 
   // apply correction
   Vec3 correction = gravityDirection * (correctionRate * DeltaSeconds);
   if (correction.LengthSquared() < gravityDelta.LengthSquared()) {
      GravityVector += correction;
   } else {
      GravityVector += gravityDelta;
   }
 
   // ...
}

There's a little complexity added just in case someone sets your ShakinessMaxThreshold to be less than or equal to ShakinessMinThreshold, which is nonsensical (and would cause divide by zero if they're equal!). In that case, we just use the ShakinessMaxThreshold to choose between CorrectionStillRate and CorrectionShakyRate. Speaking of safety, I'm assuming your Normalize() is safe to use even when the vector is all zeroes (it's common to just return a zero vector in that case). If not, add appropriate protections here.

Once we have our correctionRate, we turn it into a correction vector. We also check if that permitted correction is more than we need so we don't overshoot our desired gravity vector.

If you're following along with your own code, you can give this a go. It should work pretty well! In fact, if you are using your gravity vector in a way where sudden corrections won't be jarring to the player (for example, for gyro aiming), you could comfortably stop here. But there's still some shakiness with the gravity vector, even when holding the controller nearly still. This is where consideration number 3 comes in: "We can hide corrections by keeping them in small proportion to the controller's turn rate."

Let's say we're limiting our correction rate to 10% of the controller's rotation rate. That means if the controller is turning at 100 degrees per second, we can correct the gravity vector by 10 degrees per second. Since that correction is always much smaller than the controller's overall rate of rotation, corrections don't feel intrusive, and don't happen while holding the controller still.

But! We're not correcting our gravity direction by angle. And converting between vectors and angles is slow and messy, so let's not. If we convert our gyro input to radians per second (some platforms / libraries will give you radians per second anyway, while others will use degrees per second), that gives us the linear velocity of a point 1 unit away from the controller's axis of rotation.

This gives us a very simple calculation for limiting our correction rate:

// settings and state...
// limit further corrections to this proportion of the rotation speed
float CorrectionGyroFactor = 0.1;
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // ...
 
   // my input library has the gyro report degrees per second, so convert to radians per second here
   float angleRate = gyro.Length() * PI / 180;
 
   // ...shakiness and correctionRate stuff goes here...
 
   float correctionLimit = angleRate * GravityVector.Length() * CorrectionGyroFactor;
   correctionRate = min(correctionRate, correctionLimit);
 
   // apply correction
   // ...
}

Wasn't that easy? Now, in GamepadMotionHelpers I omit the GravityVector.Length() part, because that library uses g units and so I expect the gravity vector's length to be roughly 1 anyway.

Okay, we're nearly there. Now we need to account for consideration number 4: "If there's a big difference between our remembered gravity vector and a trusted accelerometer gravity vector, we need to make corrections quickly." If we have a large error but our controller is at rest, this change we just made prevents any corrections from being made! So we should only make full use of CorrectionGyroFactor when we believe there's very little correcting needed anyway. Here's how I do that:

// settings and state...
// limit further corrections to this proportion of the rotation speed
float CorrectionGyroFactor = 0.1;
 
// thresholds for what's considered "close enough"
float CorrectionGyroMinThreshold = 0.05;
float CorrectionGyroMaxThreshold = 0.25;
 
// no matter what, always apply a minimum of this much correction to our gravity vector
float CorrectionMinimumSpeed = 0.01;
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // ...
 
   // my input library has the gyro report degrees per second, so convert to radians per second here
   float angleRate = gyro.Length() * PI / 180;
 
   // ...shakiness and correctionRate stuff goes here...
 
   float correctionLimit = angleRate * GravityVector.Length() * CorrectionGyroFactor;
   if (correctionRate > correctionLimit) {
      float closeEnoughFactor;
      if (CorrectionGyroMaxThreshold > CorrectionGyroMinThreshold) {
         closeEnoughFactor = clamp((gravityDelta.Length() - CorrectionGyroMinThreshold) / (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold), 0, 1);
      } else if (gravityDelta.Length() > CorrectionGyroMaxThreshold) {
         closeEnoughFactor = 1;
      } else {
         closeEnoughFactor = 0;
      }
      correctionRate = correctionRate + (correctionLimit - correctionRate) * closeEnoughFactor;
   }
 
   // finally, let's always allow a little bit of correction
   correctionRate = max(correctionRate, CorrectionMinimumSpeed);
 
   // apply correction
   // ...
}

CorrectionGyroMinThreshold and CorrectionGyroMaxThreshold have been tuned assuming GravityVector will be roughly of length 1, so you'll need to adjust these if you're using different units. Basically, if the difference between our target gravity vector and GravityVector is small enough (at or below CorrectionGyroMinThreshold), we consider the error small enough that we'll limit corrections to CorrectionGyroFactor times the gyro input (in this case, 10% of the rotation rate from the gyro). If the error is too large (at or above CorrectionGyroMaxThreshold), we don't want to limit the correction rate at all. And if it's somewhere in between, we apply that limit more softly, avoiding hard transitions.

Then right at the end we've added one more setting for you to tune: CorrectionMinimumSpeed. I think that even when the controller is perfectly still and the error isn't large, it's nice to still be doing at least a tiny bit of correction.

So finally, here's everything put together:

// SETTINGS
// the time it takes in our acceleration smoothing for 'A' to get halfway to 'B'
float SmoothingHalfTime = 0.25;
 
// thresholds of trust for accel shakiness. less shakiness = more trust
float ShakinessMaxThreshold = 0.4;
float ShakinessMinThreshold = 0.01;
 
// when we trust the accel a lot (the controller is "still"), how quickly do we correct our gravity vector?
float CorrectionStillRate = 1;
// when we don't trust the accel (the controller is "shaky"), how quickly do we correct our gravity vector?
float CorrectionShakyRate = 0.1;
 
// if our old gravity vector is close enough to our new one, limit further corrections to this proportion of the rotation speed
float CorrectionGyroFactor = 0.1;
// thresholds for what's considered "close enough"
float CorrectionGyroMinThreshold = 0.05;
float CorrectionGyroMaxThreshold = 0.25;
 
// no matter what, always apply a minimum of this much correction to our gravity vector
float CorrectionMinimumSpeed = 0.01;
 
// STATE
float Shakiness;
Vec3 SmoothAccel;
Vec3 GravityVector;
 
Vec3 CalculateGravityFancy(Vec3 gyro, Vec3 accel) {
   // convert gyro input to reverse rotation
   Quat reverseRotation = AngleAxis(gyro.Length() * DeltaSeconds, -gyro.X, -gyro.Y, -gyro.Z);
 
   // rotate gravity vector
   GravityVector *= reverseRotation;
   SmoothAccel *= reverseRotation;
   float smoothInterpolator = exp2(-DeltaSeconds / SmoothingHalfTime);
   Shakiness *= smoothInterpolator;
   Shakiness = max(Shakiness, (accel - SmoothAccel).Length());
   SmoothAccel = lerp(accel, SmoothAccel, smoothInterpolator);
   // ...
 
   Vec3 gravityDelta = -accel - GravityVector;
   Vec3 gravityDirection = gravityDelta.Normalized();
   float correctionRate;
   if (ShakinessMaxThreshold > ShakinessMinThreshold) {
      float stillOrShaky = clamp((Shakiness - ShakinessMinThreshold) / (ShakinessMaxThreshold - ShakinessMinThreshold), 0, 1);
      correctionRate = CorrectionStillRate + (CorrectionShakyRate - CorrectionStillRate) * stillOrShaky;
   } else if (Shakiness > ShakinessMaxThreshold) {
      correctionRate = CorrectionShakyRate;
   } else {
      correctionRate = CorrectionStillRate;
   }
 
   // limit in proportion to rotation rate
   // my input library has the gyro report degrees per second, so convert to radians per second here
   float angleRate = gyro.Length() * PI / 180;
   float correctionLimit = angleRate * GravityVector.Length() * CorrectionGyroFactor;
   if (correctionRate > correctionLimit) {
      float closeEnoughFactor;
      if (CorrectionGyroMaxThreshold > CorrectionGyroMinThreshold) {
         closeEnoughFactor = clamp((gravityDelta.Length() - CorrectionGyroMinThreshold) / (CorrectionGyroMaxThreshold - CorrectionGyroMinThreshold), 0, 1);
      } else if (gravityDelta.Length() > CorrectionGyroMaxThreshold) {
         closeEnoughFactor = 1;
      } else {
         closeEnoughFactor = 0;
      }
      correctionRate = correctionRate + (correctionLimit - correctionRate) * closeEnoughFactor;
   }
 
   // finally, let's always allow a little bit of correction
   correctionRate = max(correctionRate, CorrectionMinimumSpeed);
 
   // apply correction
   Vec3 correction = gravityDirection * (correctionRate * DeltaSeconds);
   if (correction.LengthSquared() < gravityDelta.LengthSquared()) {
      GravityVector += correction;
   } else {
      GravityVector += gravityDelta;
   }
}

There it is! We now have a good approximation of where gravity is pointing in the controller's space. When I was implementing it myself, I had CalculateGravitySimple running alongside it to compare (make sure different versions of your CalculateGravity* function aren't updating the same GravityVector and interfering with each other). This helped me tune the settings to something that worked well, with pleasing looking steadiness when the controller is steady, without letting the error ever get out of hand even with prolonged shaking and rotating the controller. I highly recommend doing this yourself, just printing to the screen the angle between the gravity vector calculated the simple way and calculated the fancy way.

Conclusion

Now, what can you do with your gravity vector? Maybe you want to implement very robust gravity-aware gyro aiming. Maybe you want to lean the controller to steer a vehicle. If you're trying to match an in-game object's orientation to the controller's orientation, you can use this to counter rounding errors accumulated in the rotation over time.

An accurate gravity vector isn't just useful for dealing with orientations. If you add the GravityVector to your accel input, the gravity portion of acceleration gets cancelled out, and you can use that result to detect intentional shaking of the controller in any axis you choose. This is why we don't normalize the gravity vector at the end — we've got something that better represents the direction and strength of gravity as detected by the accelerometer, and is important for separating deliberate shakes from the constant acceleration of holding the controller still:

Shake = GravityVector + accel;

So there you have it! This sensor fusion solution has been working well for me and users of GamepadMotionHelpers. And implementing it yourself isn't terribly complicated. But if you'd rather have something you can just drop into your project, GamepadMotionHelpers is there. Currently, it's a single .hpp file you can drop into your C++ project. You can look at JoyShockMapper to see how it's used.

Feel free to find me on Twitter if you have any questions or feedback. Thanks!