Added MATLAB scripts for parameter identification

MATLAB/thrustAndDragConstant/calc_drag_constant.m

+function [ Kd ] = calc_drag_constant( 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
+

MATLAB/thrustAndDragConstant/calc_thrust_constant.m

+function [ Kt ] = calc_thrust_constant( 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
+

MATLAB/thrustAndDragConstant/thrust_drag_constant_calculations.m

+% Import data as a table.
+data = readtable('C:\Users\Andy\Documents\MATLAB\MicroCART\Thrust and Drag Constant\Thrust and Drag Constant Data.xlsx');
+
+filePath_noLoadCurrentData = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv';
+
+[I_0, I_1, I_2] = zero_load_current(filePath_noLoadCurrentData);
+
+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 Pmin and Pmax, as well as the motor parameters Rm, Kv, Kq, and If
+Pmin = 0.40;
+Pmax = 0.8;
+Rm = 0.2308;
+Kv = 96.3422;
+Kq = 96.3422;
+If0 = I_0 * sign(rotor_speed_0) + I_1 * rotor_speed_0 + I_2 * rotor_speed_0.^2;
+If1 = I_0 * sign(rotor_speed_1) + I_1 * rotor_speed_1 + I_2 * rotor_speed_1.^2;
+If2 = I_0 * sign(rotor_speed_2) + I_1 * rotor_speed_2 + I_2 * rotor_speed_2.^2;
+If3 = I_0 * sign(rotor_speed_3) + I_1 * rotor_speed_3 + I_2 * rotor_speed_3.^2;
+If = [If0; If1; If2; If3];
+
+% Call the calc_thrust_constant() function.
+Kt = calc_thrust_constant(data);
+
+% Call the calc_drift_constant() function.
+Kd = calc_drag_constant(data, Pmin, Pmax, Rm, Kv, Kq, If );
+

MATLAB/zeroLoadCurrent/Kd_error.m

+data = readtable('C:\Users\Andy\Documents\MATLAB\MicroCART\Thrust and Drag Constant\Thrust and Drag Constant Data.xlsx');
+
+filePath_noLoadCurrentData = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv';
+
+[I_0, I_1, I_2, dutyCycle_error, error, residual_error, residual_error_withIfConstant] = zero_load_current(filePath_noLoadCurrentData);
+
+Pmin = 0.40;
+Pmax = 0.8;
+Rm = 0.2308;
+Kv = 96.3422;
+Kq = 96.3422;
+If = 0.44;
+
+% 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);
+
+duty_cycle_percentage = data.(1);
+Vb = data.(6);
+
+u = ((data.(1)/100) - Pmin)/(Pmax - Pmin);
+
+If = I_0 * sign(rotor_speed_0) + I_1 * rotor_speed_0 + I_2 * rotor_speed_0.^2;
+w_num = -1 + sqrt( 1 - 4*Rm*Kv*Kq*Kd*(Kv*Rm*If - Kv*u.*Vb));
+w_den = 2*Rm*Kv*Kq*Kd;
+w = w_num / w_den;
+
+figure()
+hold on
+plot(duty_cycle_percentage, w); grid()
+plot(duty_cycle_percentage, rotor_speed_0);
+plot(duty_cycle_percentage, rotor_speed_1);
+plot(duty_cycle_percentage, rotor_speed_2);
+plot(duty_cycle_percentage, rotor_speed_3);
+
+xlabel('Duty Cycle Percentage');
+ylabel('Rotor Speed (rad/s)')
+
+figure()
+plot(dutyCycle_error, error); grid()
+xlabel('Duty Cycle Percentage');
+ylabel('Error');
+
+residual_error
+
+residual_error_withIfConstant
\ No newline at end of file

MATLAB/zeroLoadCurrent/zero_load_current.m

+function [I_0, I_1, I_2, dutyCycle, error, residual_error, residual_error_withIfConstant] = zero_load_current(absoluteFilePath)
+%This function takes in an absolute file path to a .csv or Excel file with 
+%the following format:
+%   Second column should be the motor speed in revolutions per minute
+%   Third column should be the current (A)
+%absoluteFilePath = 'C:\Users\Andy\Documents\MATLAB\MicroCART\Zero Load Current\No Load Friction Current.csv';
+
+    %Get the data from the file
+    data = readtable(absoluteFilePath);
+
+    %Extract the motor speed column and convert it to radians per second
+    dutyCycle = data.(1);
+    motorSpeed_rpm = data.(2);
+    motorSpeed_radPerSec = motorSpeed_rpm * pi/30;
+
+    %Extract the current column
+    current = data.(3);
+
+    %Define the three columns of the A matrix for the least squares 
+    %approximation, using this equation:
+    %   
+    %   I_f = sgn(motorSpeed)*I_0 + motorSpeed*I_1 + (motorSpeed^2)*I_2
+    %
+    %   for this least squares equation: A*x = b (where A and b are matrices)
+    %
+    % where: sgn(motorSpeed) -- first column of the A matrix,
+    %        motorSpeed      -- second column of the A matrix
+    %        motorSpeed^2    -- third column of the A matrix
+    %        I_f             -- b matrix
+    %
+    %  and: [I_0; I_1; I_2]  -- x vector that we are solving for
+    A_col1 = sign(motorSpeed_radPerSec);
+    A_col2 = motorSpeed_radPerSec;
+    A_col3 = motorSpeed_radPerSec .^ 2;
+
+    %Create the A matrix from its three columns
+    A = [A_col1, A_col2, A_col3];
+
+    %Define the b matrix
+    b = current;
+
+    %Least squares approximation -- solving for x, which is I_vector
+    I_vector = (A' * A)^(-1) * A' * b;
+
+    %Get the error vector
+    error = A*I_vector - b;
+    
+    If_constant = 0.511;
+    error_withIfConstant = A*[If_constant; 0; 0] - b;
+    
+    %Get the residual (?)
+    residual_error = sum(error.^2);
+    
+    residual_error_withIfConstant = sum(error_withIfConstant.^2);
+
+    %Extract the components of the vector
+    I_0 = I_vector(1);
+    I_1 = I_vector(2);
+    I_2 = I_vector(3);
+
+end %function