function[ ] = BNO055_NoiseProfile()
    
    %---------------------------------------------------------------------
    % Clean workspace
    %---------------------------------------------------------------------
    close all
    clear fig
    clc
    
    % Description:
    %
    % Step 1: import byte data from text file
    % Step 2: convert bytes to floats
    % Step 3: apply correction offsets and scaling factors
    % Step 4: filter the data
    % Step 5: numerically integrate to obtain velocity estimate
    % Step 6: compute error based on desired velocity profile
    
    %--------------------------------------------------------------------------
    % File name and directory
    %--------------------------------------------------------------------------
        %Name of file to import
        fileName = 'H:\01-Jun-2011-02-00-36';
        
        fprintf('File name and directory: %s\n', fileName);
    
    %---------------------------------------------------------------------
    % Import sensor data and convert int16 to doubles
    %---------------------------------------------------------------------
        fprintf('Importing sensor data...\n');
        
        [accelX_int16, accelX_double] = bytes2float(strcat(fileName,'_accelX.txt'));
        [accelY_int16, accelY_double] = bytes2float(strcat(fileName,'_accelY.txt'));
        [accelZ_int16, accelZ_double] = bytes2float(strcat(fileName,'_accelZ.txt'));

        % Raw sensor output
        dataRaw = [accelX_int16, accelY_int16, accelZ_int16];

        % Sample rate
        fs = 100; % [Hz]

        % Identify the data range of samples for processing data
        readRawData = [1, length(dataRaw)];

        
        fprintf('Applying offsets and scaling factors...\n');
        
        % Correct for linear offset and scaling of raw data
        dataOffset = removeOffset(dataRaw);

    %---------------------------------------------------------------------
    % Filter the data
    %---------------------------------------------------------------------
        fprintf('Filtering data...\n');
        
        % length of DFT
        nfft = 8192;
    
        % De - noise filter
        mu = 0.005;
        Dx = denoiseFilter(dataOffset(:,1), mu);
        Dy = denoiseFilter(dataOffset(:,2), mu);
        Dz = denoiseFilter(dataOffset(:,3), mu);
        dataDenoise = [Dx, Dy, Dz];
        
        % check the freq response of the de-noise filter
        whiteGaussianNoise = randn(10000,1);
        wgn = denoiseFilter(whiteGaussianNoise,mu);

        % Moving average filter
        [MAx, Hx, Fx] = movingAverageFilter(double(dataOffset(:,1)), 100, nfft, fs);
        [MAy, Hy, Fy] = movingAverageFilter(double(dataOffset(:,2)), 100, nfft, fs);
        [MAz, Hz, Fz] = movingAverageFilter(double(dataOffset(:,3)), 100, nfft, fs);
        dataMVA = [MAx, MAy, MAz];

    %---------------------------------------------------------------------
    % Power Spectrum Density Analysis
    %---------------------------------------------------------------------
        fprintf('Compute PSD...\n');
    
        dataDetrendDenoise = zeros(size(dataRaw,1),3);
        dataDetrendMVA = zeros(size(dataRaw,1),3);
        dataDetrendRaw = zeros(size(dataRaw,1),3);

        dataDetrendDenoise = detrend(dataDenoise);
        dataDetrendMVA = detrend(dataMVA);
        dataDetrendRaw = detrend(double(dataRaw));

        % length of DFT
        %nfft = 256;

        % time values form input signal
        time = 1/fs.*size(dataRaw,1); % [sec]

        % length of window
        window = hamming(nfft);

        % number of overlapped samples
        noverlap = length(window)/2;
        
        % check the freq response of the de-noise filter
        whiteGaussianNoise = randn(10000,1);
        wgn = denoiseFilter(whiteGaussianNoise,mu);
        
        % PSD of whiteGaussianNoise and Denoise Filter
        [wgnWelch,f_wgnWelch] = pwelch(whiteGaussianNoise, window, noverlap, nfft, fs);
        [denoiseFilterWelch,f_denoiseFilterWelch] = pwelch(wgn, window, noverlap, nfft, fs);

        % PDS of raw and detrend data
        for j =1:3
            [dataWelchDenoise(:,j),f_detrend] = pwelch(dataDetrendDenoise(:,j), window, noverlap, nfft, fs);
            [dataWelchMVA(:,j),f_detrend] = pwelch(dataDetrendMVA(:,j), window, noverlap, nfft, fs);
            [dataWelchRaw(:,j),f_detrend] = pwelch(dataDetrendRaw(:,j), window, noverlap, nfft, fs);
        end

    %---------------------------------------------------------------------
    % Calcualte average, RMS, std, CI
    %---------------------------------------------------------------------
        calibrationData = dataRaw;

        data_avg = zeros(length(readRawData),3);
        data_rms = zeros(length(readRawData),3);
        data_std = zeros(length(readRawData),3);
        data_CI = zeros(length(readRawData),3);
        distance = zeros(length(readRawData),1);
    
        alpha = 0.05;

        for p = 1:size(readRawData,1)

            for q = 1:3
                x = calibrationData(readRawData(p,1):readRawData(p,2),q);
                data_avg(p,q) = mean(double(x));
                data_rms(p,q) = rms(double(x));
                data_std(p,q) = std(double(x));
                data_CI(p,q) = confidence_T(alpha,  double(x), 2); % use 2-tail t-test

            end

            distance(p,1) = p*50; % [mm]

        end
    
    %---------------------------------------------------------------------
    % Plot the data
    %---------------------------------------------------------------------
        % Custom colors for plotting
            r1 = [255/255 51/255 51/255];
            g1 = [153/255 255/255 51/255];
            b1 = [0/255 51/255 204/255];
        
            r2 = [153/255 0/255 51/255];
            g2 = [0/255 153/255 51/255];
            b2 = [0/255 204/255 255/255];
           
            % turn on color picker
            %  c = uisetcolor('Select a color')

            %% Plot the raw data
            figure(1);
            hold on
            plot(dataRaw(:,1), 'r');
            plot(dataRaw(:,2), 'g');
            plot(dataRaw(:,3), 'b');
            
%             plot(dataOffset(:,1),'Color', r1, 'LineStyle', '--');
%             plot(dataOffset(:,2), 'Color', g1, 'LineStyle', '--');
%             plot(dataOffset(:,3), 'Color', b1, 'LineStyle', '--');

            title('BNO055 Accelerometer Raw Data');
            xlabel('Samples');
            ylabel('Acceleration (m/sec^2)');
            
            %legend('x-axis: Raw', 'y-axis: Raw', 'z-axis: Raw', 'x-axis: Offset', 'y-axis: Offset', 'z-axis: Offset', 'Location', 'best');
            legend('x-axis: Raw', 'y-axis: Raw', 'z-axis: Raw','Location', 'best');

            grid;
            hold off
            
            %% Plot the Welch spectrum of raw data
            figure(2)
            hold on
            
            plot(f_detrend, 10*log10(dataWelchRaw(:,1)), 'r')
            plot(f_detrend, 10*log10(dataWelchRaw(:,2)), 'g')
            plot(f_detrend, 10*log10(dataWelchRaw(:,3)), 'b')
                                  
            title('Welch Method - Raw Data');
            xlabel('Frequency (Hz)')
            ylabel('Magnitude (dB)')
            legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
            grid;
            hold off

                       
            %% Plot the Welch spectrum after filtering
            figure(3)
            hold on
            
            plot(f_detrend, 10*log10(dataWelchDenoise(:,1)), 'Color', r1)
            plot(f_detrend, 10*log10(dataWelchDenoise(:,2)), 'Color', g1)
            plot(f_detrend, 10*log10(dataWelchDenoise(:,3)), 'Color', b1)
            
            plot(f_detrend, 10*log10(dataWelchMVA(:,1)), 'Color', r2)
            plot(f_detrend, 10*log10(dataWelchMVA(:,2)), 'Color', g2)
            plot(f_detrend, 10*log10(dataWelchMVA(:,3)), 'Color', b2)
            
            title('Welch Method - Detrend Data');
            xlabel('Frequency (Hz)')
            ylabel('Magnitude (dB)')
            legend('x-axis: Denoise', 'y-axis: Denoise', 'z-axis: Denoise','x-axis: MVA', 'y-axis: MVA', 'z-axis: MVA', 'Location', 'best');
            grid;
            hold off
            
            %% Plot the filtered data
            figure(4);
            hold on
            
            plot(dataDenoise(:,1), 'Color', r1);
            plot(dataDenoise(:,2), 'Color', g1);
            plot(dataDenoise(:,3), 'Color', b1);
            
            plot(dataMVA(:,1), 'Color', r2, 'LineStyle', '--');
            plot(dataMVA(:,2), 'Color', g2, 'LineStyle', '--');
            plot(dataMVA(:,3), 'Color', b2, 'LineStyle', '--');

            xlabel('Samples');
            ylabel('Acceleration (mm/sec^2)');
            title('BNO055 Accelerometer Data Affter Applying Filtering');
            legend( ...
                    'x-axis: Denoise', 'y-axis: Denoise', 'z-axis: Denoise',...
                    'x-axis: MVA', 'y-axis: MVA', 'z-axis: MVA', 'Location', 'best');
                
            grid
            hold off
            
            %% Plot the PSD of the De-noise filter
            figure(5)
            hold on
            
            plot(f_wgnWelch, 10*log10(wgnWelch), 'Color', r1)
            plot(f_denoiseFilterWelch, 10*log10(denoiseFilterWelch), 'Color', b1)
            
            title('Frequency Response of Denoise Filter');
            xlabel('Frequency (Hz)')
            ylabel('Magnitude (dB)')
            
            legend('White Gaussian Noise', 'De-noise filter response', 'Location', 'best');
            grid;
            hold off
            
            %% Plot the PSD of the MVA filter
            figure(6)
            hold on
            
            plot(Fx,10*log10(Hx),'r-');
            plot(Fy,10*log10(Hy),'g-');
            plot(Fz,10*log10(Hz),'b-');
            
            title('Frequency Response of Moving Average Filters');
            xlabel('Frequency (Hz)')
            ylabel('Magnitude (dB)')
            
            legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
            grid;
            hold off
            
            %%
            
%             % Ploy bar graph of Average
%             bar(data_avg, 'r')
%             Labels = {'x-axis', 'y-axis', 'z-axis'};
%             set(gca, 'XTick', 1:3, 'XTickLabel', Labels);
%             set(gca, 'XTickLabel', Labels);
%             title('Frequency Response of Moving Average Filters');
%             xlabel('Axes XYZ')
%             ylabel('Acceleration (mm/sec^2)')
%             text(x,y',num2str(y','%0.2f'),'HorizontalAlignment','center', 'VerticalAlignment','bottom')
%             
            
            %% Plot bar graph of RMS
            
            %% Plot bar graph of STD
            
            
            %% Plot the RMS values
%             figure(3);
%             hold on
%             plot(data_rms(:,1), 'ro-');
%             plot(data_rms(:,2), 'go-');
%             plot(data_rms(:,3), 'bo-');
% 
%             xlabel('Linear Table Distance (mm)');
%             ylabel('Acceleration (mm/sec^2)');
%             title('RMS of Sensor Ouput Values');
%             legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
%             grid;
%             hold off

            %% Plot the STD values
%             figure(4);
%             hold on
%             plot(data_std(:,1), 'ro-');
%             plot(data_std(:,2), 'go-');
%             plot(data_std(:,3), 'bo-');
% 
%             xlabel('Linear Table Distance (mm)');
%             ylabel('Acceleration (mm/sec^2)');
%             title('Standard Deviation of Sensor Output Values');
%             legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
%             grid;
%             hold off

            %% Plot the CI values
%             figure(5);
%             hold on
%             errorbar(data_avg(:,1), data_CI(:,1), 'ro-');
%             errorbar(data_avg(:,2), data_CI(:,2), 'go-');
%             errorbar(data_avg(:,3), data_CI(:,3), 'bo-');
% 
%             xlabel('Linear Table Distance (mm)');
%             ylabel('Acceleration (mm/sec^2)');
%             title('95% CI Values');
%             legend('x-axis', 'y-axis', 'z-axis', 'Location', 'best');
%             grid
%             hold off
        
        
        
    %---------------------------------------------------------------------
    % Function Definitions
    %---------------------------------------------------------------------
    
        % Calcualte the confidence interval
        function[CI] = confidence_T(alpha, input_data, tail)

            % reference: t_inv - https://www.mathworks.com/matlabcentral/answers/260564-how-to-implement-tinv-manually-without-statistics-and-machine-learning-toolbox
            % Inputs:
            %
            % alpha
            % standard_dev: standard deviation of sample
            % size: number of data points in sample
            % tail:
            %    if tail = 1; 1-tailed t-distribution
            %    if tail = 2; 2-tailed t-distribution

            % Local functions
            tdist2T = @(t,v) (1-betainc(v/(v+t^2),v/2,0.5));        % 2-tailed t-distribution
            tdist1T = @(t,v) 1-(1-tdist2T(t,v))/2;                  % 1-tailed t-distribution

            % Calculations
            v = length(input_data)-1;                               % degrees of freedom
            SEM = std(input_data)/sqrt(length(input_data));         % Standard Error

            if tail == 1
                t_inv = @(alpha,v) fzero(@(tval) (max(alpha,(1-alpha)) - tdist1T(tval,v)), 5);  % T-Statistic Given Probability ‘alpha’ & Degrees-Of-Freedom ‘v’
                ts = t_inv(alpha,v);                                % T-Score
            end

            if tail == 2
                t_inv = @(alpha,v) fzero(@(tval) (max(alpha,(1-alpha)) - tdist2T(tval,v)), 5);  % T-Statistic Given Probability ‘alpha’ & Degrees-Of-Freedom ‘v’
                ts = t_inv(alpha,v);                                % T-Score
            end

            CI = ts*SEM;                         % Confidence Intervals
        end

        % Import IMU data - output floats
        function[data_int16, data_double] = bytes2float(fileName)

            % SD Card directory
            filePath = fileName; %strcat('H:\', fileName);
            fileID = fopen(filePath);

            % Directory on computer - DOWNLOADS
            %fileID = fopen('C:\Users\nraymond\Downloads\01-Jun-2011-01-00-30_accelZ.txt');

            % Read the file, assuming signed 8 bit integers
            A = fread(fileID);

            % Close the file
            fclose('all');

            numSamples = length(A)/2;

            %Data_byte = zeros(numSamples,2);
            data_double = zeros(numSamples,1);

            index = 1;
            for k=1:2:numSamples*2
                MyByteArray = [A(k) A(k+1)];
                MyByteArray = uint8(MyByteArray);
                MySingle = typecast(MyByteArray,'int16');
                accelDouble = double(MySingle)/100.0; % [m/sec^2]

                data_int16(index,1) = MySingle;
                data_double(index,1) = accelDouble;

                index = index + 1;
            end

        end
        
        % Filter to remove noise from signal - BLACK BOX
        function [ f ] = denoiseFilter( g, mu )
            
            I = length(g);
            u = zeros(I,1);
            y = zeros(I,1);
            rho = 10;

            eigD = abs(fftn([-1;1],[I 1])).^2;

            for k=1:100
                f = real(ifft(fft(mu*g+rho*Dt(u)-Dt(y))./(mu+rho*eigD)));
                v = D(f)+(1/rho)*y;
                u = max(abs(v)-1/rho,0).*sign(v);
                y = y - rho*(u-D(f));
            end

            function y = D(x)
                y = [diff(x);x(1)-x(end)];
            end

            function y = Dt(x)
                y = [x(end)-x(1);-diff(x)];
            end

        end
        
        %% Apply moving average filter
        function [ mva, H, F ] = movingAverageFilter(inputSignal, movingAverageSize, nfft, fs)

        % Define moving average filter
        a = 1;
        b = ones(1,movingAverageSize)./movingAverageSize;
        
        % Frequency response of filter
        [H,F] = freqz(b,a,nfft,fs);

        % Apply moving average filter
        mva = filter(b,a,inputSignal);
    end

        % Correct for offsets and scaling
        function[offsetData] = removeOffset(rawData)
            % reference: http://cache.freescale.com/files/sensors/doc/app_note/AN4399.pdf
            g = 980.7; % [mm/sec^2]

            % Calibration values obtained from calibration in six positions
            Gfx = [948.6, -989.1];
            Gfy = [1129.0, -792.2];
            Gfz = [984.7, -981.4];

            % Gain value
            Wx = 2 / (Gfx(1)/1000.0 - Gfx(2)/1000.0);
            Wy = 2 / (Gfy(1)/1000.0 - Gfy(2)/1000.0);
            Wz = 2 / (Gfz(1)/1000.0 - Gfz(2)/1000.0);

            % Linear offset value
            Vx = -(Gfx(1) + Gfx(2))/(Gfx(1) - Gfx(2))*g;
            Vy = -(Gfy(1) + Gfy(2))/(Gfx(1) - Gfy(2))*g;
            Vz = -(Gfz(1) + Gfz(2))/(Gfx(1) - Gfz(2))*g;

            Gx = zeros(length(rawData),1);
            Gy = zeros(length(rawData),1);
            Gz = zeros(length(rawData),1);

            % Apply gain and offsets
            Gx(:,1) = Wx.*rawData(:,1) + Vx;
            Gy(:,1) = Wy.*rawData(:,2) + Vy;
            Gz(:,1) = Wz.*rawData(:,3) + Vz;

            offsetData = [Gx, Gy, Gz];

        end

        
end