IMU Filter Michael Asher Emmanuel Malikides November 5, 2011 Abstract Despite the ubiquitousness of GPS devices, on board inertial navigation remains important. An IMU like the Sparkfun Ultimate IMU used, contains all the necessary sensors for inertial navigation. These sensors, particularly gyroscopes, are subject to error. An algorithm combining different modalities (acceleration, rotation, and the earth s magnetic field) to filter this error is developed. The possibility of parallelising the algorithm for implementation on a Field Programmable Gate Array is explored. Background Quaternions The issue of the updating and storing an attitude in 3 dimensional space was addressed through use of quaternions. Rotations can be stored as the a set of three rotations, each around a coordinate axis. However, when one of the angles approaches π 2, separate solutions for the other two angles for a given rotation cannot be found as their axes are parallel. To avoid this problem, known as gimbal lock, quaternions can be used to represent rotations. Quaternions are also more numerically stable than other possible representations. Quaternions can be described as extended complex numbers q = w + ix + jy + kz, with i 2 = j 2 = k 2 = ijk = 1 and w, x, y, z R. They can be computationally represented as quadruples of real numbers q = [x, y, z, w]. They have the following operations. Addition q + q = [v + v, w + w ]. Multiplication qq = [v v +wv +w v, ww v v ]. Norm N(q) = qq = w 2 +v v. Conjugate q = [v, w] = [ v, w]. Unit quaternions form a universal covering of the 3D rotations, capturing all their geometry, topology and group structure. This is represented through writing unit quaternions in the form q = [x, y, z, w] = [ v sin(θ), cos(θ)] for some vector v and angle θ. Now let p = [ v p, w p ] be some quaternion. Then conjugation of p by q represents a rotation by 2θ about v. These rotations can be composed through multiplication, as the rotation formed from first rotating with q 1 then q 2 is given by q 2 q 1 1
Equations of Motion for Inertial Navigation [TW04] An important aspect of navigation is the reference frame in which readings are taken. The two frames of reference we will refer to in this report are the navigation frame (denoted by subscript n), which has origin at the navigation system s location and axes along north, east and down, and the body frame (denoted by subscript b), which has origin at the location of the navigation system and axes aligned with roll, pitch and yaw of vehicle. Roll, pitch and yaw refer to counter clockwise angles of rotation about the x, z and y axes respectively. Navigation involves knowing the position p, represented as a vector from the original to the present origin in the navigation frame, and the posture q which is represented as a rotation which takes the axes in the body frame to those in the navigation frame. The available sensors give readings for the linear acceleration, f b, and angular velocity, ω b, of the system, and ultimately allow the estimation of position velocity and posture of the sensor. The angular velocity reading given by the gyroscopes is given by: ω b = d θ dt b Thus by integrating over one timestep we can find the change in angle about each θ, which defines a rotation r b that represents the change in attitude for that timestep in the body frame. Changing this rotation into the navigation frame, assuming the current attitude q is correct gives: r n = qrq 1 We can then update the attitude, q = r n q = qrq 1 q = qr. Now let g b denote the acceleration due to gravity, and a b the acceleration of the sensor without gravity. We have a b = g b + f b q a b = q g b + q f b a n = g n + q f b Now g n is a constant. So the above gives us a n. We also know that a n = d v n dt n = d2 p n dt 2 n so integrating twice gives the position p n. Hardware An inertial measurement unit (IMU) contains the sensors necessary for inertial navigation, at a minimum, 3 gyroscopes and 3 accelerometers with each set arranged orthogonally. The device used, Sparkfun s Ultimate IMU, also contains 3 magnetometers which can be used to filter the signal as detailed below. Sparkfun s Ultimate IMU, the IMU used, contains the following sensors. 2
Analog Devices 3-Axis ±2 to 16g Digital Accelerometer ADXL345 The sensor is a polysilicon surface-micromachined structure built on top of a silicon wafer. Polysilicon springs suspend the structure over the surface of the wafer and provide a resistance against acceleration forces. Deflection of the structure is measured using differential capacitors that consist of independent fixed plates and plates attached to the moving mass. Acceleration deflects the beam and unbalances the differential capacitor, resulting in a sensor output whose amplitude is proportional to acceleration. Phase-sensitive demodulation is used to determine the magnitude and polarity of the acceleration. Honeywell 3-Axis Digital Compass IC HMC5843 The HMC5843 utilizes Anisotropic Magnetoresistive (AMR) to measure magnetic fields from tens of micro-gauss to 6 gauss. The Honeywell HMC5843 magnetoresistive sensor circuit is a trio of sensors and application specific support circuits to measure magnetic fields. With power supply applied, the sensor converts any incident magnetic field in the sensitive axis directions to a differential voltage output. The magnetoresistive sensors are made of a nickel-iron (Permalloy) thin-film and patterned as a resistive strip element. In the presence of a magnetic field, a change in the bridge resistive elements causes a corresponding change in voltage across the bridge outputs. These resistive elements are aligned together to have a common sensitive axis that will provide positive voltage change with magnetic fields increasing in the sensitive direction. Because the output only is in proportion to the one-dimensional axis (the principle of anisotropy) and its magnitude, additional sensor bridges placed at orthogonal directions permit accurate measurement of arbitrary field direction. InvenSense ITG-3200 3-Axis gyro IC The sensor consists of three independent vibratory MEMS gyroscopes, which detect rational rate about the x (roll), y (pitch) and z (yaw) axes. When the gyros are rotated about any of the sense axes, the Coriolis Effect causes a deflection that is detected by a capacitive pickoff. The resulting signal is amplified, demodulated, and filtered to produce a voltage that is proportional to the angular rate. This voltage is digitized using the individual on-chip 16-bit ADCs to sample each axis. The basic principle of operation of such sensors is that the vibratory motion of part of the instrument creates an oscillatory linear velocity. If the sensor is rotated about an axis orthogonal to this velocity, a Coriolis acceleration is induced. This acceleration modifies the motion of the vibrating element and provided that this can be detected, it will indicate the magnitude of the applied rotation. [TW04] Magnetometer Calibration [RAL10] Antisotropic Magnetoresistive (AMR) sensors like the Honeywell model used have a number of sources of error. There is a offset error which is constant for the lifespan of the device. Four identical elements make up the sensor, and this error is due to difficulty in depositing the permalloy of which they are made evenly and of the same density. Sensitivity error refers to the nonlinear variation in the magnitude of the sensed magnetic field. A cross axis 3
sensitivity error occurs becaause over time, the sensor obtains uneven magnetization. While the errors above are deterministic, sensor measurement noise is a stochastic process. Lastly magnetic field specific errors result from magnetic perturbations in the vicinity of the sensor. These can be further classified into hard iron (permanent field independent of Earth s) and soft iron (induced field). These errors can be modelled mathematically as follows. The three instrumentation errors; offset, sensitivity and cross axis can be modelled by a bias b so, scale factor s and transformation M respectively. b = [ b sox b soy b soz ] T S = diag (s x s y s z ) M = [ɛ x ɛ y ɛ z ] 1 where ɛ x, ɛ y, ɛ z represent the sensors x, y, z axes in the body frame Magnetic deviation errors can be represented as a hard iron bias b hi = [ b hix b hiy b hiz ] T and a 3 3 matrix for soft iron errors which impact both the intensity and the direction of the sensed field. A complete error model for true heading h, magnetometer readings ĥ and error e is then, ĥ = SM(A si h + b hi ) + b so + e. For descriptive purposes we introduce, A = SMA si b = SMb hi + b so. So ĥ = Ah + b + e. (1) With only instrumental errors, the norm of the magnetometer reading should be equal to the magnitude of the Earth s magnetic field and so the locus described by its readings should be a sphere with centre equal to the bias in the body frame. When all errors are considered, Renaudin et. al [RAL10] show that the locus is an ellipsoid and that (1) can be solved using an adaptive least squares algorithm. Rotating the Ultimate IMU in place and plotting the magnetometer reading confirms this. See the below figure. Calibration as described by Renaudin was not implemented but a heuristic approach was attempted. 4
Kalman Filtering The standard approach to this problem involves the application of a Kalman filter. The Kalman filter functions, in essence, by keeping track of the state in question, predicting the state at a future timestep through some linear model, then combining the subsequent measurement of the state with the prediction via a weighted sum. More formally, the filter addresses the problem of tracking systems that can be represented by the following linear stochastic difference equation: x k = Ax k 1 + Bu k 1 + w k 1 Where x R n is the state vector at time k, u k 1 R l is an optional control input, A is a matrix representing the linear model that updates the state given the previous state, B is a matrix that relates the control input to the state and w k 1 is the process noise. This is accompanied by some measurement process: z k = Hx k + v k Where z k R m is the measurement vector, H is a m n matrix that predicts the next measurement given the current state vector, and v k is again some noise term, called the measurement noise. Crucially the noise terms are assumed to be gaussian, with distributions p(w) N(0, Q) and p(v) N(0, R). Here Q and R are called the process noise covariance, and measurement noise covariance,respectively. With the state defined as above, given some initial state vector x k, and measurement vector z k the Kalman filter combines the two through the update: 5
x + k = x k + K(z k Hx k ) where the superscript notation indicates x + k is the updated estimate of the state. Recall that the matrix H predicts the measurement given the previous state, thus the term (z k Hx k ) represents the difference between the prediction and the actual measurement. This is weighted with some matrix K. K, termed the kalman gain matrix, is determined to minimise the error covariance of x + k. This is roughly equivalent to minimising the expected magnitude of the error in the updated prediction. The result is given by: K = P k H T HP k H T + R The trouble with this technique is that one must find the covariance matrices. This is typically a difficult problem and the usually approached by guessing, or tuning until it works. Moving Average Filters Filters are processes that remove information from a signal. A moving average filter is a kind of low pass filter removes high frequency components from a signal. This is useful for removing random noise in measurements, which is often higher frequency than the underlying signal of interest. The moving average filter functions by performing a weighted sum over a fixed number of past samples. Let x(t) denote the input signal at timestep t, y(t) the output, and m the number of samples stored. Then the following is the result of the moving average at any time t: y(t) = 1 (x(t) + x(t 1) +... + x(t m + 1)) m An important property of filters in the time domain is their step response. This is the output of the filter when the input is a step function. It is desirable to have a fast step response, an output that follows the input step function very closely. This is because it is desirable to have a filter that responds quickly to changes in the signal, while still filtering out the noise. The moving average filter is optimal in the sense that sense that it reduces random noise most efficiently while retaining a sharp step response. It is also simple, and can be implemented recursively, making it efficient compared to alternate filters. A variant of the moving average filter termed, for the purpose of this report, the weighted moving average filter gives preference to more recent data with the justification that it is more likely to represent the true state of the system, because it is strongly time-dependant. This can be simply implemented by changing the weights of each element of the sum. Let a (0, 1). The normalised weighted moving average filter output is given by: y(t) = 1 a 1 a m (x(t) + ax(t 1) + a2 x(t 2) +... + a m 1 x(t m + 1)) a can now be tuned to by decreasing it to give faster response (higher weighting for more recent data), but less smoothing. 6
On an FPGA, however, the storage of previous inputs can be difficult. In this case the filter can be recursively implemented as: 1 y(t) = (x(t) + ay(t 1)) 1 + a Called an infinite impulse response filter, this results in similar behaviour to the weighted moving average with the exception that there is no bound on the number of previous data points incooperated into the sum. This means there is less flexibility when choosing a weight, because weights too close to 1 will result in a slow response, when compared with the moving average filter, where the number of past samples included is explicitly truncated. That is, the infinite impulse response filter sacrifices fast step response for fast computation time and less memory usage. Filtering Algorithm The algorithm developed aimed to overcome random error and long-term bias measurements in the accelerometer, gyroscope and compass measurements. The method by which each error was accounted for is detailed below. Random error in gyroscope, accelerometer and magnetometer This type of error was accounted for in all three cases with a recursive infinite impulse response filter. The weightings were determined by experiment, though bounds can be estimated as done for the acceleration, and subject to the assumptions of the application at hand. Gyro bias Tests demonstrated that the bias and random noise in the gyroscopes were the most significant, in the sense that they had the lowest signal to noise ratio. Thus the removal of this error was one of the prime goals of this design. The bias was accounted for by using the accelerometer and magnetometer readings to obtain an estimate of the orientation of the sensor. This estimate was then compared with that given by the integrated gyroscope signal, and updated according to a simple weighted sum: q = E M E g q Where q is the attitude, E g is a quaternion representing the error in the orientation as determined from the gravitational vector and E M is a quaternion rotation determined from the magnetometer reading. The quaternion E M was determined at each time-step by comparing the acceleration vector in the inertial frame as estimated by the updated attitude from the gyroscope reading with the initial stored acceleration reading. Assuming that the craft is not accelerating for long periods of time, the acceleration should, if the gyroscope reading were correct, point roughly in the same direction as this initial reading. Thus the rotation that takes the accelerometer reading to the initial value gives a measure of the error in the gyroscope reading. Let g n denote the normalised initial accelerometer reading, and a n denote the 7
normalised current reading, and w denote the weight. The corrective rotation is then given by: θ = Arccos(g n a n ) E g = [ g n a n sin( wθ ), cos(wθ 2 2 )] The error from the magnetometer reading was determined in an exactly analogous manner. This method was then further improved by storing the updates, and correcting the gyroscope reading in advance. Because the gyroscope data was determined to be roughly normally distributed, the mean of past values should serve as a reasonable estimate of future values. This learning of the gyro drift meant that the weights of the corrections due to the accelerometer and magnetometer readings can be reduced, allowing a relaxation of the assumption that the craft is only accelerating for short periods of time, and widening the applicability of the algorithm. The gyroscope bias was also learnt, along with the accelerometer bias, in an initial calibration stage, and taken as the mean of some number of initial samples. Bias in the accelerometer reading Initially the accelerometer reading was stored, and kept as a reference. As a reasonably accurate reading was needed this was taken over an average of some initial number of samples. This includes the initial bias value. However, as tests indicated this bias drifted over a period of 5 minutes or so, this initial value needed to be updated. This was done by performing a weighted update at each timestep of the initial accelerometer reading. The weighting was determined per dimension (individually for x, y and z) by the variance from the initial stored value over a long period of recorded data as below: w glearning = w gmax exp( V ariance(a t x a xi )) V ariance(a t, b) = mean((a t b) 2 ) Where w glearning is the final update weight w gmax is the maximum value this can take, a t x is a past list of stored data for the x values of accelerometer readings, and a xi is the accepted initial accelerometer reading; the accepted gravitational acceleration plus bias1. Bias in the Magnetometer reading This was determined in a calibration stage, run before the main loop began. It was determined based on the premise stated in the introduction that the bias vector can be determined as the centre of an ellipsoid comprised of the readings given by the magnetometer. This centre was simply computed by determining the maximum and minimum readings, and taking the middle of these two numbers along each axis. Thus during the initial calibration stage these maximum and minimum readings had to be detected, so the calibration 8
included revolving the sensor about each axis. Which should give a reasonable estimate of the bias, though it should be noted revolving about each axis does not guarantee the maximum and minimum values will be encountered. Parameter estimation For the purpose of this report some assumptions were made about the typical behaviour of the craft on which this filter was placed. This was necessary to justify otherwise arbitrary choices of constants. Firstly, for efficiency the infinite impulse response filter was used for filtering out random noise. The weight applied to each successive value reflects a compromise between effective noise filtration and step response of the filter. To quantify this it was assumed that the craft recieved position updates from an external source (for example a gps) every minute. Thus the acceptable amount of accumulated error in the position was assumed to be 1 metre per minute. This is equivalent to saying the maximum error in velocity is one metre per minute. Let dt be the update period of the sensor (assumed constant for the purpose of this estimate), a be the maximum error in the accelerometer reading. Then: S = 1 2 = 1 2 = 1 2 60/dt (a i + a)(dt) 2 i=1 60/dt i=1 60/dt i=1 a i (dt) 2 + 1 2 60/dt i=1 a i (dt) 2 + 1 2 60 adt a(dt) 2 Thus 2ds 60dt = a This implies the error in each acceleration reading, for our timestep, which is at most 6.7 10 2 s, can be at most 0.5 ms 2. With the goal of determining the weight of the infinite impulse response filter we now assume that the order of magnitude of the acceleration does not exceed 100ms 2, and further that the acceptable lag time in the response of our filter is 0.5 s. This means for our timestep, within roughly ten samples, the possibly erroneous past data samples must have negligible effect on the output of the filter, where negligible is quantified by the upper bound on the error in acceleration derived previously. Let w max be the weight of the infinite impulse response filter. Then 100wmax 10 i=0 wi max < 0.5 assuming all other terms of the infinite sum are negligible and we obtain w max < 0.5. The long term update of the bias in the accelerometer reading also hinged upon the assumption that the craft was not accelerating for long periods of time, as the update weight was made a function of the variance of past data, so if long term acceleration was sustained the bias might be updated to an incorrect value. It is assumed that the maximum time of continuous smooth acceleration for our craft is 10 seconds. Again, assuming that the maximum reading attained by the 9
accelerometers is 100ms 1, we require that the magnitude of the update of the bias vector 100w g exp( V ariance(a t d) << 0.5 where w gmax is the maximum gravitational learning weight and a ( t) is the past data over 10 seconds of the accelerometer values. If we assume the variance is zero over this time period, we reach the same conclusion as before, that w gmax < 0.5. Main loop The compilation of the above yields the main loop. while system is stationary and away from magnetic interference do read sensor data (gyroscope, accelerometer, magnetometer) accepted-gravity := sliding average of acceleration data accepted-heading := sliding average of magnetometer data gyro-drift := sliding average of raw (euler) gyroscope data end while while True do read sensor data (gyroscope, accelerometer, magnetometer) accelero-reading, magno-reading, gyro-reading := filtered data angle-change := (gyro-reading - gyro-drift) time-step (quaternion) attitude := attitude angle-change gravity-error := rotation from accelero-reading to accepted-gravity compass-error := rotation from magno-reading to accepted-heading gyro-drift := gyro-drift + ( weighted gravity-error and compass-error ) attitude := (c-1 compass-error) (c-2 gravity-error) attitude accepted-gravity := long term update of accelero-reading in inertial frame velocity + = (accelero-reading - accepted gravity) in inertial frame timestep position + = 1 2 velocity time-step end while 10
Experiments Analysis of Sensor Error If left stationary the sensor produces the following results. The approximate normal distributions produced by the gyroscopes indicate that learning the mean of past values will likely be an effective estimate of the future drift, at least over a short timescale. Demonstration of typical timescales involved in long term bias drift 11
Gyroscope Drift Learning To determine the effectiveness of learning the gyroscope drift, data from the stationary IMU was recorded and filtered with and without learning the gyroscope drift. The corrections obtained from the gravity and magnetometer reading were used as a measure of the effectiveness of the gyroscope drift update and plotted over time. If learning gyroscope drift is effective then the correction made according to gravity and magnetometer readings should grow smaller. The below graphics show the angles of rotations from the present heading and acceleration vectors to their long term averages. In order they are with no gyroscope drift learning and then with gyroscope drift learning and weights of 0.2, 0.5 and 0.7 on the acceleration vector correction. It is apparent that learning gyroscope drift is effective at reducing the need to correct attitude readings using gravity. This broadens the applicability of the algorithm since it is not necessary to weight the gravity correction as heavily. It also allows for the possibility of updating attitude from gyroscope readings at a higher frequency than quality acceleration data can be obtained. 12
13
14
Effectiveness of Learning Magnetometer Bias Bias was learnt by moving the device around and finding the center of the resulting locus of readings in the body frame. Once this calibration had been performed, the device was rotated once in the horizontal plane and the angle from its original position recorded. The below graphs plot this angle over time for correction weights of 0, 0.2 and 0.5 respectively. It can be seen that the magnetic correction results in less drift in the plane orthogonal to gravity. 15
16
Gravity Learning The next two graphs show the measured displacement values of a still device without changing the value for gravity and then with adjusting it using a long term average with weighting 0.3. Note the different vertical scales. It is clear that adjusting for varying accelerometer bias improves position estimates. An algebraic model like that suggested by Titteron [TW04] is inadequate in that it does not account for time dependent device bias. 17
18
Parallelising The end goal of the project was to parallelise the above algorithm in order to make its implementation on a Field Programmable Gate Array possible. Some considerations for programming in this environment and their respective implications are listed below. Memory is limited. Several kb total on chip. This constraint would limit such constructs as long term sliding averages. Data paths should be pipelined, there is no access to shared memory. Arithmetic operations on pipelines take up space. Space for operators is more important that time. Operator complexity should be minimised by using approximations, bit shifts and look up tables where possible. No floating point arithmetic. In order to parallelise the algorithm, the below data flow chart was developed. 19
Gyro Smoothing Gyro Reading Gryo Drift Drift Correct Time Stamp Accelerometer Attitude Integrate Magnetometer Smoothing Combine Smoothing Accelero Reading Attitude Magno Reading Rotate Rotate Gravity Accelero Reading (inerital) Heading Magno Reading (iinertial) Compare Compare Weighted Sum Rotate Weighted Sum Attitude Gryo Drift Transform Acceleration Subtract Weighted Sum Acceleration Gravity Integrate Velocity Add Velocity Integrate Position Add Position 20
Conclusion A simple and more parallelisable approach to the problem of inertial navigation has been developed. In contrast to the general approach in literature, the algorithm does not employ the kalman filter, resulting in more intuitive tuning, and less computation. The algorithm aims to compensate for random noise by employing simple smoothing filters. It accounts for time dependant drift in accelerometer reading and gyroscope readings. These aspects of the filter were tested yeilding positive results. It was shown that the method for accounting for time dependant bias in the gyroscope was effective at reducing the needed attitude adjustment. It was also shown by the implemented method for adjusting for compass bias that a relatively simple method for calibration can still be effective. References [RAL10] V. Renaudin, M.H. Afzal, and G. Lachapelle. Complete triaxis magnetometer calibration in the magnetic domain. Journal of Sensors, 2010. [TW04] D.H. Titterton and J.L. Weston. Strapdown inertial navigation technology, volume 17. Peter Peregrinus Ltd, 2004. 21