Miniature Inertial Measurement Unit - IMU

IMU sensors : 1 accelerometer MMA7260 from freescale (3 axis) coupled with 2 gyrometers ADIS16080 from analog device (1 axis each)

An Inertial Measurement Unit is based on inertial sensors to estimate its absolute angle also called atitude in space. The micro-Inertial Measurement Unit (IMU) presented here estimates its atitude around two rotation angles. The three principal components are:

• 2 one axis MEMS rate gyro measuring roll and pitch speed rate
• 1 tri axial MEMS accelerometer measuring both the IMU acceleration and the gravity
• 1 dsPIC (30f or 33f) or 1 PIC24

The atitude of the IMU system is calculated using information from the rate gyros and the accelerometers. After a quick description of the angle estimation using accelerometers or gyro, this document describe the electronics board and shows how to log data throught the UART port of the PC. Then, the document present the different estimation methods that fuse datas from gyro and accelerometers to retrieved the atitude of the IMU system :

• Complementary filter
• non-adaptative Kalman filter

Theses two methods are simulated with simulink using real data from the board and results are compared. The method to implement in no time the softwar onboard an embedded PIC or dsPIC based board using the rapid prototyping system described on this website : the Simulink blockset for PIC/dsPIC.

Problems of estimating angles from inertial sensors

Estimating angles from rate gyro

A rate gyro measures its angular speed rate. Low noise and high precision of theses MEMS sensors are good enough. Thus, integrating the value of the rate gyro over time provide a good estimation of the angular displacement. Provided the initial position of the system is known, the integration provide the angular position. The integration function also acts as a low pass filter. The sensor noise, which is already low, is even lowered by the integration. However, integrating the rate gyro also integrate its DC component. The DC component drifts slowly over time. The integration of error induced an unrecoverable bias on the estimated angle. It is not possible to cancel it subtracting the DC offset value. Typically, the integrated angle value will drift of about 1° within one minute (depending on the rate gyro used). Another sensor must be used to recover the bias induced by the integration of error.

• <Tex>\Theta</Tex> the absolute pitch angle and q the pitch angular rate. <Tex>\hat \Theta = \int_0^t{q(t) dt}+\Theta_0</Tex>
• <Tex>\Phi</Tex> the absolute roll angle and p the roll angular rate. <Tex>\hat \Phi = \int_0^t{p(t) dt} + \Phi_0</Tex>

Estimating angles from accelerometers

Accelerometers sense the both the gravity vertical acceleration vector (9.81m/s) added with the acceleration of the sensor itself. Accelerometers are usually quite noisy and needs to be filtered. The acceleration of a system (Hovering helicopters, flying gliders in stable straight flight...) could usually be considered as small compared to the gravity acceleration value. If the system is considered to not accelerate, it is possible to extract the pitch and roll absolute angle using the measured gravity vector.

• ax the acceleration measured with the accelerometer on the x axis (longitudinal)
• ay the acceleration measured with the accelerometer on the y axis (lateral)
• az the acceleration measured with the accelerometer on the z axis (vertical)

Using the acceleration vector (i.e. gravity) measured with the accelerometers, we get the <Tex>\Theta</Tex> and <Tex>\Phi</Tex> angles :

• <Tex>\hat \Theta= \arctan \left( \frac{a_z}{a_x} \right)</Tex>
• <Tex>\hat \Phi = \arctan \left( \frac{a_z}{a_y} \right)</Tex>

Note than when the axis az and ay are in the horizontal plane, the result is undefined. But pitch or roll angle is not defined as well (think of a plane which has its nose in the sky, perpendicular to the horizontal. Its absolute roll angle is undefined). Absolute calibration of the accelerometers gain is not necessary because a fraction is computed. The only point to care about is the offset of the three axes and to be sure that the gain is the same for all three axes. However, the angle estimated from accelerometers is sensible to the acceleration of the system. In others words, the estimated angle is biased whenever the system accelerates. The estimated angle is also quite noisy due to the accelerometers sensors and cannot be used directly.

Estimating angles from both Gyrometers and Accelerometers

Angle estimation from either Gyrometers only or Accelerometers only do not provide good results. The characteristics of these two types of sensors are complementary:

• Gyros provide a clean estimation of angle in dynamic situation on a short range time
• Accelerometers provide a noisy but absolute angle reference in static situation

It is therefore possible choose a data fusion algorithm so as to coupled the static reference angle from computed from the Accelerometers with the dynamic and clean angle variation estimation computed from the gyrometers.

Embedded Electronics

This IMU use three sensor : One Three axial accelerometer and two one axis rate gyro. The 3 axes accelerometer MMA7260QT is powered by the 2.5V analog output reference created by the 'Y axis' rate gyro

The Embedded IMU Schematic is equipped with 1 accelerometer (3 axis) coupled with 2 gyrometers (1 axis each) One of the gyro supplies the 2.5V alimentation requited by the accelerometer. Each gyro has two analog input pin connected to its internal ADC. Thus, the two gyro convert the three analog values of X,Y, and Z of the accelerometer. The IMU transfer the 5 data (3 acceleration + 2 gyrometers) to the microcontroller through a bus SPI.

The chosen dsPIC: dsPIC 30f4012 is equipped with a 10Mhz quartz. This quartz frequency allows using the UART at 115200bps which is the max baud rate available on most personal computers (Serial port's limitation).

Log sensor’s raw data for use in simulink simulation

Using the UART multiplexed block

IMU_LogData Simulink model : Once this model is compiled, the PIC (30f3012) transmit the values from the X and Y axis of the accelerometer, and the Y axis of the rate gyro. Data are sent directly to Matlab (or labview) through the UART at 115200bps with the Tx_Labview_Matlab bloc.

The model IMU_LogData is compiled and the generated .hex file is loaded into the dsPIC (using the bootloader tinybld). The model sample the data at 1Khz, corresponding to the time-step of 1ms. During 1ms, the UART can send 11 Bytes when configured at 115200 bps (including start and stop bits).We log 3 int16 data (X and Z acceleration ; and Y raw gyro) that correspond to 6 bytes. Adding one protocol byte for each different variable sent, the model send 9 bytes at each time step. Because the model send less that 11 data by time-step, all data will be send and received by the computer that log theses data. If this model send more that 11 bytes within one time step, some data could not be sent and would therefore be lost (it is OK for plotting data but not for use as input data in a Simulink simulation). The data that get out from the IMU_Sensors block are fixed point data type. They are converted into uint16 using the Store Integer option : in other word, their bits are not modified, but their representation for simulink is now uint16 (thus, their value has changed for simulink !) . They can be sent by the TxOutputMultiplexed block that is able to send int8, uint8, int16, uint16, int32 or uint32 data type. The dsPIC is connected to the PC COM port (usint an equivalent to Max232 component for level translation) and data are logged using the graphical user interface (open the Interface TxMatlab block, for more information, see examples). The following lines are typed at the maltab prompt to save data into a .mat file :

 >> [R T] = padr(Rn,1,t_Rn);
 >> AxelX_GyroY_AxelZ = [T R]';
 >> AxelX_GyroY_AxelZ(1,:) = [0:33332]*1e-3;
 >> save AxelX_GyroY_AxelZ3 AxelX_GyroY_AxelZ;

padr is a command from the blockset. It allows to remove the NaN from the Rn matrix. The resulting R matrix is transposed and time is added in its first line. The time T which is a modified copy of t_Rn is not accurate since it is estimated by the time at which values are received. We know that the data are sent at a 1Khz frequency so we can redefine the time of the AxelX_GyroY_AxelZ matrix before saving it into the AxelX_GyroY_AxelZ3.mat file.

Using the UART Tx block

To do

Simulation based on real data

Angle estimation fusing rate gyro data with accelerometer data: A complementary filter combines the two estimation. High frequency part of the angle estimated from the rate gyro is added to the low frequency part of the angle estimated with the accelerometer. Thus, the low drift of the estimated angle from the gyro is filtered and only high frequency estimated angle from the gyro is used. The low frequency part of the angle estimated rely on the angle estimated using the accelerometer. The high frequency of the angle is estimated with the gyro which provide a fast and accurate information but which cannot be integrated due to the accumulation of the bias error.

The IMU_simu_RealData simulink model simulate the complementary filter using the real data previously logged with the dsPIC. Many filters can be tested using the same real data. The frequency of the filter and bias values can be tuned to get the best result (with the logged data)

The model IMU_Simu_RealData uses the logged data contained in the file AxelX_GyroY_AxelZ3.mat. These data are used to design a simple complementary filter and simulate it. Because we are using real data, the input of the simulation has precisely the right properties (noise, bias...). The filter can be design and simulated with data that are close to reality. In this model, datas are converted back into their former fixed point representation data type. In other word, their binary data is not modified, but the scaling is modified. After the first conversion block, they are equivalent to the data of the IMU_sensors block of the previous model. Data are converted again into double or single to simplify the design at this step. The upper part of the model estimates the Y axis angle computing the atan of the X and Z accelerometers values. Because we use the Atan function, the two accelerometer axis do not need to be scaled. The only constraint is that the X and Z acceleration share the same scaling. The lower part of the model estimates the Y axis angle integrating the Y rate gyro. The dynamic of the gyro (low pass filter) is compensated for. The rate gyro is also high pass filtered to remove most of the DC part. The complementary filter, two first order filter (high pass and low pass) with cut off frequency at 0,08Hz. The final estimation of the angle is the addition of the resulting values from theses two filters. The result is presented in pink on the lower part of the following figure.

Top curves : The X (yellow) and Z (cyan) accelerometers raw data with the Y axis rate gyro (pink). The y scale is the raw numerical value as read from the Gyro-SPI bus. The x scale is time (simulation duration is 33,32s). The three bottom curves are the simulated estimation of the angle (Y axis) using three different methods. The yellow curve is the estimated angle from the X and Z accelerometers, sensing the gravity. The cyan curve is the estimated angle, integrated from the Y rate gyro. The pink curve is the estimated angle estimated with the complementary filter that fusion the relevant information from both the Y rate gyro and the X-Z accelerometer

Complementary Filter

Download the three models described above here with sample of sensor's data. The aim of this complementary filter is to provide a method to implement a model on the dsPIC that has been previously improved thanks to the simulation capabilities of simulink. Simulation uses real data logged with the dsPIC peripheral (same electronic circuit, noise etc...) In a first step, the real data are logged by the first model IMU_LogData. The complementary filter is simulated using the real data in the IMU_simu_RealData. The resulting model is then implemented into the dsPIC in the IMU_ComplementaryFilter model.

Complementary Filter running on dsPIC

The IMU_ComplementaryFilter simulink model is a 'copy-past' mixture made of the IMU_LogData 'data management' part with the IMU_simu_RealData model tuned and optimize filter. This model implements the complementary filter on the dsPIC and sends the result to matlab. It will have the same behaviors of the simulated filter.

To design the filter for the dsPIC, the two precedent models are mixed up. The filter is inserted by copy-past into the first model used to log data. Data logged in real time from the complementary filter implemented on the dsPIC (30f4012 running at 10MHz). X axis : time in second. Y axis : angle multiplied by 100 (see model) The blue curve is the estimated angle from the X and Z accelerometers, resolving the gravity vector. The red curve is the estimated angle integrated from the Y rate gyro. The green curve is the angle estimated through the complementary filter that fusion the relevant information from both the Y axis rate gyro and the X and Z accelerometers.

The estimated angle using accelerometers (blue curve) is quite noisy. However, it steady value is correct. The estimated angle relying on the integration of the gyro is clean but drift (red curve). Note that the dsPIC has just been switch on and the High pass filter of the pseudo integrator of the gyro has not yet removed the remaining DC bias of the gyro. The DC bias of the Gyro has probably change due to non constant temperature in my house. (I do not have a clim in my tiny room which is in a roof in Marseille! ). Thus, the gyro integrated angle is slowing drifting away from its real value. The green curves that use both data is clean and drift free. Its dynamics is as fast as the gyro dynamic. Note that at the beginning, the High pass filter (with a higher bandwidth than the gyro pseudo-integrator high pass filter) has not yet removed the gyro bias. However, at the end of the animation (11 secondes long: from 3, 5 to 14, 5), both curves (blue and green) are merging as in the simulation.

The IMU_ComplementaryFilter simulink model is a 'copy-past' mixture made of the IMU_LogData 'data management' part with the IMU_simu_RealData model tuned and optimize filter. This model implements the complementary filter on the dsPIC and sends the result to matlab. It will have the same behavious of the simulated filter.

Remarks about the complementary filter

The integration of the gyro should be done after the high-pass filter of the complementary filter. This filter placed before the integration would avoid getting high value in the integration internal computation. High value with floating point data mean that the calculation loses its precision. The high-pass filter of the complementary filter has been placed at the end for reason of simplicity and to compare easily the estimation of the angle with the three methods ( integrating gyro, using accelerometer, or by combining both).

All calculation are done in double data type or single data type. Theses float data slow down the calculation and use a large amount of memory. It is quite powerful with the help of simulink to build a fixed point complementary filter. Thus, next step is to go back to the simulation model and to design a fixed point complementary filter! (Using the matlab fixed point toolbox)

Alpha Beta Filter

coming soon

Conclusion

Programming dsPIC or PIC24 becomes exciting and fun with this toolbox. You do not have to waste your time with configuration of the dsPIC peripherals! Focus only on simulink simulation, Real time Workshop capabilities, and on fixed point optimization.

Futur work

I am planning to improve this IMU ( fixed point, kalman filtering ? ) and then, to use it in an autopilote for remote controlled glider ( as previous project described on my french website : http://lubink.free.fr)

Point to clarify, error, remarks ? Please, leave your comment on the forum !