Added analysis code for IMU data and Simulink block for Mahony filter

controls/dataCollection/IMU_standstill/analyzeIMU_data.m

+function analyzeIMU_data( fileName )
+%UNTITLED2 Summary of this function goes here
+%   Detailed explanation goes here
+
+
+    IMU_dataDirectory = 'C:\Users\Tara\Desktop\Project Documents\Current Project Documents\EE 492\Data\IMU Data';
+    IMU_dataFile = [IMU_dataDirectory , '\' , fileName];
+    
+    IMU_data = readtable(IMU_dataFile);
+
+    g = 9.8;    %meters per second squared
+
+    %Set the column values of the table
+    timeCol = 1;
+    accelXCol = 2;
+    accelYCol = 3;
+    accelZCol = 4;
+    gyroXCol = 5;
+    gyroYCol = 6;
+    gyroZCol = 7;
+
+    %Pull the data columns into arrays
+    timeArray = IMU_data{:, timeCol};
+    accelXArray = IMU_data{:, accelXCol};
+    accelYArray = IMU_data{:, accelYCol};
+    accelZArray = IMU_data{:, accelZCol};
+    gyroXArray = IMU_data{:, gyroXCol};
+    gyroYArray = IMU_data{:, gyroYCol};
+    gyroZArray = IMU_data{:, gyroZCol};
+
+    %Get the mean and standard deviation of the accelerometer data and multiply
+    %by the g constant to get this value in meters per second
+    accelX_mean = mean(accelXArray) * g;
+    accelY_mean = mean(accelYArray) * g;
+    accelZ_mean = mean(accelZArray) * g
+    accelX_dev = std(accelXArray) * g;
+    accelY_dev = std(accelYArray) * g;
+    accelZ_dev = std(accelZArray) * g;
+
+    %Get mean and standard deviation of gyroscope data, which is in rad / sec
+    gyroX_mean = mean(gyroXArray);
+    gyroY_mean = mean(gyroYArray);
+    gyroZ_mean = mean(gyroZArray);
+    gyroX_dev = std(gyroXArray);
+    gyroY_dev = std(gyroYArray);
+    gyroZ_dev = std(gyroZArray);
+
+    %Calculate the variances from the standard deviations
+    accelX_var = accelX_dev ^ 2;
+    accelY_var = accelY_dev ^ 2;
+    accelZ_var = accelZ_dev ^ 2;
+    gyroX_var = gyroX_dev ^ 2;
+    gyroY_var = gyroY_dev ^ 2;
+    gyroZ_var = gyroZ_dev ^ 2;
+
+    %Print received data from accelerometer nicely to command prompt
+    disp( ' ' );
+    disp('Accelerometer data: ');
+    disp( [ 'X acceleration mean ( m / s^2 ): ' , num2str(accelX_mean) ] );
+    disp( [ 'Y acceleration mean ( m / s^2 ): ' , num2str(accelY_mean) ] );
+    disp( [ 'Z acceleration mean ( m / s^2 ): ' , num2str(accelZ_mean) ] );
+    disp( [ 'X acceleration standard deviation ( m / s^2 ): ' , num2str(accelX_dev) ] );
+    disp( [ 'Y acceleration standard deviation ( m / s^2 ): ' , num2str(accelY_dev) ] );
+    disp( [ 'Z acceleration standard deviation ( m / s^2 ): ' , num2str(accelZ_dev) ] );
+    disp( [ 'X acceleration variance ( (m/s^2) ^ 2 ): ', num2str(accelX_var) ] );
+    disp( [ 'Y acceleration variance ( (m/s^2) ^ 2 ): ', num2str(accelY_var) ] );
+    disp( [ 'Z acceleration variance ( (m/s^2) ^ 2 ): ', num2str(accelZ_var) ] );
+    disp( ' ' );
+
+
+    %Print received data from gyroscope nicely to command prompt
+    disp('Gyroscope data: ');
+    disp( [ 'Angular velocity about X mean ( rad / s ): ' , num2str(gyroX_mean) ] );
+    disp( [ 'Angular velocity about Y mean ( rad / s ): ' , num2str(gyroY_mean) ] );
+    disp( [ 'Angular velocity about Z mean ( rad / s ): ' , num2str(gyroZ_mean) ] );
+    disp( [ 'Angular velocity about X standard deviation ( rad / s ): ' , num2str(gyroX_dev) ] );
+    disp( [ 'Angular velocity about Y standard deviation ( rad / s ): ' , num2str(gyroY_dev) ] );
+    disp( [ 'Angular velocity about Z standard deviation ( rad / s ): ' , num2str(gyroZ_dev) ] );
+    disp( [ 'Angular velocity about X variance ( (rad/s) ^ 2 ): ', num2str(gyroX_var) ] );
+    disp( [ 'Angular velocity about Y variance ( (rad/s) ^ 2 ): ', num2str(gyroY_var) ] );
+    disp( [ 'Angular velocity about Z variance ( (rad/s) ^ 2 ): ', num2str(gyroZ_var) ] );
+    disp( ' ' );
+
+%     %Graph plots:
+%     figure();
+%     plot(timeArray, accelXArray * g);
+%     title('X Acceleration Data ( m / s^2 )');
+%     xlabel('Time (s)');
+% 
+%     figure();
+%     plot(timeArray, accelYArray * g);
+%     title('Y Acceleration Data ( m / s^2 )');
+%     xlabel('Time (s)');
+% 
+%     figure();
+%     plot(timeArray, accelZArray * g);
+%     title('Z Acceleration Data ( m / s^2 )');
+%     xlabel('Time (s)');
+% 
+%     figure();
+%     plot(timeArray, gyroXArray);
+%     title('Angular Velocity about X Data (rad/s)');
+%     xlabel('Time (s)');
+% 
+%     figure();
+%     plot(timeArray, gyroYArray);
+%     title('Angular Velocity about Y Data (rad/s)');
+%     xlabel('Time (s)');
+% 
+%     figure();
+%     plot(timeArray, gyroZArray);
+%     title('Angular Velocity about Z Data (rad/s)');
+%     xlabel('Time (s)');
+% 
+% 
+%     %Get the magnitude of the accelerometer data
+%     mag_accel = sqrt( accelXArray.^2 + accelYArray.^2 + accelZArray.^2 ) * g;
+%     mag_accel_mean = mean( mag_accel );
+%     mag_accel_dev = std( mag_accel );
+%     mag_accel_var = mag_accel_dev ^ 2;
+% 
+%     %Display these results and graph them
+%     disp( ['Magnitude of acceleration vector mean ( m / s^2 ): ' , num2str(mag_accel_mean) ] );
+%     disp( ['Magnitude of acceleration vector standard deviation ( m / s^2 ): ' , num2str(mag_accel_dev) ] );
+%     disp( ['Magnitude of acceleration vector variance ( ( m/s^2 )^2 ): ' , num2str(mag_accel_var) ] );
+%     figure();
+%     plot( timeArray, mag_accel, 'b' );
+%     hold on;
+%     plot( timeArray, ones(1, length(timeArray)) * g , 'r-' );
+%     title('Magnitude of Acceleration Vector with Respect to Nominal ( m / s^2 )');
+%     xlabel('Time (s)');
+%     legend( 'Accel Mag Measured' , 'Accel Mag Nominal' );
+% 
+%     %Also graph the percent error
+%     mag_accel_percentError = abs(mag_accel - g) / g * 100;
+%     figure();
+%     plot( timeArray, mag_accel_percentError);
+%     title('Magnitude of Acceleration Vector Percent Error from Nominal (%) ');
+%     xlabel('Time (s)');
+%     ylabel('Percent Error (%)');
+% 
+%     mag_accel_percentError_mean = mean( mag_accel_percentError );
+%     mag_accel_percentError_dev = std( mag_accel_percentError );
+% 
+%     disp(' ');
+%     disp( ['Percent error from nominal mean ( % ): ' , num2str(mag_accel_percentError_mean) ] );
+%     disp( ['Percent error from nominal standard deviation ( % ): ' , num2str(mag_accel_percentError_dev) ] );
+%     disp(' ');
+
+
+
+
+
+    %Following example given in MATLAB help
+    %Looking at the frequency response characterisitcs of the noise
+    timeArray_diff = diff( timeArray );
+    timeArray_diffMean = mean( timeArray_diff ); %seconds
+    fs = 1 / timeArray_diffMean;
+
+    %Get the fast fourier transform
+    accelX_fft = fft( accelXArray*g - accelX_mean );
+    accelY_fft = fft( accelYArray*g - accelY_mean );
+    accelZ_fft = fft( accelZArray*g - accelZ_mean );
+    gyroX_fft = fft( gyroXArray - gyroX_mean );
+    gyroY_fft = fft( gyroYArray - gyroY_mean );
+    gyroZ_fft = fft( gyroZArray - gyroZ_mean );
+
+    %Determine the length of our data and obtain only first half of FFT data
+    N = length(timeArray);
+    accelX_fft = accelX_fft( 1:N/2+1 );
+    accelY_fft = accelY_fft( 1:N/2+1 );
+    accelZ_fft = accelZ_fft( 1:N/2+1 );
+    gyroX_fft = gyroX_fft( 1:N/2+1 );
+    gyroY_fft = gyroY_fft( 1:N/2+1 );
+    gyroZ_fft = gyroZ_fft( 1:N/2+1 );
+
+    %Get the power spectral density of the signal
+    accelX_PSD = ( 1 / (2*pi*N) ) * abs( accelX_fft ) .^ 2;
+    accelY_PSD = ( 1 / (2*pi*N) ) * abs( accelY_fft ) .^ 2;
+    accelZ_PSD = ( 1 / (2*pi*N) ) * abs( accelZ_fft ) .^ 2;
+    gyroX_PSD = ( 1 / (2*pi*N) ) * abs( gyroX_fft ) .^ 2;
+    gyroY_PSD = ( 1 / (2*pi*N) ) * abs( gyroY_fft ) .^ 2;
+    gyroZ_PSD = ( 1 / (2*pi*N) ) * abs( gyroZ_fft ) .^ 2;
+
+    %Multiply everything but the first and last term by 2
+    accelX_PSD(2:end-1) = 2*accelX_PSD(2:end-1);
+    accelY_PSD(2:end-1) = 2*accelY_PSD(2:end-1);
+    accelZ_PSD(2:end-1) = 2*accelZ_PSD(2:end-1);
+    gyroX_PSD(2:end-1) = 2*gyroX_PSD(2:end-1);
+    gyroY_PSD(2:end-1) = 2*gyroY_PSD(2:end-1);
+    gyroZ_PSD(2:end-1) = 2*gyroZ_PSD(2:end-1);
+
+    %Get the frequency range vector
+    freq = 0:(2*pi)/N:pi;
+
+    %Show the plots of the power spectral density in the log domain
+    figure();
+    plot( freq/pi, 10*log10(accelX_PSD) );
+    %plot( freq/pi, accelX_PSD, '*' );
+    title('Periodogram of X Acceleration Data Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+    %ylabel('Power Frequency');
+
+    figure();
+    plot( freq/pi, 10*log10(accelY_PSD) );
+    title('Periodogram of Y Acceleration Data Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+
+    figure();
+    plot( freq/pi, 10*log10(accelZ_PSD) );
+    title('Periodogram of Z Acceleration Data Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+
+    figure();
+    plot( freq/pi, 10*log10(gyroX_PSD) );
+    title('Periodogram of Angular Velocity about X Data Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+
+    figure();
+    plot( freq/pi, 10*log10(gyroY_PSD) );
+    title('Periodogram of Angular Velocity about Y Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+
+    figure();
+    plot( freq/pi, 10*log10(gyroZ_PSD) );
+    title('Periodogram of Angular Velocity about Z Using FFT');
+    xlabel('Normalized Frequency (\times\pi rad/sample)');
+    ylabel('Power Frequency (dB/rad/sample)');
+
+
+
+    %Estimate the average power spectral density, ignoring the first value
+    accelX_PSD_mean = mean( accelX_PSD(2:end) );
+    accelX_PSD_mean_dB = 10*log10( accelX_PSD_mean );
+    accelY_PSD_mean = mean( accelY_PSD(2:end) );
+    accelY_PSD_mean_dB = 10*log10( accelY_PSD_mean );
+    accelZ_PSD_mean = mean( accelZ_PSD(2:end) );
+    accelZ_PSD_mean_dB = 10*log10( accelZ_PSD_mean );
+    gyroX_PSD_mean = mean( gyroX_PSD(2:end) );
+    gyroX_PSD_mean_dB = 10*log10( gyroX_PSD_mean );
+    gyroY_PSD_mean = mean( gyroY_PSD(2:end) );
+    gyroY_PSD_mean_dB = 10*log10( gyroY_PSD_mean );
+    gyroZ_PSD_mean = mean( gyroZ_PSD(2:end) );
+    gyroZ_PSD_mean_dB = 10*log10( gyroZ_PSD_mean );
+
+    %Display the average power spectral densities in dB
+    disp( ' ' );
+    disp( [ 'Average PSD for X Acceleration Data Noise (dB): ' , num2str(accelX_PSD_mean_dB) ] );
+    disp( [ 'Average PSD for Y Acceleration Data Noise (dB): ' , num2str(accelY_PSD_mean_dB) ] );
+    disp( [ 'Average PSD for Z Acceleration Data Noise (dB): ' , num2str(accelZ_PSD_mean_dB) ] );
+    disp( [ 'Average PSD for Angular Velocity about X Data Noise (dB): ' , num2str(gyroX_PSD_mean_dB) ] );
+    disp( [ 'Average PSD for Angular Velocity about Y Data Noise (dB): ' , num2str(gyroY_PSD_mean_dB) ] );
+    disp( [ 'Average PSD for Angular Velocity about Z Data Noise (dB): ' , num2str(gyroZ_PSD_mean_dB) ] );
+    disp( ' ' );
+
+    disp( ' ' );
+    disp( [ 'Average PSD for X Acceleration Data Noise : ' , num2str(accelX_PSD_mean) ] );
+    disp( [ 'Average PSD for Y Acceleration Data Noise : ' , num2str(accelY_PSD_mean) ] );
+    disp( [ 'Average PSD for Z Acceleration Data Noise : ' , num2str(accelZ_PSD_mean) ] );
+    disp( [ 'Average PSD for Angular Velocity about X Data Noise : ' , num2str(gyroX_PSD_mean) ] );
+    disp( [ 'Average PSD for Angular Velocity about Y Data Noise : ' , num2str(gyroY_PSD_mean) ] );
+    disp( [ 'Average PSD for Angular Velocity about Z Data Noise : ' , num2str(gyroZ_PSD_mean) ] );
+    disp( ' ' );
+
+
+
+end
+