% Quadrotor State Space Model with Non-linearities
close all
%%%%%%%%%%%%%%%%%%
% Propeller motors
%%%%%%%%%%%%%%%%%%
Kt = 10^-2; % motor torque constant [Nm/A]
Kb = 10^-2; % motor back emf constant [Vs]
R = 1; % motor internal resistance [V/A]
%%%%%%%%%%%
% Propeller
%%%%%%%%%%%
p = 1.225; % density of air [kg/m^3]
Dp = 0.1; % prop diameter [m]
Ct = 0.1; % prop thrust coeficient [N s^2/(kg m)]
Cp = 0.7; % prop drag coefficient [N s^2/(kg m)]
b = Ct*p*Dp^4/(4*pi^2); % prop thrust factor
d = Cp*p*Dp^5/(8*pi^3); % prop drag factor
% gearbox = none so...
r = 1; % reduction ratio
n = 1; % gearbox efficiency
%%%%%%%%%%%%%%%%%%%%%%%%%%
% Moments of inertia
%%%%%%%%%%%%%%%%%%%%%%%%%%
m = 0.1; % motor-propeller and mount mass [kg]
L = 0.1; % body center to motor shaft massless arm length [m]
% Body moment of inertial about X and Y axes
Ix = 2*m*L^2;
Iy = 2*Ix;
% Z-axis is 4 motor assemblies on massless rods
Iz = 4*m*L^2; % [kg m^2]
% Motor and propeller referred to motor shaft
mp = 0.002; % propeller mass [kg]
Jp = 1/12 * mp * Dp^2; % prop moment of inertia [kg m^2]
mm = 0.03; % motor mass [kg]
mr = mm/2; % rotor mass [kg]
rr = 0.01; % rotor radius [m]
Jm = 1/2 * mr * rr^2; % motor moment of inertia [kg m^2]
Jt = Jm + Jp; % total rotational inertia of motor-prop assembly [kg m^2]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% State equations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% skip the body inertial effects for now...
% Maximum rate of body rotation about any axis for controllable flight
rmax = pi; % [1/s]
% initialize rate vectors for which to determine gains
ratesteps = 10;
%dps = [0:rmax/ratesteps:rmax]; % roll rate
dps = [1]; % yaw rate
%dth = [0:rmax/ratesteps:rmax]; % pitch rates
%dph = [0:rmax/ratesteps:rmax]; % yaw rates
dth = [1]; % pitch rates
dph = [1]; % roll rates
N = length(dps);
izxoy = (Iz - Ix)/(2*Iy);
iyzox = (Iy - Iz)/(2*Ix);
ixyoz = (Ix - Iy)/(2*Iz);
C = [1 0 1 0 1 0];
q=1;
Q = [q 0 0 0 0 0;
0 0 0 0 0 0;
0 0 q 0 0 0;
0 0 0 0 0 0;
0 0 0 0 q/10 0;
0 0 0 0 0 0];
R = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
N = 0;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% initialize the sim
tout=0;
iph = pi/4; % roll
ith = pi/4; % pitch
ips = pi/4; % yaw
initcons = [iph 0 ith 0 ips 0];
ic = initcons; % initial conditions: phi, theta, psi (roll, pitch, yaw)
tend = 1.5; % simulation end time
xout = ic;
xfb = ic;
tstep = .02;
tout = 0;
refin = [0 0 0 0 0 0];
refout= refin;
yout = C.*ic;
eout = refout-yout; % error output init
tm = 0;
vidfile = VideoWriter('quad_regulator_sim.mp4','MPEG-4');
open(vidfile);
fig = figure('Name','Quadrotor Attitude Regulator');
[plt, rplt, tplt, yplt] = paintbody([0 0 0], yout(1), yout(3), yout(5),tout,xout,tend);
MM = getframe(fig);
writeVideo(vidfile,MM);
pause(4);
while tm < tend,
% linearized plant has angular rates in the state transition matrix
% the plant model thus changes over the flight envelope
% update the model and recompute the gain.
% an implementation would most likely involve a gain lookup table.
% for an estimated phidot, thetadot, psidot as an index into a 3-dim
% lookup table. However, for this simulation we just update the
% model and recompute the gain directly.
A = [0 1 0 0 0 0;
0 0 0 xfb(6)*iyzox 0 xfb(4)*iyzox;
0 0 0 1 0 0;
0 xfb(6)*izxoy 0 0 0 xfb(2)*izxoy;
0 0 0 0 0 1;
0 xfb(4)*ixyoz 0 xfb(2)*ixyoz 0 0];
B = [0 0 0 0;
L/Ix 0 0 Jt/Ix*xfb(4);
0 0 0 0;
0 L/Iy 0 Jt/Iy*xfb(2);
0 0 0 0;
0 0 1/Iz 0];
% recompute gain K
[K, S, CLP] = lqr(A,B,Q,R, N);
% full state feedback
% u is cross-body torque
u = -K*(xfb)';
% u is the cross-body and yaw torque command input.
% To simulate bias and disturbance in this command torque
% being applied to the platform run it into a motor speed model
% where desired speeds can be estimated, perturbed, and supplied
% out as torque inputs to the plant: the copter body.
% Remove comments on next two lines to map control output u
% through to a prop speed model and back out as cross-body torque
% Tout = BodyTorqueToPropSpeed(u);
% u=Tout;
% Initial conditions for next iteration are the states of the last
% output of the solver for this iteration
icon = xfb;
% invoke the ODE solver to compute state variables for each interval of
% tspan. "rpcont" if where the state-space model lives so examine that.
tspan = 0:tstep/10:tstep;
[t,x] = ode45(@(t,x) ssodefunc(t,x,A,B,C,u), tspan, icon);
% post-solver iteration data collection
% the solver output time vector is zero-based. Offset by iteration start
% time
tt = tm+t;
tout = [tout; tt]; % append this iteration time vector to total sim time
xout = [xout; x]; % append iteration states to sim state data
sz = size(x); % size of the state vector
xfb=x(sz(1),:);
% Apply the state-space 'C' matrix to the state matrix to obtain angular
% output for this iteration
yo = C.*x;
% the "sampled" output for the next iteration is that last position
% solved in this iteration.
yout = yo(sz(1),:);
% set the start time tm for the next iteraton.
tm = tm + tstep;
paintbody_update(plt, rplt, tplt, yplt,[0 0 0], yout(1), yout(3), yout(5),tout,xout);
MM = getframe(fig);
writeVideo(vidfile,MM);
end
close(vidfile);