Blockset described on this wiki is deprecated since 2012.

For Model Based Design (MBD), use the free MPLAB Device Blocks for Simulink, tool from Microchip.
Updated Rapid Control Prototyping (RCP) custom projects are published at: https://lubin.kerhuel.eu.

Miniature Inertial Measurement Unit - IMU

From http://www.kerhuel.eu/wiki - Simulink device driver Blockset for dsPIC / PIC24 / PIC32 Microcontrollers --[[User:LubinKerhuel|LubinKerhuel]] 12:40, 3 September 2009 (UTC)
Jump to navigation Jump to search
IMU sensors : 1 accelerometer MMA7260 from freescale (3 axis) coupled with 2 rate gyros ADIS16080 from analog device (1 axis each)

A DIY Miniature Inertial Measurement Unit (IMU) implemented on a dsPIC with a rapid prototyping tool. This IMU is based on inertials sensors. It estimates its absolute angle in space (atitude)


All files: simulink model, matlab script and data files (.mat) can be downloaded from the Download section at the bottom of this page.


Introduction

The micro-Inertial Measurement Unit (IMU) described estimates its atitude about the two rotation angles : pitch and roll. The three main electronics parts 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 earth gravity
  • 1 dsPIC (30f or 33f or 1 PIC24) for the calculation tasks

The atitude is estimated for each axes fusing information from one rate gyros with two accelerometers.

After a descripion of the electronics board, two methods to log raw data from the sensors are presented. Simulink simulation relying on theses real data logged provides an easy way to tune and compare different data fusion methods. The inherent problem of atitude estimation using MEMs is then settled. Then, three data fusing methods using both rate gyro and accelerometers are described and compared :

  • Complementary filter (1st and 2nd order)
  • Method described by Pisano 2005
  • Non-adaptative Kalman filter

The implementation of the simulink model using a dsPIC is then described. I am using the rapid prototyping tool described on this website (dsPIC blockset for Simulink) allowing to generate the .hex code file for the microcontrolor directly from the simulink model file with a one push button procedure. Real time constraint are analysed and fixed point implementation is considered.



Embedded Electronics

fig:IMU_SensorSchematic This IMU use three MEMS sensors : One Three axial analog accelerometer MMA7260QT and two one axis rate gyro ADIS16080 with SPI digital interface. The 3 axes accelerometer is powered by the 2.5V analog output reference generated by the 'Y axis' rate gyro

The Embedded IMU Schematic is equipped with one three axis accelerometer MMA7260QT coupled with two one axis rate gyro ADIS16080. (see fig:IMU_SensorSchematic)

The overall sensor card is suplied with a single stabilized 5V. The 2.5V alimentation required by the accelerometer is provided by the 2.5V Voltage Reference of one gyro. The low current requirement for the accelerometers do not perturbe the rate gyro. Each rate gyro has two unused analog input. Thus, the two gyros have 4 analog input, 3 of which are used to convert the three analog values ax, ay, and az from the accelerometer. This sensor board transmit the 5 data (3 acceleration + 2 rate gyros) to the microcontroller through its digital SPI bus.

The stabilized 5V is generated with a microchip MCP1252-33X50 component.



Log raw data from sensor

Data from the inertial sensors are logged with matlab. Theses real data feeds a simulink model allowing to develop/debug/compare different data fusion algorithm.

All sensor data are received through the SPI peripheral of the dsPIC. Data are received from the two gyros using the SPI Input/Output interrupt driven block. The code generated by this block use interrupt to constantly transmit and receive data from the two rate gyros and obtain the 2 angular speed rate and the 3 acceleration measurement. The Simulink block rate is 1ms thus 5 measurement are read each 1ms. The value read is the last measurement received through the SPI bus (which is much faster than 1ms, we are sure that values are updated at each 1ms Simulink time step)

The dsPIC used (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).

Data are transmitted to the PC through the UART peripheral. We have the following constraints:

  • model time-step is 1ms
  • UART speed rate is 115200bps
  • 5 values of 12 bits resolution must be transmitted

Two data logging methods are developped: The first logging method use the Tx Output Multiplexed for Matlab-Labview block. The other method use the Tx Output block. Both logging method use the graphical matlab interface developed: Interface Tx-Matlab

Using the UART Tx Output Multiplexed for Matlab-Labview block

IMU_dsPIC_30f4012_TestElectronics Simulink model: Once this model is compiled, the dsPIC (30f4012) transmit the values from the X, Y and Z axis of the accelerometer, and the Y and X axis of the rate gyro. Data are sent to Matlab (or labview) through the UART at 115200bps with the Tx Output Multiplexed for Matlab-Labview block. All data cannot be send ; there will be missing data thus this model is usefull to check every component is working only.

The model IMU_LogData is compiled and the generated .hex file is loaded into the dsPIC (using the bootloader tinybld). The model samples the data at 1kHz, corresponding to the time-step of 1ms. During 1ms, the UART can send 11,52 Bytes when configured at 115200 bps (taking into account the start and stop bits). We log 5 int16 data that is a total of 10 bytes. The multiplexing protocol of the Tx Output Multiplexed for Matlab-Labview block.]] adds one byte for each variable sent. Thus, the model sends 15 bytes at each time step. Because the simulink model try to send more than 11 data per time-step, some data will be lost. The Intelligent Spreading option will spread the lost over all data allowing to view all data on the graph (If not checked, the two last data may never be sent).

Because of the lost of some data, this method is easy and great for plotting data. It allows checking that all components are working. It is Not OK to use these data to feed a Simulink model! (You may interpolate the data but it is a bit tricky…)

The data that get out from the Tx Output Multiplexed for Matlab-Labview block are the sensor raw data. It is 16 bits data but only the 12 lower bits contains the measured value.

The dsPIC is connected to the PC COM port (using an equivalent to Max232 component for level translation) and data are logged using the graphical user interface. The following lines are typed at the maltab prompt to save data into a .mat file:

 >> [R T] = padr(Rn,1,t_Rn);     % remove NaN values
 >> datas = [T R]';
 >> save mydatafile datas;

padr is a command from the blockset. It allows removing 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 the file.

Keep in mind than missing data on columns [2 3 4 5] are stuffed with their previous non missing value. Missing data on column 1 suppressed the time step. In other word, datas are incomplete and should not feed a simulink simulation. If we decided to log 3 datas only (2 accelerometers and 1 gyro), all data would be received (9 bytes sent each time step). However,another method described below allows logging all the 5 values at 1kHz without missing data!

Using the UART Tx Output block

IMU_dsPIC_30f4012_LogData_Raw Simulink model: Once this model is compiled, the dsPIC (30f4012) transmit the raw values from the MEMs sensors. Data are sent to Matlab (or labview) through the UART at 115200bps with the Tx Output block. The UART buffer do not overflow with this mode, because 9 bytes are sent and the 115200bpd UART speed allow to send 11,52 bytes in one 1ms time-step. Thus all data are successfully transmitted to matlab. Logged data can feed a simulink simulation.
fig:RawData - Raw data from the sensor. On this experiment, the IMU box were rotated by 90 degrees with interval of about 5s. File Simulink_Dynamic_Calibration3.mat plotted using the script Caracterisation_Capteurs.m. Theses raw data are used to feed simulink simulation.
fig:RawDataSpectre - Spectral Analysis of the Raw data from Simulink_Dynamic_Calibration3.mat. The IMU box were rotated by 90 degrees with interval of about 5s. The y-Gyro is perturbed by several high frequency peak starting at 92Hz. This may due to a soldering problem. Theses high frequency peak have no influence on the angle estimation.

The UART at 115200 can transmit only 11,52 data within a 1ms time-step. Thus, the simulink model implemented on the dsPIC must transmit less than 12 datas to have all data being sent. The method described is more complex but allow to log all 5 measurement without lost with a sampling rate of 1kHz.

5 values of 12 bits length must be sent. Each data is stored in a 2 data bytes length (16 bits) and the 5 values, thus 10 data byes are concatenated. One signature byte with the fixed 55 is sent first, before the 10 bytes frame and provide the synchronization reference. This signature is may also used to check that no data were lost (checking that 10 bytes are present between every 55 signature values).

From the matlab GUI interface >> rs232gui, data received are no more multiplexed with the Tx Output Multiplexed for Matlab-Labview block. The demultiplexing algorithm is switched off by pushing once the RAW button (bottom left of the graphical interface). The embedded script in the rs232gui interface should be cleared. The R vector contains the raw data stream received by the Serial port of the computer. Once the recording is finished, this R vector is saved (files Rawdatas*.mat). The script Extract_RawDatas.m load the saved R vector and extract the 5 values. The key part of the script is shown below.

R = reshape(R,11,n); % reshape : from vector to a matrix of 11 column
idxError = find(R(1,:) ~= 55)  % Check data integrity
if ~isempty(idxError)
     error('Data corrupted')
end
  
%% Reconstruct Raw data as received through the SPI bus
RawGyro_Y = R(2,:)*2^8 + R(3,:);
RawAccel_Y = R(4,:)*2^8 + R(5,:);
RawAccel_Z = R(6,:)*2^8 + R(7,:);
RawGyro_X = R(8,:)*2^8 + R(9,:);
RawAccel_X = R(10,:)*2^8 + R(11,:);
T = [0:.001:(length(RawGyro_Y)-1)*.001];

On the embedded simulink model, a mechanism allow to send data during a predifined time: The model embedded on the dsPIC send data only when the value variable60 is different from 0. This variable is changed throught the UART using the same rs232gui graphical user interface with the send1 and send2 button. When receiving values [60 x], the embedded simulink model load the value x into the Simulink variable 'variable60'. The Simulink model decreases automatically this variable at a sampling rate of 1 second. This mechanism make the user able to log data during a predefined time.

Data Analysis

5 files corresponding to 5 differents experiments are saved : 2 in static situation (IMU sensor board is keep static on the table) and 3 in dynamic situation while the IMU sensor board orientation is modified by a 90° rotation approximatly every 5 seconds. The script Caracterisation_Capteurs.m plot the data previously saved for each different experiments.

Gain

The gain of the two rate gyro does not comply with the datasheet value. The datasheet give an angular speed rate of ±80°/s with a resolution of 0.039°/s. In practice, on the electronic sensor board, the Y rate gyro measures a range of ± 524°/s with a resolution of 0.26°/s. The X rate gyro can measure a range of ± 188°/s with a resolution of 0.09°/s.

The gain of the accelerometers is equal on all of the three axis ans precise calibration is not necessary. A slight correction align the offset of each axis.

Noise

Figures RawData and RawDataSpectre show respectively the temporal and frequency analysis plot of the experiment stored in the Simulink_Dynamic_Calibration3.mat file.

This experiment is a dynamic one: with orientation changes. The temporal analysis shows that the rate gyro y axis contains more noise than the rate gyro x axis. This may be due to a soldering problem (It was very hard to sold all theses parts with the very basic soldering material I have !), to a problem due to the ADIS16080 part itself or to the use of the y rate gyro ref voltage to power the acceelrometer (that need a very low curent). Anyway, The frequency analysis shows on the y rate gyro axis that this noise is mostly present within peaks frequency starting at 93Hz. As the gyro is integrated (integration act as a low pass filter) this high frequency noise has no influence on the angle estimation.

The frequency analysis also show that the accelerometers is perturbed by the 50Hz component that probably commes from the europe 50Hz general AC power. As the accelerometers is also low pass filtered, this high frequency noise has also no influence on the estimated angle.

Frequency above 20Hz are considered as high frequency as the IMU is to be placed onboard a RC model airplane. The angular dynamic of theses aerial vehicle is far below 20Hz.


All files described can be download (see bottom : Download section).



Problems of angles estimation from inertial sensors

Estimating angles from rate gyro

A rate gyro measures its angular speed rate. Precision of theses MEMS sensors are usually good. 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 and the sensor has a zero mean noise, the integration would provides the real angular orientation. The integration function also acts as a low pass filter. The sensor noise, which is already low, is even lowered by this integration.

  • <Tex>\Theta</Tex> the absolute pitch angle and q the pitch angular rate.

<Tex>\hat \Theta(t) = \int_{t_0}^t{q(t) dt}+\Theta(t_0)</Tex>

  • <Tex>\Phi</Tex> the absolute roll angle and p the roll angular rate.

<Tex>\hat \Phi(t) = \int_{t_0}^t{p(t) dt} + \Phi(t_0)</Tex>

Unfortunately, the rate gyro sensor is not ideal. Integrating the rate gyro also integrate its DC value. The integration of any error induced an unrecoverable bias on the estimated angle. As this DC value drifts slowly over time, it is not possible to cancel it by subtracting to the gyro output its DC offset value. Thus, Integration of a rate gyro results in an angle drift of about 1° within one minute (depending on the rate gyro used!). It is necessary to use another sensor so as to recover the bias introduced by the integration of errors.

Estimating angles from accelerometers

Accelerometers sense both the gravity acceleration vector (9.81m/s^2) which is by definition vertical, added to the linear acceleration of the sensor itself. Accelerometers are usually quite noisy and needs to be filtered. The acceleration of a system could in many case considered as small compared to the gravity acceleration value. If the system is considered as quasi static (Hovering helicopters, flying gliders in straight flight...), 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 (eg: a plane which has its nose in the sky, perpendicular to the horizontal has its roll angle undefined). Absolute calibration of the accelerometers gain is not necessary because a fraction is calculated within the trigonometric arctan function. The only point to care about is the offset of the three axes and the gain that must be equal for all three axes.

However, the first weakness of the angle estimated from accelerometers is its sensibility to the acceleration of the system. In others words, the estimated angle is biased whenever the system accelerates. The second weakness is the noisy result due to the accelerometers sensors itself.

Merging estimation from Accelerometers and Gyro

Angle estimation from either a rate gyro 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 make a data fusion algorithm so as to couple the static reference angle computed from the Accelerometers with the dynamic angle variation estimation from the rate gyro. The fusing algorithm tuning takes into account the dynamic of the system. If the system is subject to high acceleration very often, the estimated angle will rely mostly on the rate gyro so as to remove the errors introduced by the acceleration measurement. Otherwise, the estimated angle may rely more on the accelerometers.




Data fusion algorithms

IMU Simulation - Simulation of an IMU using on real data. Several data fusion algorithms are compared including Complementary Filter and Non-adaptative Kalman Filter. The simulink model file IMU_Simulation.mdl can be Downloaded at the bottom of this page.

Simulation with real data

The model IMU_Simulation.mdl uses the logged data stored in .mat files. These data are used to design, and simulate algorithm to estimate angles. Because we are using real data, the input of the simulation has precisely the right properties (noise, bias...). The data stored are exactly the same data read at the output of sensors. Once the simulation provide optimal result, it is possible to implement the tuned algorithm as is in the PIC/dsPIC.

The raw data feeds the Sensor Calibration block. This block removes bias of rate gyro and accelerometers. Because we use the arctan function, the accelerometer axes do not need to be scaled. Only gyro measurement are scaled.

This Simulation compares Three angle estimation methods:

  • Complementary filter (1st and 2nd order)
  • Method described by Pisano 2005
  • Non-adaptative Kalman filter

Complementary Filter

1st order

Complementary Filter for IMU angle estimation

A complementary filter combines the two estimations. 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 0.8Hz first order low pass filter transfer function for the accelerometer is :

<Tex>Lp(s) = \frac{0.5}{s+0.5}</Tex>

  • The 0.8Hz first order High pass filter transfer function for the rate gyro is :

<Tex>Hp(s) = \frac{s}{s+0.5}</Tex>

Bode diagram of a 0.08Hz 1st order complementary filter. The blue curve is the low pass filter. The green curve is the high pass filter and the red curve is the addition of both filters. The red curve has a phase of 0° and a gain of 1.
  • From the analog device datasheet, the rate gyro has a cut off frequency at 40Hz. Thus, the transfer function of the integration of the rate gyro including the inverse dynamic of the rate gyro is:

<Tex>Int(s) = \frac{1}{314} * \frac{s+314}{s}</Tex>

  • For the rate gyro, we obtain the simplification :

<Tex>Hp(s) * Int(s) = \frac{s}{s+0.5} * \frac{1}{314} * \frac{s+314}{s} = \frac{1}{314} * \frac{s+314}{s+0.5}</Tex>

The upper part of the complementary filter block estimates the Y axis angle computing the arctan of the X and Z accelerometers values. The lower part of the complementary filter blocks 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.

This first order complementary filter gives correct results. However, the gyro bias which is integrated generate a ramp. The order 1 filter output has a steady state error while estimating the gyro static value due to this ramp. An order 2 filter is however capable to remove this steady state error.

2nd order

We design a 2nd order complementary filter.

  • The high pass filter is:

<Tex>Hp_2(s) = \frac{s^2}{(s+0.2)^2}</Tex>

  • The low pass filter is the complementary:

<Tex>Lp_2(s) = 1 - Hp_2(s) = \frac{0.4s + 0.02}{(s+0.2)^2}</Tex>

  • As for the 1st order complementary filter, the high pass filter can combine with the integration of the rate gyro. We do not take into account the dynamic of the rate gyro here:

\frac{1}{s} * \frac{s^2}{(s+0.2)^2} = \frac{s}{(s+0.2)^2}</Tex>

This complementary filter is now able to compensate for any rate gyro bias with no steady state error.

Method described by Pisano 2005

This method is given for completness. The results are very similar to the order 1 complementary filter with the drawback of the steady state error due to the rate gyro bias estimation.

Non adaptative Kalman Filter

The non adaptative Kalman filter track the rate gyro bias and estimate the angle by integrating the unbiased gyro.

This method gives results similar to the 2nd order complementary filter with the advantage of also providing directly the gyro bias estimation and the unbiased angular rate of the system. Calculation are not much complicated.


to be continued...

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.


Download

IMU_for_dsPIC.zip : All Simulink and Matlab file used with data logged. Will be updated soon

The demo version of the blockset could compile model with up to 6 I/O pins. The Simulink model presented could not be compiled as is with the demo version of the blockset because SPI bus use 5 pins and UART uses 2 pins. Removing the UART blocks, makes possible compiling theses models. Adding for example one PWM output may give the possibility to get the result for one angle.

List of files:

  • Simulink models
    • IMU_dsPIC_30f4012_TestElectronics.mdl
    • IMU_dsPIC_30f4012_LogData_Raw.mdl
    • IMU_Simulation.mdl
    • IMU_dsPIC_30f4012_ComplementaryFilter_20Mips.mdl
    • IMU_dsPIC_30f4012_AlphaBetaFilter_20Mips.mdl
  • M script
    • Extract_RawDatas.m
    • Script_Kalman.m
    • RealTimeVisualisationScript.m
  • Mat files
    • RawDatas_*.mat : Raw data received from the dsPIC
  • mat files to feed simulink model file IMU_Simulation.mdl
    • Simulink_Dynamic_Calibration.mat
    • Simulink_Dynamic_Calibration2.mat
    • Simulink_Dynamic_Calibration3.mat
    • Simulink_Static.mat
    • Simulink_Static_60s.mat


On-going projects

I am planning to use this IMU 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 !


-Old data

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 running on dsPIC

This model implements the complementary filter on the dsPIC and sends the result to matlab. It has the same behaviors as 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 changed 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 seconds 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 behaviors of the simulated filter.

Remarks about complementary filter

The high pass filter, the inverted rate gyro dynamic and the integrator are merged into one transfer function.

All calculation is 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)