%% Simulate Magnetic Jamming on |ahrsFilter|
% This example shows how performance of the |ahrsfilter| System object(TM)
% is affected by magnetic jamming.
%

% Copyright 2018 The MathWorks, Inc.


%%
% Load |StationaryIMUReadings|, which contains accelerometer, magnetometer,
% and gyroscope readings from a stationary IMU.
%

load 'StationaryIMUReadings.mat' accelReadings magReadings gyroReadings SampleRate

numSamples = size(accelReadings,1);


%%
% The |ahrsfilter| uses magnetic field strength to stabilize its
% orientation against the assumed constant magnetic field of the Earth.
% However, there are many natural and man-made objects which output
% magnetic fields and can confuse the algorithm. To account for the
% presence of transient magnetic fields, you can set the
% |MagneticDisturbanceNoise| property on the |ahrsfilter| object.
% 
% Create an |ahrsfilter| object with the decimation factor set to 2 and
% note the default expected magnetic field strength.
%

decim = 2;
FUSE = ahrsfilter('SampleRate',SampleRate,'DecimationFactor',decim);

%%
% Fuse the IMU readings using the attitude and heading reference system
% (AHRS) filter, and then visualize the orientation of the sensor body over
% time. The orientation fluctuates at the beginning and stabilizes after
% approximately 60 seconds.
%

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');
time = (0:decim:(numSamples-1))'/SampleRate;

figure(1)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data')

%%
% Mimic magnetic jamming by adding a transient, strong magnetic field to
% the magnetic field recorded in the |magReadings|. Visualize the magnetic
% field jamming.
%

jamStrength = [10,5,2];
startStop = (50*SampleRate):(150*SampleRate);
jam = zeros(size(magReadings));
jam(startStop,:) = jamStrength.*ones(numel(startStop),3);

magReadings = magReadings + jam;

figure(2)
plot(time,magReadings(1:decim:end,:))
xlabel('Time (s)')
ylabel('Magnetic Field Strength (\mu T)')
title('Simulated Magnetic Field with Jamming')
legend('z-axis','y-axis','x-axis')

%%
% Run the simulation again using the |magReadings| with magnetic jamming.
% Plot the results and note the decreased performance in orientation
% estimation.
%

reset(FUSE)
orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(3)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Default Properties')

%%
% The magnetic jamming was misinterpreted by the AHRS filter, and the
% sensor body orientation was incorrectly estimated. You can compensate for
% jamming by increasing the |MagneticDisturbanceNoise| property. Increasing
% the |MagneticDisturbanceNoise| property increases the assumed noise range
% for magnetic disturbance, and the entire magnetometer signal is weighted
% less in the underlying fusion algorithm of |ahrsfilter|.
%
% Set the |MagneticDisturbanceNoise| to |200| and run the simulation again.
%
% The orientation estimation output from |ahrsfilter| is more accurate and
% less affected by the magnetic transient. However, because the
% magnetometer signal is weighted less in the underlying fusion algorithm,
% the algorithm may take more time to restabilize.
%

reset(FUSE)
FUSE.MagneticDisturbanceNoise = 20;

orientation = FUSE(accelReadings,gyroReadings,magReadings);

orientationEulerAngles = eulerd(orientation,'ZYX','frame');

figure(4)
plot(time,orientationEulerAngles(:,1), ...
     time,orientationEulerAngles(:,2), ...
     time,orientationEulerAngles(:,3))
xlabel('Time (s)')
ylabel('Rotation (degrees)')
legend('z-axis','y-axis','x-axis')
title('Filtered IMU Data with Magnetic Disturbance and Modified Properties')
