Estimate Phone Orientation Using Sensor Fusion

Open Live Script

MATLAB Mobile™ reports sensor data from the accelerometer, gyroscope, and magnetometer on Apple or Android mobile devices. Raw data from each sensor or fused orientation data can be obtained. This examples shows how to compare the fused orientation data from the phone with the orientation estimate from theahrsfilterobject.

Read the Accelerometer, Gyroscope, Magnetometer, and Euler Angles

Read the logged phone sensor data. The MAT-filesamplePhoneData.matcontains sensor data logged on an iPhone at a 100 Hz sampling rate. To run this example with your own phone data, refer toSensor Data Collection with MATLAB Mobile.

matfile ="samplePhoneData.mat"; SampleRate = 100;% This must match the data rate of the phone.[Accelerometer,Gyroscope,Magnetometer,EulerAngles]...= exampleHelperProcessPhoneData(matfile);

Convert to North-East-Down (NED) Coordinate Frame

MATLAB Mobile uses the convention shown in the following image. To process the sensor data with theahrsfilterobject, convert to NED, a right-handed coordinate system with clockwise motion around the axes corresponding to positive rotations. Swap the x- and y-axis and negate the z-axis for the various sensor data. Note that the accelerometer readings are negated since the readings have the opposite sign in the two conventions.

Accelerometer = -[Accelerometer(:,2) Accelerometer(:,1) -Accelerometer(:,3)]; Gyroscope = [Gyroscope(:,2) Gyroscope(:,1) -Gyroscope(:,3)]; Magnetometer = [Magnetometer(:,2) Magnetometer(:,1) -Magnetometer(:,3)]; qTrue = quaternion([EulerAngles(:,3) -EulerAngles(:,2) EulerAngles(:,1)],..."eulerd","ZYX","frame");

Correct Phone Initial Rotation

The phone may have a random rotational offset. Without knowing the offset, you cannot compare theahrsfilterobject and the phone results. Use the first four samples to determine the rotational offset, then rotate the phone data back to desirable values.

% Get a starting guess at orientation using ecompass. No coefficients% required. Use the initial orientation estimates to figure out what the% phone's rotational offset is.q = ecompass(Accelerometer,Magnetometer); Navg = 4; qfix = meanrot(q(1:Navg))./meanrot(qTrue(1:Navg)); Orientation = qfix*qTrue;% Rotationally corrected phone data.

Tune the AHRS Filter

To optimize the noise parameters for the phone, tune theahrsfilterobject. The parameters on the filter need to be tuned for the specific IMU on the phone that logged the data in the MAT-file. Use thetune函数与logged orientation data as ground truth.

orientFilt = ahrsfilter(SampleRate=SampleRate); groundTruth = table(Orientation); sensorData = table(Accelerometer,Gyroscope,Magnetometer); tc = tunerconfig("ahrsfilter"MaxIterations = 30,...ObjectiveLimit=0.001,Display="none"); tune(orientFilt,sensorData,groundTruth,tc);

Fuse Sensor Data With Filter

Estimate the device orientation using the tunedahrsfilterobject.

reset(orientFilt); qEst = orientFilt(Accelerometer,Gyroscope,Magnetometer);

Plot Results

Plot the Euler angles for each orientation estimate and the quaternion distance between the two orientation estimates. Quaternion distance is measured as the angle between two quaternions. This distance can be used as an error metric for orientation estimation.

numSamples = numel(Orientation); t = (0:numSamples-1).'/SampleRate; d = rad2deg(dist(qEst, Orientation)); figure plot(t,eulerd(qEst,"ZYX","frame")) legendyawpitchroll标题("ahrsfilter Euler Angles") ylabel("Degrees") xlabel("Time (s)")

Figure contains an axes object. The axes object with title ahrsfilter Euler Angles, xlabel Time (s), ylabel Degrees contains 3 objects of type line. These objects represent yaw, pitch, roll.

figure plot(eulerd(Orientation,"ZYX","frame")) legendyawpitchroll标题("Phone Euler Angles") ylabel("Degrees") xlabel("Time (s)")

Figure contains an axes object. The axes object with title Phone Euler Angles, xlabel Time (s), ylabel Degrees contains 3 objects of type line. These objects represent yaw, pitch, roll.

figure plot(t,d) title("Orientation Error") ylabel("Degrees") xlabel("Time (s)")% Add RMS errorrmsval = sqrt(mean(d.^2)); line(t, repmat(rmsval,size(t)),LineStyle="-.",Color="red"); text(t(1),rmsval + 0.7,"RMS Error = "+ rmsval,Color="red")

Figure contains an axes object. The axes object with title Orientation Error, xlabel Time (s), ylabel Degrees contains 3 objects of type line, text.

Useposeplotto view the orientation estimates of the phone as a 3-D rectangle.

figure pp = poseplot(MeshFileName="phoneMesh.stl");fori = 1:numel(qEst) set(pp,"Orientation",qEst(i)); drawnowend

Figure contains an axes object. The axes object is empty.