Matlab-Simulink device driver Blockset for Microchip dsPIC / PIC24 / PIC32 Microcontrollers.

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, search
IMU sensors : 1 accelerometer MMA7260 from freescale (3 axis) coupled with 2 rate gyros ADIS16080 from analog device (1 axis each)

Implementation of a miniature Inertial Measurement Unit (IMU) on a dsPIC microcontroller with Mathworks (tm) rapid prototyping tools coupled with the dsPIC blockset. Simulink is used to both simulate and generate the embedded software that is downloaded onboard the dsPIC microcontroller. This IMU estimates pitch and roll angles using two different types of Micro Electro Mechanical Systems (MEMS) sensors: rate gyro and accelerometers.

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

Contents

Introduction

The micro Inertial Measurement Unit (IMU) It is an attitude indicator (ADI) or artificial horizon also called gyro horizon that estimates its attitude about both axis: pitch and roll. The electronics parts are:

The first part settle the inherent problem of angle estimation from inertial sensors using MEMS. Then, a first IMU based on a 5 DoF sensor board is presented. Its sensor board is described. Raw data measurement are recorded and different data fusing algorithms are compared with simulation based on the real data previously recorded. The implementation of the simulink model using a dsPIC is then described. I am using extensively the rapid prototyping tool described on this website (dsPIC blockset for Simulink) allowing generating the .hex code file for the microcontroller directly from the simulink model file (previously simulated) with a one push button procedure. Limitation of this 5DoF sensor board is explained and an IMU algorithm using a 6 DoF sensor board is presented and implemented

Problems of angles estimation from inertial sensors

Estimating angles from rate gyro

One rate gyro (ADIS 16080) with the simple material used for soldering. Thin wires come from a washing machine motor's coil.

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, an integration will provides the angular orientation. The integration process also acts as a low pass filter and reduce the high frequency noise (despite the fact theses sensors has already low noise).


Equation

Equation

Unfortunately, the rate gyro sensor is not ideal. Integrating the rate gyro also integrate its DC value(bias) or its non zero mean noise. The integration of this error introduced a growing error on the estimated angle. As the gyro's DC value drifts slowly over time, it is not possible to cancel this bias by subtracting this DC offset value to the gyro output. Thus, Integration of a rate gyro results in an angular drift which is about 1° per minute (strongly depending on the rate gyro used!). It is necessary to use another sensor so as to recover the gyro bias and correcting for the angle error introduced by the integration process.

Estimating angles from accelerometers

Accelerometers measures both the acceleration and gravity induced forces (From Wikipedia definition). Accelerometers are usually quite noisy and needs to be filtered. In many situation, (induced force of) acceleration of a system could be considered as small compared to the gravity (induced force). If the system do not changes its velocity and orientation, (Hovering helicopters, plane in straight flight...), it is possible to extract the pitch and roll absolute angle using the measured gravity vector.

Let's consider:

Using the acceleration vector (i.e. gravity) measured with the accelerometers, we get the Equation and Equation angles :

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). 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 gain and offset of all the three axes that must be equal.

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 (i.e. change its velocity or its direction). The second weakness is the noisy result that comes from the accelerometers sensors itself. This noise is even greater when the accelerometers sensor is placed onboard a vibrating vehicle (vibration from motors and propellers for plane and helicopters and vibration from displacement from wheeled vehicle). Low pass filtering may lower the noise from vibration but would correct the bias due to the system own acceleration. Adding to this, the low pass filter would add a phase (i.e. delay) that may be prejudicial in an Auto-Pilot feedback loop.

Merging estimation from accelerometers and rate gyro

Angle estimation from either a rate gyro alone or an accelerometer alone does not provide good results. However, the characteristics of these two types of sensors are complementary:

It is therefore possible to design a data fusion algorithm that merges the static reference angle computed from the accelerometers with the dynamic angle variation estimated from the rate gyro. If the system is regularly subject to high acceleration, the estimated angle should rely mostly on the rate gyro so as to remove the errors induced by the acceleration. Otherwise, the estimated angle could rely more on the accelerometers.

We have the following trade-off:

Thus, the fusion algorithm should take into account the dynamic characteristics of the system.

IMU based on a 5 DoF sensor board (2 rate gyro, 3 accelerometers)

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 bus interface. The 3 axes accelerometer is powered by the 2.5V analog output reference generated by the 'Y axis' rate gyro component

The Embedded IMU Schematic is equipped with one three axis accelerometer MMA7260QT coupled with two one axis rate gyro ADIS16080 (see fig:IMU_SensorSchematic). This custom made board could be replaced by a commercial one like the SEN-00741 from Sparkfun called IMU 5 Degrees of Freedom.

The overall sensor board is supplied with a single stabilized 5V generated with a microchip MCP1252-33X50 component. The 2.5V alimentation required by the accelerometer chip is provided by the 2.5V voltage reference of one rate gyro. The low current required for the accelerometers does not disturb 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 transmits the 5 measurements (3 acceleration + 2 rate gyros) to the microcontroller through a digital SPI bus.

Log raw data from the 5 DoF sensor board

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 is interrupt based. Thus, interruption that continuously occurs allows updating the 2 angular rates and the 3 acceleration measurement. At least, five interrupt must occurs within one model time step (1ms here) to get all values updated. Each block's output the last value received for the corresponding output through the SPI bus.

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

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

Two data logging methods are developed: The first logging method uses the Tx Output Multiplexed for Matlab-Labview block. It is the easiest method for viewing and recording variable evolution from a dsPIC. The second method uses the Tx Output block. This last method presented allows recording/viewing more data within one time-step. 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 sent; there will be missing data thus this model is useful 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 sampling time is 1 Khz, 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 should transmit 15 bytes through the UART at each time step. Because the simulink model try to send more than 11 bytes per time-step, some data will not be sent and will be lost. The Intelligent Spreading option will spread the data loss over all channels allowing to view all curves on the graph (If not checked, the two last channels 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 data 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 any 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 was 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.

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 data to have all data being sent. The method described is more complex but allow logging 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 provides 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 predefined time: The model embedded on the dsPIC send data only when the value variable60 is different from 0. This variable is changed through 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 makes the user able to log data during a predefined time.

Data Analysis

fig:RawDataSpectre - Spectral Analysis of the Raw data from Simulink_Dynamic_Calibration3.mat. The IMU box was rotated by 90 degrees with interval of about 5s. The y-Gyro is perturbed by several high frequency peaks starting at 92Hz. This may due to a soldering problem. Theses high frequency peak have no influence on the angle estimation.

5 files corresponding to 5 different 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 approximately 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 axes and precise calibration is not necessary. A slight correction aligns 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 these 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 accelerometer (that need a very low current). 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 shows that the accelerometers are perturbed by the 50Hz component that probably comes from the European 50Hz general AC power. As the accelerometers is also low pass filtered, this high frequency noise has also no influence on the estimated angle.

Frequencies 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 downloaded (see bottom: Download section).






IMU Data fusion algorithms

IMU Simulation - Simulation of an IMU using 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 IMU algorithms. Using real data, the input of the simulation has the correct properties (noise, bias, nonlinearity...). The data stored are exactly the same data read at the output of sensors. Once the simulation provide correct results, the tuned algorithm is implemented as is (from simulink) directly in the microcontroller (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 scaling. Only rate gyro measurements are scaled.

This Simulation compares Three angle estimation methods:

Complementary Filter

1st order

Complementary Filter for IMU angle estimation

A complementary filter combines the angle estimations from both the rate gyro and the accelerometers. High frequency part of the angle estimated from the rate gyro is added to the low frequency part of the angle estimated with the accelerometers. Thus, the low drift of the estimated angle from the gyro is filtered out and only the high frequency part of the rate gyro estimated angle is retained. The low (or DC) frequency part of the angle estimated rely on the accelerometers angle estimation.

Equation

Equation

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.

Equation

Equation

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 fairly good results. However, the integrated rate gyro bias generates a ramp. The 1st order filter has a small steady state error while filtering the integrated rate gyro 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.

Equation

Equation

Equation

This 2nd order complementary filter compensates for the rate gyro bias with no steady state error.

Method described by Pisano 2005

This method is not developed here but is given for completeness (see simulink files). 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.

State Space Approach

The state space structure tracks the rate gyro bias and estimate the angle by integrating the unbiased gyro.

The state space approach is equivalent to 2nd order complementary filter with the advantage of explicitly providing the estimated rate gyro bias and the unbiased angular rate of the system. The structure is a state space observer. The two states are the angle estimation and the gyro bias. The matlab function acker allows placing (through the gain K) the pole of the observer, defining the convergence of both the angle estimation and of the rate gyro bias. Defining a high speed convergence rate make the output more sensitive to the acceleration measurement, inducing noise from the accelerometers and sensitivity to the own acceleration of the unit that induce false angle estimation. A low convergence rate will make the bias estimation of the rate gyro very slow.

It is possible to set a different convergence rate for the two states. The gain K is static in this version. Using a variable gain K based on sensor noise measurement and system's own dynamic would result in the implementation of a Kalman Filter. However, added calculation are not worth because the characteristics of the system are fixed (Dynamic of the vehicle is fixe, and sensor noise is stationary). The gain obtained using a kalman filter does not justify its complexity.


Download

IMU_for_dsPIC.zip : All Simulink and Matlab file used with data logged. (updated on 21 september 2008)

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 uses 5 pins and UART uses 2 pins (total is 7 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.

Models name containing 'Simulation' are simulation of algorithm based on real data logged from the electronics. Models name without 'Simulation' are model that are directly compiled to generate an .hex file that can be downloaded into the microcontroller (Works with the registered version of the dsPIC blockset).

List of files:


Implementation on a dsPIC

Complementary Filter Order 1

This model implements the complementary filter on the dsPIC and sends the result to matlab. It has the same behaviours 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, its 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. Thus, the integrated angle drifts slowly 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 behaviours of the simulated filter.

Fixed point

Fixed point version is working and can be downloaded in the .zip file at the bottom of the page.


IMU based on a 6 DoF sensor board (3 rate gyro, 3 accelerometers)

Limitations the IMU based on a 5DoF sensor board

The IMU based on the 5DoF sensor board have no rate gyro on the yaw axis. Thus, when the angular yaw rate is not null while the system is not horizontal, the pitch and roll angle of the system are modified despite the angular pitch and roll rate is 0! Thus, the pitch and roll angle estimation based on pitch and roll rate gyro is false, until the accelerometers information is taken into account and correct for the error (like a bias correction).


This problem is of particular concern when the IMU is used onboard a plane. When we decompose a plane change of flight direction:

Thus, the pitch rate which tends to make the plane climbing is compensated for by the yaw rate that compensates and maintains plane heading aligned with the horizon. The IMU based of a 5DoF sensor board does not take into account the yaw rate. For a change of flight direction, the IMU measure a growing pitch angle despite (the plane is climbing) despite the true horizontal maintained by the plane.


Adaptation of the 5DoF IMU algorithm for a 6 DoF IMU

Electronic board evolution

The electronic sensor board is base on the electronic board presented above except than one rate gyro was added for measuring the yaw axis.

The added rate gyro had an unexpected effect; il changes most of the gain of the others rate gyro. Thus, the calibration was done again starting from scratch. Hopefully, the calibration had to be done only once because the sensors parameters do not changed.

Extended IMU algorithm

The algorithm used is an extension of the previous complementary filter algorithm. I used the state space approach.

Two version of the extended algorithm were done. The simpler one simply used exactly the same state space subsystem, but modified the two gyro input for both the pitch and roll input. The new pitch and roll input are a combination of both pitch and yaw or roll and yaw. The new pitch and roll input value for the subsystem (state space model) are the pitch and roll rotational rate in the earth frame of reference (in opposition to the previous plane frame of reference). The two rate angle were combined using trigonometric relation (cosine or sine) based on the current estimated angle. This method improved the results. However, the state space model estimation of the gyro bias is difficult as its input is now a combination of two rate gyro.

A second version was developed where the state space model were modified. In this last version, the state space model is capable to estimate a bias for the two rate gyro separately (all 3 rate gyro bias are estimated).

Theses new IMU algorithms take into account angle coupling and provide a correct attitude estimate.

All files of this 3 DoF IMU are provided as is.

IMU_2008_10_as_is.zip: All Simulink and Matlab file used with data logged for the 6DoF IMU. (Updated on 30 December 2008)

This IMU is being tested onboard a Remote Control (RC) flying wing. It is destined to become an important part of the autopilot currently under development. For more information, see page :

Acknowledgement

I would like to thanks companies that helped building this IMU by providing freely electronics components. Microchip provides microcontrollers and few others electronics parts. Analog Device provided samples of their great ADIS rate gyro part. Freescale also provide useful ds176 parts that acts like their MAX232 part but require less external component. Coil Craft provided really nice coil allowing to stabilize efficiently the sensor board's power lines.

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

Personal tools
Namespaces
Variants
Actions
Blockset
Examples
Blocks (Root level)
Digital I/O
Peripheral I/O
Serial PORT
Numerical BUS
others
Lubin Kerhuel
Toolbox