function [ Kt ] = calcThrustConstant( data )
%CALC_THRUST_CONSTANT
%   Calculates the thrust constant (Kt) given experimental data. The thrust
%   constant is described in detail in sections 4.2.1.1 and 5.5.1 of 
%   "Model development, system identification, and control of a quadrotor
%   helicopter" by Matt Rich.

%   Input Arguments:
%   data: experimental data

% 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);

% Define the A matrix as the sum of each rotors speed squared.
A = (rotor_speed_0.^2 + rotor_speed_1.^2 + rotor_speed_2.^2 + rotor_speed_3.^2);

% Convert weight (g) to thrust force (N) by converting grams to kilograms and
% multiplying by the acceleration of gravity.
T = (data.Scale_g_/1000)*9.8;

% Calculate the thrust constant (Kt) through the following
% equation: Kt = (A'A)^-1.*A'.*T as defined on page 65 of "Model
% development, system identification, and control of a quadrotor
% helicopter" (Matt Rich's Thesis).
Kt = ((A'*A)^(-1))*(A'*T) 

end