%% Model Tilt Using Gyroscope and Accelerometer Readings
% Model a tilting IMU that contains an accelerometer and gyroscope using
% the |imuSensor| System object(TM). Use ideal and realistic models to
% compare the results of orientation tracking using the |imufilter| System
% object.
%

% Copyright 2018 The MathWorks, Inc.

%%
% Load a struct describing ground-truth motion and a sample rate. The
% motion struct describes sequential rotations:
%
% # yaw: 120 degrees over two seconds
% # pitch: 60 degrees over one second
% # roll: 30 degrees over one-half second 
% # roll: -30 degrees over one-half second
% # pitch: -60 degrees over one second
% # yaw: -120 degrees over two seconds
%
% In the last stage, the motion struct combines the 1st, 2nd, and 3rd
% rotations into a single-axis rotation. The acceleration, angular
% velocity, and orientation are defined in the local NED coordinate system.
%

load y120p60r30.mat motion fs
accNED = motion.Acceleration;
angVelNED = motion.AngularVelocity;
orientationNED = motion.Orientation;

numSamples = size(motion.Orientation,1);
t = (0:(numSamples-1)).'/fs;

%%
% Create an ideal IMU sensor object and a default IMU filter object.
%

IMU = imuSensor('accel-gyro','SampleRate',fs);

aFilter = imufilter('SampleRate',fs);

%% 
% In a loop:
%
% # Simulate IMU output by feeding the ground-truth motion to the IMU
% sensor object.
% # Filter the IMU output using the default IMU filter object.
%

orientation = zeros(numSamples,1,'quaternion');
for i = 1:numSamples
    
    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));
    
    orientation(i) = aFilter(accelBody,gyroBody);

end
release(aFilter)

%%
% Plot the orientation over time.
%

figure(1)
plot(t,eulerd(orientation,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Ideal IMU Data, Default IMU Filter')
legend('Z-axis','Y-axis','X-axis')

%%
% Modify properties of your |imuSensor| to model real-world sensors. Run
% the loop again and plot the orientation estimate over time.
%

IMU.Accelerometer = accelparams( ...
    'MeasurementRange',19.62, ...
    'Resolution',0.00059875, ...
    'ConstantBias',0.4905, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',0.003924, ...
    'BiasInstability',0, ...
    'TemperatureBias', [0.34335 0.34335 0.5886], ...
    'TemperatureScaleFactor',0.02);
IMU.Gyroscope = gyroparams( ...
    'MeasurementRange',4.3633, ...
    'Resolution',0.00013323, ...
    'AxesMisalignment',2, ...
    'NoiseDensity',8.7266e-05, ...
    'TemperatureBias',0.34907, ...
    'TemperatureScaleFactor',0.02, ...
    'AccelerationBias',0.00017809, ...
    'ConstantBias',[0.3491,0.5,0]);

orientationDefault = zeros(numSamples,1,'quaternion');
for i = 1:numSamples
    
    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));
    
    orientationDefault(i) = aFilter(accelBody,gyroBody);
    
end
release(aFilter)

figure(2)
plot(t,eulerd(orientationDefault,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Realistic IMU Data, Default IMU Filter')
legend('Z-axis','Y-axis','X-axis')


%%
% The ability of the |imufilter| to track the ground-truth data is
% significantly reduced when modeling a realistic IMU. To improve
% performance, modify properties of your |imufilter| object. These values
% were determined empirically. Run the loop again and plot the orientation
% estimate over time.
%

aFilter.GyroscopeNoise          = 7.6154e-7;
aFilter.AccelerometerNoise      = 0.0015398;
aFilter.GyroscopeDriftNoise     = 3.0462e-12;
aFilter.LinearAccelerationNoise = 0.00096236;
aFilter.InitialProcessNoise     = aFilter.InitialProcessNoise*10;

orientationNondefault = zeros(numSamples,1,'quaternion');
for i = 1:numSamples
    [accelBody,gyroBody] = IMU(accNED(i,:),angVelNED(i,:),orientationNED(i,:));
    
    orientationNondefault(i) = aFilter(accelBody,gyroBody);
end
release(aFilter)

figure(3)
plot(t,eulerd(orientationNondefault,'ZYX','frame'))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
title('Orientation Estimation -- Realistic IMU Data, Nondefault IMU Filter')
legend('Z-axis','Y-axis','X-axis')

%%
% To quantify the improved performance of the modified |imufilter|, plot
% the quaternion distance between the ground-truth motion and the
% orientation as returned by the |imufilter| with default and nondefault
% properties.
%

qDistDefault = rad2deg(dist(orientationNED,orientationDefault));
qDistNondefault = rad2deg(dist(orientationNED,orientationNondefault));

figure(4)
plot(t,[qDistDefault,qDistNondefault])
title('Quaternion Distance from True Orientation')
legend('Realistic IMU Data, Default IMU Filter', ...
       'Realistic IMU Data, Nondefault IMU Filter')
xlabel('Time (s)')
ylabel('Quaternion Distance (degrees)')
