Integrating Gyroscopes
Gyroscopes measure angular velocity. As with regular velocity, by multiplying by time you get change in position (a distance). By adding these distances up as you go, you can estimate position. Naturally, every little error will also get added up and eventually you’ll end up with large errors. This is called drift and is the reason gyros can’t be used alone in an orientation sensing system.
To integrate angular rates from gyroscopes, you must calculate angular displacement and add this displacement to an accumulator to estimate orientation. This quaternion accumulates angular displacement from the gyros. I’ve always called it ‘q’. q represents the estimated orientation of the body.
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 |
You’ll probably know from previous reading that to ‘add’ some orientation using quaternions, you must multiply.
|
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 rates are assumed to be small, there is a faster approximation that 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
Using accelerometers and magnetometers are terribly noisy sensors. Gyroscopes are comparatively noise-free, but they only measure angular rates. Every small error is also integrated and eventually q will become very 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). This process of accumulating errors is called gyro drift. Accelerometers and magnetometers can be used to create a quaternion (accel_q) that represents the actually orientation of the body referenced to Earth, and this can be used to correct q for gyro drift.
Spherical Linear Interpolation (SLERP) can be used to calculate a point in between two quaternions. The idea is to 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.
Pingback: IMU Maths - How To Calculate Orientation - CamelSoftware