insCFMotionOrientation

Estimate orientation from recorded IMU data by using a complementary filter, represented by an insCF filter object.

Load the rpy_9axis.mat file into the workspace. The file contains recorded accelerometer, gyroscope, and magnetometer sensor data from a device oscillating in pitch (around the y-axis), yaw (around the z-axis), and roll (around the x-axis). The file also contains the sample rate of the recording. Extract the sensor data and sample rate from the loaded workspace variable.

ld = load("rpy_9axis.mat");
accelerometerData = ld.sensorData.Acceleration;
gyroscopeData = ld.sensorData.AngularVelocity;
magnetometerData = ld.sensorData.MagneticField;
imuSampleRate = ld.Fs; % Hz

Create a timetable from the accelerometer, gyroscope, and magnetometer data.

imuDataTT = timetable(accelerometerData,gyroscopeData,magnetometerData,'SampleRate',imuSampleRate,'StartTime',seconds(1/imuSampleRate),'VariableNames',{'Accelerometer', 'Gyroscope', 'Magnetometer'});

Create an insCF filter object using sensor models for accelerometer, magnetometer, and gyroscope. To model orientation, specify insCFMotionOrientation as the motion model.

acc = insCFAccelerometer;
mag = insCFMagnetometer;
gyro = insCFGyroscope;
motionModel = insCFMotionOrientation;
filter = insCF(acc,mag,gyro,motionModel);

Specify the initial orientation of your sensors. You can obtain the initial orientation by using the ecompass function with the accelerometer and magnetometer data at the first time step.

initialOrientation = [0.006727036930852 -0.131203115007920 -0.058108427699335 -0.989628162602834];
stateparts(filter,"Orientation",initialOrientation)

Specify the sensor gains. The sensor gain value must be between 0 and 1.

gainparts(filter,"Accelerometer",0)
gainparts(filter,"Magnetometer",0.01)

Fuse the accelerometer, gyroscope, and magnetometer data using the insCF filter.

outTT = estimateStates(filter,imuDataTT);

Visualize the estimated orientation.

t = outTT.Properties.RowTimes;
plot(t,eulerd(outTT.Orientation,"ZYX","frame"));
title("Orientation Estimate");
legend("Z-rotation","Y-rotation","X-rotation");
xlabel("Time");
ylabel("Degrees");