% Log Analysis Toggle    
    logAnalysisToggle = 1;          % 1 for log analysis, 0 for normal operation
    
% Model Parameters
    m = 1.244;                      % Quadrotor + battery mass
    g = 9.81;                       % Acceleration of gravity
    Jxx = 0.0130;                   % Quadrotor and battery motor of inertia around bx (pitch)
    Jyy = 0.0140;                   % Quadrotor and battery motor of inertia around by (roll)
    Jzz = 0.0285;                   % Quadrotor and battery motor of inertia around bz (yaw)
    Jreq = 4.2012e-05;              % Rotor and motor moment of inertia around axis of rotation 
    Kt = 8.6519e-6;                 % Rotor thrust constant
    Kh = 0;                         % Rotor in-plane drag constant
    Kd = 1.0317e-7;                 % Rotor drag constant
    rhx = 0.16;                     % X-axis distance from center of mass to a rotor hub
    rhy = 0.16;                     % Y-axis distance from center of mass to a rotor hub
    rhz = 0.03;                     % Z-axis distance from center of mass to a rotor hub
    Rm = 0.2308;                    % Motor resistance
    Kq = 96.3422;                   % Motor torque constant
    Kv = 96.3422;                   % Motor back emf constant
    If = 0.511;                     % Motor internal friction current
    Pmin = 1e5;                     % Minimum zybo output duty cycle command
    Pmax = 2e5;                     % Maximum zybo output duty cycle command
    Tc = 0.04;                      % Camera system sampling period
    Tq = 0.005;                     % Quad sampling period
    tau_c = 0;                      % Camera system total latency
    Vb = 11.1;                      % Nominal battery voltage (V)
    x_controlled_o = 0;             % Equilibrium lateral controller output
    y_controlled_o = 0;             % Equilibrium longitudinal controller output
    yaw_controlled_o = 0;           % Equilibrium yaw controller output
    omega_o = sqrt((m*g)/(4*Kt));   % Equilibrium Rotor Speed
    
    % Equilibrium height controller output
    height_controlled_o = (((Rm*If + ...
    + (((omega_o * 2 * Rm * Kv * Kq  ...
    * Kd + 1)^2) - 1)/(4* Rm*Kv^2*Kq ...
    *Kd))/Vb)*(Pmax- Pmin)+Pmin);

% Import Data and determine errors
if logAnalysisToggle == 1
    
    % Import Data to Workspace
    data = importdata('loggingAnalysis/logFiles/logData.csv');

    % Set up time vector
    time = data.data(:, 1);

    % Determine x position error
    x_setpoint = data.data(:, 25);
    x_position = data.data(:, 20);
    x_error = timeseries(x_setpoint - x_position, time);

    % Determine y position error
    y_setpoint = data.data(:, 26);
    y_position = data.data(:, 21);
    y_error = timeseries(y_setpoint - y_position, time);

    % Determine z position error
    z_setpoint = data.data(:, 27);
    z_position = data.data(:, 22);
    z_error = timeseries(z_setpoint - z_position, time);

    % Determine pitch error
    pitch_setpoint = data.data(:, 9);
    pitch_value = data.data(:, 23);
    pitch_error = timeseries(pitch_setpoint - pitch_value, time);

    % Determine roll error
    roll_setpoint = data.data(:, 10);
    roll_value = data.data(:, 24);
    roll_error = timeseries(roll_setpoint - roll_value, time);

    % Determine yaw error
    yaw_setpoint = zeros(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW SETPOINT
    yaw_value = ones(length(time), 1); % NEEDS UPDATED WHEN LOG FILE INCLUDES YAW VALUE
    yaw_error = timeseries(yaw_setpoint - yaw_value, time);

    % Determine pitch rate error
    pitchrate_setpoint = data.data(:, 11);
    pitchrate_value = data.data(:, 6);
    pitchrate_error = timeseries(pitchrate_setpoint - pitchrate_value, time);

    % Determine roll rate error
    rollrate_setpoint = data.data(:, 12);
    rollrate_value = data.data(:, 5);
    rollrate_error = timeseries(rollrate_setpoint - rollrate_value, time);

    % Determine yaw rate error
    yawrate_setpoint = data.data(:, 13);
    yawrate_value = data.data(:, 7);
    yawrate_error = timeseries(yawrate_setpoint - yawrate_value, time);

    % Pull motor commands from log
    x_command = data.data(:, 14);
    y_command = data.data(:, 15);
    z_command = data.data(:, 8);
    yaw_command = data.data(:, 16);
end
