Integrating Gyroscopes – Calculating Orientation pt2
Integrating Gyroscopes
If you are not familiar with integration, read this first.
Gyroscopes measure angular velocity. Angular velocity is the rate of change of angular displacement.
You know that distance = speed * time.
Well, angular displacement = angular velocity * time
Gyroscopes can’t measure orientation with reference to the world, they can only work out angular displacement from a starting point. Additionally, because they can only do this by integrating measurements, every little error is also integrated and soon large errors will accumulate. This is called gyro drift. That is why gyroscopes alone cannot be used to measure orientation.
Calculate angular displacement and add the displacement to a quaternion. The quaternion accumulates all of the angular displacements from the gyros. I’ve always called this quaternion ‘q’, and q represents the estimated orientation.
etk::Quaternion has a function called fromAngularVelocity(). It takes angular velocity in radians per second, as well as time ( dt ) in seconds where time is the gyroscope sample rate in seconds. If you’re reading the gyros 200 times per second, dt would be 1/200=0.005s.
|
1 2 3 4 5 6 7 8 9 10 |
etk::Quarternion qw; etk::Vector<3> w = gyros.read(); float dt = 0.005; qw.fromAngularVelocity(w, dt); qw.normalize(); //ensure qw is a unit quaternion //qw represents the angular displacement //that has occured since the last gyro reading |
Multiplying two quaternions will ‘add’ the orientation of one to the other. In this case we want to apply angular displacement ‘qw’ to orientation ‘q’.
|
1 2 |
q = q * qw; //apply angular displacement 'qw' to orientation 'q' q.normalize(); //make sure q remains a unit quaternion |
The fromAngularVelocity function uses sin and cos to calculate angular displacement. If the angular displacement is small, a faster approximation can be used.
|
1 2 |
Quaternion qw(1, w.x()*dt/2.0, w.y()*dt/2.0, w.z()*dt/2.0); qw.normalize(); |
This works sufficiently well in most cases and is substantially faster than the fromAngularVelocity function.
Correcting Gyros For Drift
Accelerometers and magnetometers are noisy sensors. Gyroscopes are fairly noise-free, but they only measure angular velocity. Every small error is integrated and q will quickly become inaccurate. These errors can be due to mechanical vibration, temperature effecting gyro bias, lack of floating point precision and gyros measuring the rotation of the earth (15 deg per hour). The accumulation of errors is called gyro drift.
Accelerometers and magnetometers can be used to create a quaternion (accel_q) that represents orientation referenced to Earth, and this can be used to correct q for gyro drift.
Spherical Linear Interpolation (SLERP) is a function that is used to calculate a point in between two quaternions. Use SLERP to find a point somewhere between q and accel_q, and then move q to that point.
|
1 2 3 4 |
//get acceleration quaternion Quaternion accel_q = get_accel_mag_rotation(state, dt); //use SLERP to move q closer to accel_q q = q.slerp(accel_q, ORIENTATION_FILTER_GAIN); |
ORIENTATION_FILTER_GAIN is a value between 0 and 1 which determines how much slerp should move q. If it’s 0, q won’t change. If it’s 1, q will become equal to accel_q every time. The ideal value will depend on how quickly q drifts and how noisy accel_q is. A reasonable starting point is 0.05.
2 thoughts on “Integrating Gyroscopes – Calculating Orientation pt2”