IMU

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

Fusing the rate gyro information with the accelerometer information provide a better estimation of the roll and pitch angles.

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

Fusing speed rate with acceleration ?

Angle estimation using rate gyro data only : The initial value of the integrator must be equal to the initial angle of the rate gyro. Speed rate is then integrated to track the angle. However, non zero mean error of the rate gyro, due to temperature drift, is integrated. This integration of error induced an unrecoverable bias. Also, The initial value are impossible to set without additional sensor.

Angle estimation using accelerometer data only : Accelerometer sense the gravity acceleration added with the acceleration of the sensor itself. Decomposing the gravity vector on the accelerometer axis is unique except for the yaw rotation axis. The resolution of the roll and the resolution of the pitch angle is done using the atan function. Note that calibration of the accelerometer is not necessary with the calculation. We must just have the same scale for the three axis measured.

• roll = atan(z/x) with x the roll rotation axis, and z the yaw rotation axis
• pitch = atan(z/y) with y the pitch rotation axis, and z the yaw rotation axis

However, the roll and pitch angle resolved are correct only when the accelerometer measure the gravity only. In other word, the resolution is biased when the accelerometer accelerate.

Angle estimation fusing rate gyro data with accelerometer data : A complementary filter combines the two estimation. High freqency 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 acurate information but which cannot be integrated due to the accumulation of the bias error.

Electronics

IMU Schematic : 1 accelerometer (3 axis) coupled with 2 gyrometers (1 axis each) One Gyro supply the 2.5V alimentation to the accelerometer. The two gyro convert the three analog values of X,Y, and Z of the accelerometer. The IMU transfer the 5 data to the microcontroler through the SPI bus.

This IMU use three sensor : One Three axial accelerometer and two one axis rate gyro. The accelerometer is powered by the 2.5V analog refference of the 'Y axis' rate gyro.

Complementary Filter

Download the three models described abore 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 use of the simulation capabilities of simulink. Simulation uses real data logged by the dsPIC peripheral (same electronic circuit, same noise etc...)
In a first step, the real data are logged by the 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.

Log real data

The model IMU_LogData is compiled and the resulting .hex file is charged into the dsPIC (using the boot loader tinybld).

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 thanks to the Tx_Labview_Matlab bloc. The quartz and PLL of the dsPIC are choosen in order to have an low UART %error when UART is configured at 115200bps, the max baud rate available on most PC. The model sample rate is 1Khz, corresponding to a 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 X (int16) data (X and Z acceleration ; and Y raw gyro). Adding one protocol byte for each 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 sent and logged by the PC. Note that if the model send more that 11 bytes, some data would be lost (it is OK for visual checking but not for use as input data into in a simulation). The data that get out from the IMU_Sensors block are fixed point data type. They are transformed into uint16 using the Store Integer method : 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 (with the Max232 component for level translation, or equivalent) and data are logged using the graphical 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 allow 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 redifine the time of the AxelX_GyroY_AxelZ matrix before saving it into the AxelX_GyroY_AxelZ3.mat file. Simulation based on real data The model IMU_Simu_RealData uses the logged data contained in the file AxelX_GyroY_AxelZ3.mat. Theses 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.

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).

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 estimate 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 informations from both the Y rate gyro and the X-Z accelerometer.

Complementary Filter running on dsPIC

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.

Logging dara resulting from this filter running on the dsPIC, we get the following curves :

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 informations 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 biais 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 curve that use both data is clean and driftfree. 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.

Remarks

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 to get high value in the integration internal computation. High value with floating point data mean that the calculation lose 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 powerfull 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)

Conclusion

Programming dsPIC or PIC24 becomes exciting and fun with this toolbox. I do not have to worry with the time consumming configuration of the dsPIC peripheral ! I can focus on simulink simulation, on 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 !

The Microchip name PIC, dsPIC, and MPLAB are registered trademarks of Microchip Technology Inc. MATLAB, Simulink, and Real-Time Workshop are registered trademarks of The MathWorks, Inc. Lubin Kerhuel 's Website Contact : [adresse e-mail retirée]