function [ Kd ] = calcDragConstant( data, Pmin, Pmax, Rm, Kv, Kq, If )
%CALC_DRAG_CONSTANT 
%   Calculates the drag constant (Kd) given experimental data. The drag
%   constant is described in detail in sections 4.2.5, 4.2.6, and 5.5.4.1 
%   of "Model development, system identification, and control of a 
%   quadrotor helicopter" by Matt Rich.

%   Input Arguments:
%   data: experimental data
%   Pmax: Calculated maximum duty cycle of the function generators PWM wave
%   that the ESC can handle.
%   Pmin: Calculated minimum duty cycle of the function generators PWM wave
%   for initialization of the ESC.
%   Rm: Motor resistance
%   Kv: Back-EMF constant of the motor
%   Kq: Torque constant of the motor
%   If: No-Load (friction) current of the motor


% Convert RPM to angular speed of each rotor.
rotor_speed_0 = data.(2) * (pi/30);
rotor_speed_1 = data.(3) * (pi/30);
rotor_speed_2 = data.(4) * (pi/30);
rotor_speed_3 = data.(5) * (pi/30);

% Refer to the sections described in the header of this function
% for a better understanding of what this loop is doing. Note that these
% equations for each individual rotor. Calculating the total drag constant
% requires taking into account all four motors. In this function this is
% done by stacking the data of each motor in a single array and performing
% a least squares approximation of all of the data.
%
%       u: Defined in section 4.2.5
%       omega: vector of each rotors speed in rad/s
%       A: column vector of each rotors speed squared
%       b: Defined in section 5.5.4.1
%       Kd_vector: Vector containing all experimental Kd values
%       Vb: Battery voltage
u1 = ((data.(1)/100) - Pmin)/(Pmax - Pmin);
u2 = ((data.(1)/100) - Pmin)/(Pmax - Pmin);
u3 = ((data.(1)/100) - Pmin)/(Pmax - Pmin);
u4 = ((data.(1)/100) - Pmin)/(Pmax - Pmin);
u = [u1; u2; u3; u4];
Vb = [data.(6); data.(6); data.(6); data.(6)];
omega = [rotor_speed_0; rotor_speed_1; rotor_speed_2; rotor_speed_3];
A = omega.^2;
b = ((u.*Vb)/(Rm*Kq))-omega./(Rm*Kq*Kv)-If/Kq;

Kd = ((A'*A)^-1)*A'*b
end