Intialized the transfer!

Aero/misc_aero_calcs.m

+% Misc_Aero_Calcs
+
+%% Parameters
+p_f = 1.225;        % kg / m^3
+mu = 1.802e-5;      % kg / m s
+d = 0.246;          % m
+v_max = 10;         % m / s
+R_max = 2e5;        % 
+
+
+%% Reynolds Number given speed
+% R = p_f * v_max * d / mu;
+
+%% Speed given reynolds number
+% v = mu * R_max / p_f / d;
+
+%% Plot everything against velocity
+v = 0.5:0.01:12;   % 0.01 up to 12.0 m/s
+
+R = p_f .* v .* d ./ mu;
+
+C_D = (24 ./ R) .* (1 + 0.15 .* R .^ 0.681) + 0.407 ./ (1 + 8710./R);
+C_D_2 =   (8./R) .* (1./ sqrt(phi_par))...
+     + (16./R) .* (1./ sqrt(phi))...
++ (3./sqrt(R)) .* (1./ phi.^(3./4))...
++ 0.421.^(0.4 .* (- log(phi) ).^2 )...
+    .* (1 ./ phi_perp);
+
+F_D = (1/8) .* C_D .* pi .* d.^2 .* p_f .* v.^2;
+F_D_2 = (1/8) .* C_D_2 .* pi .* d.^2 .* p_f .* v.^2;
+
+figure(1);
+plot(v, R);
+xlabel('Relative Wind Speed (m/s)');
+ylabel('Reynolds Number');
+% title('Reynolds Number');
+
+figure(2);
+plot(v, C_D);
+xlabel('Relative Wind Speed (m/s)');
+ylabel('Coefficient of Drag');
+% title('Coefficient of Drag');
+
+figure(3);
+plot(v, F_D);
+xlabel('Relative Wind Speed (m/s)');
+ylabel('Drag Force (N)');
+% title('Drag Force');
+
+figure(4);
+plot(v, C_D_2);
+xlabel('Relative Wind Speed (m/s)');
+ylabel('Coefficient of Drag');
+% title('Coefficient of Drag');
+
+figure(5);
+plot(v, F_D_2);
+xlabel('Relative Wind Speed (m/s)');
+ylabel('Drag Force (N)');
+
+
+

Aero/paper_4_nonsphere_drag_calcs.m

+% Paper 4 - New simple correlation formula for the drag coefficient of non-spherical particles
+
+%% phi values for cubes
+phi = (pi/6)^(1/3);
+phi_perp = (9*pi/16)^(1/3);
+phi_par = ((9*pi/16)^(1/3) ) / (3 - (4/pi));
+
+%% C_D
+p_f = 1.225;        % kg / m^3
+mu = 1.802e-5;      % kg / m s
+d = 0.246;          % m
+v_max = 10;         % m / s
+R_max = 2e5;        % 
+v = 0.5:0.01:12;   % 0.01 up to 12.0 m/s
+R = p_f .* v .* d ./ mu;
+
+C_D =   (8./R) .* (1./ sqrt(phi_par))...
+     + (16./R) .* (1./ sqrt(phi))...
++ (3./sqrt(R)) .* (1./ phi.^(3./4))...
++ 0.421.^(0.4 .* (- log(phi) ).^2 )...
+    .* (1 ./ phi_perp);
+      
+
+
+%% Calc 4 phi_parallel
+
+% Let's say we have a cube being rotated and we want to find the average
+% parallel cross section. 
+% 
+% s = 1;
+% theta = 0:0.01:2*pi;
+% 
+% 
+% x1 = calc_x(theta + pi/4);
+% % y1 = calc_y(theta);
+% x2 = calc_x(theta + pi/2 + pi/4);
+% % y2 = calc_y(theta + pi/2);
+% x3 = calc_x(theta + pi + pi/4);
+% % y3 = calc_y(theta + pi);
+% x4 = calc_x(theta + 3*pi/2 + pi/4);
+% % y4 = calc_y(theta + 3*pi/2);
+% 
+% max_x = max( max(x1,x2), max(x3,x4) );
+% min_x = min( min(x1,x2), min(x3,x4) );
+% 
+% %%
+% % figure(1);
+% % plot(theta,x1);
+% % hold on;
+% % plot(theta,x2);
+% % plot(theta,x3);
+% % plot(theta,x4);
+% % hold off;
+% % legend('x1','x2','x3','x4');
+% % 
+% % figure(2);
+% % plot(theta,max_x);
+% % hold on;
+% % plot(theta,min_x);
+% % hold off;
+% % legend('max x','min x');
+% 
+% figure(3);
+% plot(theta,max_x - min_x);
+% legend('length');
+% 
+% %%
+% % figure(4);
+% % plot(theta, (sqrt(2)-s).*sin(2.*theta) + s);
+% % hold on;
+% % plot(theta, -(sqrt(2)-s).*sin(2.*theta) + s);
+% % plot(theta,max_x - min_x);
+% % title('Length with function');
+% 
+% % %%
+% % figure(5);
+% % plot(theta, sqrt(2)./2.*sin(2.*theta) + 1);
+% % hold on;
+% % plot(theta, -sqrt(2)./2.*sin(2.*theta) + 1);
+% 
+% 
+% %%
+% % phi_par_avg = (sqrt(2)*s + (pi/2 - 1) * s * s) / (pi/2);
+% % 
+% % length_par_avg = (sqrt(2) + (pi/2 - 1) * s) / (pi/2);
+% % 
+% % calc_length_par_avg = mean(max_x - min_x);
+% 
+% 
+% function [xs] = calc_x(theta_in)
+%     xs =  sqrt(2) ./ 2 .* cos(theta_in);
+% end
+% 
+% 
+% function [ys] = calc_y(theta_in)
+%     ys =  sqrt(2) ./ 2 .* sin(theta_in);
+% end
\ No newline at end of file

Controller_Analysis/analysis.m

+% Analysis of the system from a Controls perspective (stability,
+% linearization, feedback design, etc)
+
+%% 1) Get Model
+params = parameters_APM_Quad();
+
+quadModel = @(x,u) multirotorModel(x,u,params);
+n = 12; % 12 state system
+m = 4;  % 4 control inputs
+
+%% 2) Linearize the model about stable hover
+xeq = zeros(n,1);
+ueq = params.hoverMotorCmd   * ones(m,1);
+
+[A,B] = symLin(quadModel, xeq, ueq);
+C = eye(n);
+
+display(A);
+display(B);
+
+%% 3) Check Stability, Controllability, Observability
+
+% Stability 
+lambdas = eig(A); % all 0, regular system is not stable
+
+% Controllability
+C0 = ctrb(A,B);
+if rank(C0) == n
+    display('System is Controllable');
+else
+    display('System is NOT Controllable');
+end
+
+% Observability
+Ob = obsv(A,C);
+if rank(Ob) == n
+    display('System is Observable');
+else
+    display('System is NOT Observable');
+end
+
+%% 4) Design a Controller (with lqr)
+Q = eye(n,n);
+R = eye(m,m);
+[K,~,~] = lqr(A,B,Q,R);
+
+%% 5) Check Stability of closed loop system
+A_ = A - B*K;
+lambdas = eig(A_); 

Controller_Analysis/symLin.m

+function [A, B] = symLin(model, xeq, ueq)
+%symLin
+
+syms x1 x2 x3 x4 x5 x6 x7 x8 x9 x10 x11 x12 
+syms u1 u2 u3 u4 
+syms A_ B_
+
+xdot = model([x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12],[u1;u2;u3;u4]);
+
+A_ = jacobian(xdot,[x1;x2;x3;x4;x5;x6;x7;x8;x9;x10;x11;x12]);
+B_ = jacobian(xdot,[u1;u2;u3;u4]);
+
+% Evaluate at equilibrium points
+x1 = xeq(1);
+x2 = xeq(2);
+x3 = xeq(3);
+x4 = xeq(4);
+x5 = xeq(5);
+x6 = xeq(6);
+x7 = xeq(7);
+x8 = xeq(8);
+x9 = xeq(9);
+x10 = xeq(10);
+x11 = xeq(11);
+x12 = xeq(12);
+
+u1 = ueq(1);
+u2 = ueq(2);
+u3 = ueq(3);
+u4 = ueq(4);
+
+% Obtain Numerical Constants 
+A = double(vpa(subs(A_),5));
+B = double(vpa(subs(B_),5));
+
+end
+

Controllers/PID.m

+classdef PID
+    %PID Class trying to replicate APM's AC_PID class
+% https://github.com/ArduPilot/ardupilot/blob/master/libraries/AC_PID/AC_PID.h
+
+% Author: Matthew Romano (mmroma@umich.edu)
+% Date: 1/17/2019
+
+% TODO: 
+    % Add back in feed-forward term
+    % Potentially add back in extra functions
+    
+    properties
+        % Public (need to add it)
+        kp
+        ki
+        kd
+        imax
+        filt_hz = 20;
+        ff = 0;
+        filt_val = 1;
+        
+        % Internal
+        dt
+        integrator = 0;
+        input = 0;
+        derivative = 0;
+        input_filt_change = 0;
+    end
+        
+    methods
+        function obj = PID(kp_,ki_,kd_,imax_,filt_hz_,dt_,ff_)
+            obj.kp = kp_;
+            obj.ki = ki_;
+            obj.kd = kd_;
+            obj.imax = imax_;
+            obj.filt_hz = filt_hz_;
+            obj.dt = dt_;
+            obj.ff = ff_;
+            
+            obj.filt_val = obj.get_filt_alpha();
+        end
+        
+        function obj = set_input_filter_all(obj,input)
+            % set input to PID controller input is filtered before the PID 
+            % controllers are run this should be called before any other 
+            % calls to get_p, get_i or get_d
+            
+            if (~isfinite(input))
+                return
+            end
+            
+            obj.input_filt_change = obj.filt_val * (input - obj.input);
+            obj.input = obj.input + obj.input_filt_change;
+            
+            if (obj.dt > 0)
+                obj.derivative = obj.input_filt_change / obj.dt;
+            end
+        end      
+        
+        function [obj] = set_input_filter_d(obj,input)
+            % set_input_filter_d - set input to PID controller
+            % only input to the D portion of the controller is filtered
+            % this should be called before any other calls to get_p, get_i or get_d
+            
+            if (~isfinite(input))
+                return
+            end
+            
+            if (obj.dt > 0)
+                der = (input - obj.input) / obj.dt;
+                obj.derivative = obj.derivative + obj.filt_val * ...
+                                    (der - obj.derivative);
+            end
+                    
+            obj.input = input;
+        end
+        
+        function [result] = get_pid(obj)
+            % Get Results from PID controller
+            
+            % P term
+            
+            % I term
+            if( (obj.ki == 0) || (obj.dt == 0) )
+%                 obj.integrator = 0;
+            else
+                obj.integrator = obj.integrator + ...
+                               obj.input * obj.ki * obj.dt; 
+                if(obj.integrator < -obj.imax)
+                    obj.integrator = -obj.imax;
+                elseif(obj.integrator > obj.imax)
+                    obj.integrator = obj.imax;
+                end
+            end
+            
+            % D term
+            obj.derivative = obj.kd * obj.derivative;
+            
+            result = obj.kp * obj.input + obj.integrator + obj.derivative;
+        end
+        
+%         function [obj,result] = get_p(obj)
+%             result = obj.kp * obj.input;
+%         end
+        
+%         function [obj,result] = get_i(obj)
+%             if( (obj.ki == 0) || (obj.dt == 0) )
+%                 obj.integrator = 0;
+%             else
+%                 obj.integrator = obj.integrator + ...
+%                                obj.input * obj.ki * obj.dt; 
+%                 if(obj.integrator < -obj.imax)
+%                     obj.integrator = -obj.imax;
+%                 elseif(obj.integrator > obj.imax)
+%                     obj.integrator = obj.imax;
+%                 end
+%             end
+%             
+%             result = obj.integrator;
+%         end
+        
+%         function [obj,result] = get_d(obj)
+%             obj.derivative = obj.kd * obj.derivative;
+%             result = obj.derivative;
+%         end
+        
+        function [obj] = reset_I(obj)
+            % Reset the integrator
+            obj.integrator = 0;
+        end
+        
+        function [result] = get_filt_alpha(obj)
+            if (obj.filt_hz == 0)
+                result = 1;
+            else
+                rc = 1 / (2*pi*obj.filt_hz);
+                result = obj.dt / (obj.dt + rc);
+            end
+        end
+        
+    end
+end
+

Controllers/Tests/testHoverThrottle.m

+function [U,X_des] = testHoverThrottle(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0;
+    Tao_theta_d =  0;
+    Tao_psi_d =    0;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testNegativePitchTorque.m

+function [U,X_des] = testNegativePitchTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0;
+    Tao_theta_d =  -0.1;
+    Tao_psi_d =    0;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testNegativeRollTorque.m

+function [U,X_des] = testNegativeRollTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    -0.1;
+    Tao_theta_d =  0;
+    Tao_psi_d =    0;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testNegativeYawTorque.m

+function [U,X_des] = testNegativeYawTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0;
+    Tao_theta_d =  0;
+    Tao_psi_d =    -0.1;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testPositivePitchTorque.m

+function [U,X_des] = testPositivePitchTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0;
+    Tao_theta_d =  0.1;
+    Tao_psi_d =    0;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testPositiveRollTorque.m

+function [U,X_des] = testPositiveRollTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0.1;
+    Tao_theta_d =  0;
+    Tao_psi_d =    0;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/Tests/testPositiveYawTorque.m

+function [U,X_des] = testPositiveYawTorque(X,params)
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (params.numRotors x 1) vector of control inputs normalized from 0->1
+    
+    %% Compute Desired Thrust and Torques
+    T_d =          params.hoverThrust;
+    Tao_phi_d =    0;
+    Tao_theta_d =  0;
+    Tao_psi_d =    0.1;
+    
+    %% Compute Desired Motor Inputs
+    U = params.M \ [T_d; Tao_phi_d; Tao_theta_d; Tao_psi_d];
+
+    U = min(U,1);
+    U = max(U,0);
+    
+    %% Grab Desired States
+    X_des = [zeros(12,1)];
+    
+end
+

Controllers/attitudePID.m

+function [U,X_des] = attitudePID(X,params)
+%% attitudePID
+
+    %% Input
+        % X
+        % params
+    %% Output
+        % U (4x1) vector of control inputs normalized from 0->1
+        % X_des (12x1) vector of desired states
+    
+    %% Desired Attitude
+    rollDesired = params.xd(7);
+    pitchDesired = params.xd(8);
+    yawDesired = params.xd(9);
+    
+    rollRateDesired = params.xd(10);
+    pitchRateDesired = params.xd(11);
+    yawRateDesired = params.xd(12);
+    
+    %% Actual Attitude
+    roll = X(7);
+    pitch = X(8);
+    yaw = X(9);
+    
+    rollRate = X(10);
+    pitchRate = X(11);
+    yawRate = X(12);
+    
+    %% Update Attitude PID loops   
+    params.rollPID  = params.rollPID.set_input_filter_all(rollDesired - roll);
+    params.pitchPID = params.pitchPID.set_input_filter_all(pitchDesired - pitch);
+    params.yawPID   = params.yawPID.set_input_filter_all(yawDesired - yaw);
+    
+    
+    rollRateDesired =  rollRateDesired  + params.rollPID.get_pid();