An important part of controlling an Unmanned Air Vehicle (UAV) is having the correct states available for feedback in the governing control system. Unfortunately, sensors employed on UAVs, such as gyroscopes and accelerometers (usually packaged together and dubbed Inertial Measurement Unit (IMU)), have several problems associated with them:
- Gyroscopes measure angular rate of change and not angular position directly.
- Accelerometers measure more than just linear acceleration, for instance, gravitational acceleration and Coriolis terms.
- Measurements are noisy and biased.
- Body-frame states need to be transformed to the inertial reference frame (e.g. Euler angles).
We aim to develop a method to accurately determine the UAVs attitude (excluding yaw, since this requires an additional magnetometer) purely from the raw accelerometer and gyroscope readings.
I am using an MPU-6050 IMU (6 degrees of freedom, see Figure 1 below), which is readily available and inexpensive. It uses an interface and therefore can be easily combined with an Arduino or Raspberry Pi.
Figure 1: MPU-6050 IMU
Before we get into developing algorithms for attitude estimation, we need to define our notation and cover some sensor basics.
Throughout, we will be using the following notation:
- Ax, Ay, Az – Raw acceleration measurements along each axis ().
- Gx, Gy, Gz – Raw gyroscope measurements about each axis (), usually referred to as p, q, and r respectively.
- , – Roll angle (rotation about X axis in degrees) and pitch angle (rotation about Y axis in degrees).
- – Indicates an estimate of the roll angle.
Both sensors will have an offset (bias), which means at rest they will not read exactly zero – even though in theory they should. This can be mitigated by keeping the IMU at rest, summing the measurements over a period of time, and finally dividing by the total number of measurements. This final value – the average offset – can then be subtracted from all future readings to ensure that the actual measurement is close to zero when the system is at rest and level.
When the IMU is at rest, the accelerometer measures gravitational acceleration only. Therefore, by resolving forces using basic trigonometry, we can calculate an estimate of both pitch and roll using the raw data as follows:
We need to low-pass filter the raw accelerometer data before using the equations above, since there will be additive, high-frequency noise overlayed on the signal. Furthermore, in motion the accelerometer will measure additional acceleration terms which deteriorate our estimations for pitch and roll.
Figure 2 shows an estimate of the roll angle using only the accelerometer data, which shows the noisy and unreliable nature of this data.
Figure 2: Roll Estimate from Raw Accelerometer Data
If we low-pass filter the raw accelerometer data, for example by using a simple moving average filter, we get a much cleaner estimate. This is shown in Figure 3 below. The amplitude of the high-frequency motion however, from to , is attenuated by the low-pass filter – this is not ideal, since this will not give us the correct roll angle at higher frequencies (original estimate goes up to about 60deg, filtered estimate reaches 40deg)! As always, there is a trade-off between how well we can ‘clean up’ our signal, and how much we impact our response.
Figure 3: Roll Estimate from Filtered Accelerometer Data
The gyroscope measures angular rates of change around each axis with respect to the body frame. Assuming noise-free measurements, we could integrate these raw measurements to get the angular position relative to the body frame. Unfortunately, as usual we will have a fair amount of noise present in the gyroscope readings. If we were to integrate this, we would add up all the small errors due to noise, which then would accumulate over time and become unbounded in magnitude. This is commonly known as gyroscopic drift.
Even heavily low-pass filtering the signal will not help, as firstly this will cause a lag in the measurements and secondly will only delay the onset of drift, since we cannot completely eliminate noise in our signal. (Furthermore, integration on its own already acts as a low-pass filter, which we can best see in the Laplace domain, where integration is simply .)
Figure 4 shows an estimate of the roll angle acquired by integrating the noisy gyroscope data. The drift can be clearly seen, even after such a short period of time.
Figure 4: Gyroscopic Drift
Important: As mentioned above, the gyroscopes measure angular rates of change in the body frame. For example, imagine we start from rest and pitch up to 45 degrees, then yaw to either side by rotating 90 degrees about the yaw body frame axis – this will decrease our pitch angle in the inertial frame, simply by yawing in the body frame! Therefore, we cannot simply integrate the angular rates of change given by the gyroscope to get our roll, pitch, and yaw angles in the inertial frame. The rates of change of Euler angles depend on more than one body frame angular velocity. To get the rates of change in the inertial frame, we need to transform our states using the following matrix:
We can then integrate these transformed states – assuming zero noise that is… – to get the inertial attitude. We will use this transformation in all our sensor fusion algorithms.
3. Sensor Fusion
As shown in the previous section, both sensors available to us are far from ideal. Accelerometers are fine for when the system is not in motion and gyroscopes are fine for short periods of time, but over longer periods of time – individually – both sensors will not give reliable estimates of pitch and roll.
The principle behind sensor fusion, as the name implies, is to combine the data retrieved from two or more imperfect sensors to give us an improved and more reliable estimate of a state.
We will explore some fundamental sensor fusion techniques in this section.
3.1 Complimentary Filter
The complementary filter works by combining the desirable low-frequency characteristics of the accelerometer with the desirable high-frequency characteristics of the gyroscope. Essentially, we are performing the calculations described in Section 2.3 and 2.4, filtering the results, and then combining the two estimates to give a single improved estimate.
The method is as follows:
- Choose a constant , such that 0 < < 1. The larger , the more the accelerometer measurements are ‘trusted’. As goes to zero, we base our estimate mainly on the gyroscope measurements. A good starting point is .
- Initialise state estimate, e.g. .
- For each timestep (sampling time )…
- Retrieve raw accelerometer and gyroscope readings from IMU.
- Calculate estimate of angle from accelerometer data () using equation from Section 2.2.
- Combine this estimate with the integral of the transformed gyroscope data (see Section 2.3):
The final step (3c) is a difference equation which performs the actual filtering. The accelerometer measurements are low-pass filtered, whereas the gyroscope measurements are high-pass filtered. These signals are then combined, depending on the constant , to form the final state estimate.
The complimentary filter has many advantages. For instance, there is only one tune-able parameter (), it is easy to implement in a few lines of code and does not require a lot of processing power, and lastly it delivers satisfactory performance. This can be see in Figure 5 below, showing the complimentary filter running for two different values of .
For , the complimentary filter relies quite heavily on the accelerometer data (which we feed into the complementary filter without pre-low-pass filtering it, since that is the complimentary filter’s job!), and therefore we receive a fairly noisy output. As we lower to 0.1, we rely more on the gyroscope data. As can be seen from the plot and in comparison to only using the accelerometer for estimation (see Figure 3), the high-frequency motion is less attenuated, however the noise components are filtered out quite nicely.
Figure 5: Complimentary Filter for
3.2 Kalman Filter
You might be wondering how we best choose in the complimentary filter above. We have to take several things into account, for example, sensor noise and accuracy (accelerometer vs gyroscope) and types of motion encountered. It seems like the best choice of will vary from situation to situation, and even vary over time when running on the same system! Fortunately, there is a way of choosing filter coefficients like these optimally (mathematically at least…) – let’s take a look at the Kalman Filter which does exactly that.
The Kalman Filter is also know as a Linear Quadratic Estimator. It is a type of observer or state estimator which is optimal in the sense that it tries to minimise a quadratic cost function. Furthermore, the Kalman Filter doesn’t just take the sensor measurements into account but also the underlying dynamics of the system. I will state the Kalman Filter equations and explain how to implement them. As there is a plethora of information already available both online and in various text books, I will only give a brief explanation of what the filter is doing.
3.2.1 System Dynamics
A causal, linear, and time-invariant system can be described in state-space form using the following format (discrete time):
Where is the system’s state vector at time and is the input vector at time . A, B, and C are the system matrices: A relates the current states to the next states, B relates inputs to the next states, and C relates the system states to the measured states.
Lastly, we have additive process () and measurement () noise – both assumed to be zero-mean Gaussian processes.
Since we are going to be writing an algorithm that targets a large variety of dynamic systems, we will have to formulate our system dynamics as generally as possible. We know which IMU we will be using, but not if it’s going to be placed on a UAV, car, robot, etc. – that is down to the end-user of our software. Unfortunately, this will impact our Kalman Filter performance significantly – if we had an accurate model of our actual system dynamics, our filter performance would improve.
One way of constructing the system matrices as general as possible is to recognise the following:
- We wish to eliminate the varying gyro bias at every moment in time as this causes drift.
- We can use the transformed angular rates of changed measured by the gyros – subtracting the bias – to get an updated estimate of our angular position.
- We can form estimates of our angular position by using the accelerometers.
We then define our state vector, input vector, and measurement vector:
3.2.2 Kalman Filter Equations
Now that we have defined our system in state-space form, we can take a look at the Kalman Filter equations. These can be put into two groups: Prediction and Update.
As an analogy to the complimentary filter mentioned above, is known as the Kalman gain, which is optimally chosen depending on what the filter thinks is giving a more reliable estimate of the state – the measurement or the system dynamics model.
As shown in the previous sections, we cannot use simply one of the two sensors, as the accelerometer data is too noisy and the gyroscope data will cause drift. We achieve the greatest sensor fusion performance and easiest implementation by utilising the complementary filter, which merges both measurement sources.
If we have an accurate description of our dynamic system, we should choose the Kalman Filter instead.
An overall comparison of all state estimation schemes is given in Figure 7 below.
5. Appendix: Data and Code Listings
Raw Gyroscope and Accelerometer Data: imu_data.csv
Code Listings are available on GitHub: https://github.com/pms67/AttitudeEstimation