Estimate Orientation with a Complementary Filter and IMU Data

Connect Hardware

Connect the SDA, SCL, GND, and VCC pins of the MPU-9250 sensor to the corresponding pins of the Arduino® hardware. This example uses an Arduino® Uno board with the following connections:

Ensure that the connections to the sensors are intact. It is recommended to attach/connect the sensor to a prototype shield to avoid loose connections while the sensor is in motion. Refer theTroubleshooting Sensors(MATLAB Support Package for Arduino Hardware)page to debug the sensor related issues.

Create Sensor Object

Create anarduinoobject and anmpu9250object. Specify the sensor sampling rateFsand the amount of time to run the loops. Optionally, enable theisVerboseflag to check if any samples are overrun. By disabling theuseHWflag, you can also run the example with sensor data saved in the MAT-fileloggedMPU9250Data.mat.

The data inloggedMPU9250Data.matwas logged while the IMU was generally facing due South, then rotated:

Notice that the last two rotations around the x-axis are an additional 90 degrees. This was done to flip the device upside-down. The final orientation of the IMU is the same as the initial orientation, due South.

Fs = 100; samplesPerRead = 10; runTime = 20; isVerbose = false; useHW = true;ifuseHW一= arduino; imu = mpu9250(a,'SampleRate', Fs,'OutputFormat','matrix',...'SamplesPerRead', samplesPerRead);elseload('loggedMPU9250Data.mat','allAccel','allGyro','allMag',...'allT','allOverrun',...'numSamplesAccelGyro','numSamplesAccelGyroMag')end

Align Axes of MPU-9250 Sensor with NED Coordinates

The axes of the accelerometer, gyroscope, and magnetometer in the MPU-9250 are not aligned with each other. Specify the index and sign x-, y-, and z-axis of each sensor so that the sensor is aligned with the North-East-Down (NED) coordinate system when it is at rest. In this example, the magnetometer axes are changed while the accelerometer and gyroscope axes remain fixed. For your own applications, change the following parameters as necessary.

% Accelerometer axes parameters.accelXAxisIndex = 1; accelXAxisSign = 1; accelYAxisIndex = 2; accelYAxisSign = 1; accelZAxisIndex = 3; accelZAxisSign = 1;% Gyroscope axes parameters.gyroXAxisIndex = 1; gyroXAxisSign = 1; gyroYAxisIndex = 2; gyroYAxisSign = 1; gyroZAxisIndex = 3; gyroZAxisSign = 1;% Magnetometer axes parameters.magXAxisIndex = 2; magXAxisSign = 1; magYAxisIndex = 1; magYAxisSign = 1; magZAxisIndex = 3; magZAxisSign = -1;% Helper functions used to align sensor data axes.alignAccelAxes = @(in) [accelXAxisSign, accelYAxisSign, accelZAxisSign]....* in(:, [accelXAxisIndex, accelYAxisIndex, accelZAxisIndex]); alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign]....* in(:, [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]); alignMagAxes = @(in) [magXAxisSign, magYAxisSign, magZAxisSign]....* in(:, [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

Perform Additional Sensor Calibration

If necessary, you may calibrate the magnetometer to compensate for magnetic distortions. For more details, see the Compensating for Hard Iron Distortions section of theEstimating Orientation Using Inertial Sensor Fusion and MPU-9250example.

Specify Complementary filter Parameters

ThecomplementaryFilterhas two tunable parameters. TheAccelerometerGainparameter determines how much the accelerometer measurement is trusted over the gyroscope measurement. TheMagnetometerGainparameter determines how much the magnetometer measurement is trusted over the gyroscope measurement.

compFilt = complementaryFilter('SampleRate', Fs)

compFilt = complementaryFilter with properties: SampleRate: 100 AccelerometerGain: 0.0100 MagnetometerGain: 0.0100 HasMagnetometer: 1 OrientationFormat: 'quaternion'

Estimate Orientation with Accelerometer and Gyroscope

Set theHasMagnetometerproperty tofalseto disable the magnetometer measurement input. In this mode, the filter only takes accelerometer and gyroscope measurements as inputs. Also, the filter assumes the initial orientation of the IMU is aligned with the parent navigation frame. If the IMU is not aligned with the navigation frame initially, there will be a constant offset in the orientation estimation.

compFilt = complementaryFilter('HasMagnetometer', false); tuner = HelperOrientationFilterTuner(compFilt);ifuseHW ticelseidx = 1:samplesPerRead; overrunIdx = 1;endwhiletrueifuseHW [accel, gyro, mag, t, overrun] = imu(); accel = alignAccelAxes(accel); gyro = alignGyroAxes(gyro);elseaccel = allAccel(idx,:); gyro = allGyro(idx,:); mag = allMag(idx,:); t = allT(idx,:); overrun = allOverrun(overrunIdx,:); idx = idx + samplesPerRead; overrunIdx = overrunIdx + 1; pause(samplesPerRead/Fs)endif(isVerbose && overrun > 0) fprintf('%d samples overrun ...\n', overrun);endq = compFilt(accel, gyro); update(tuner, q);ifuseHWiftoc >= runTimebreak;endelseifidx(end) > numSamplesAccelGyrobreak;endendend

Estimate Orientation with Accelerometer, Gyroscope, and Magnetometer

With the default values ofAccelerometerGainandMagnetometerGain, the filter trusts more on the gyroscope measurements in the short-term, but trusts more on the accelerometer and magnetometer measurements in the long-term. This allows the filter to be more reactive to quick orientation changes and prevents the orientation estimates from drifting over longer periods of time. For specific IMU sensors and application purposes, you may want to tune the parameters of the filter to improve the orientation estimation accuracy.

compFilt = complementaryFilter('SampleRate', Fs); tuner = HelperOrientationFilterTuner(compFilt);ifuseHW ticendwhiletrueifuseHW [accel, gyro, mag, t, overrun] = imu(); accel = alignAccelAxes(accel); gyro = alignGyroAxes(gyro); mag = alignMagAxes(mag);elseaccel = allAccel(idx,:); gyro = allGyro(idx,:); mag = allMag(idx,:); t = allT(idx,:); overrun = allOverrun(overrunIdx,:); idx = idx + samplesPerRead; overrunIdx = overrunIdx + 1; pause(samplesPerRead/Fs)endif(isVerbose && overrun > 0) fprintf('%d samples overrun ...\n', overrun);endq = compFilt(accel, gyro, mag); update(tuner, q);ifuseHWiftoc >= runTimebreak;endelseifidx(end) > numSamplesAccelGyroMagbreak;endendend

Summary

This example showed how to estimate the orientation of an IMU using data from an Arduino and a complementary filter. This example also showed how to configure the IMU and discussed the effects of tuning the complementary filter parameters.