Move uncertain model design into uav_model and add more design parameter

1 files changed, 36 insertions, 18 deletions

diff --git a/uav_model.m b/uav_model.m
index 5650946..ce7cddb 100644
--- a/uav_model.m
+++ b/uav_model.m
@@ -2,7 +2,9 @@
 %
 % Compute model for UAV for given set of parameters.
 
-function [model] = uav_model(params)
+function [model] = uav_model(params, perf, uncert)
+
+model = struct();
 
 % ------------------------------------------------------------------------
 % Symbolic variables
@@ -121,6 +123,17 @@ f = [
   inv(J) * (tau - cross(Omega, J * Omega)); % rotational dynamics
 ];
 
+% Save function to compute the rotation matrix
+model.FrameRot = @(pitch, roll, yaw) ...
+  subs(R, [phi, theta, psi], [pitch, roll, yaw]);
+
+% Save equations of non-linear model (algebraic)
+model.nonlinear = struct(...
+  'State', xi, ...
+  'Inputs', u, ...
+  'Dynamics', f ...
+);
+
 % ------------------------------------------------------------------------
 % Linearization at equilibrium
 
@@ -187,6 +200,14 @@ sys = ss(A, B, C, D);
 % T = params.actuators.MeasurementDelay;
 % sys = ss(A, B, C, D, 'OutputDelay', T);
 
+% Save linearized dynamics (numerical)
+model.linear = struct(...
+  'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
+  'State', xi, 'Inputs', u, ... % state and input variables
+  'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
+  'StateSpace', sys ... % state space object
+);
+
 % ------------------------------------------------------------------------
 % Check properties of linearized model
 eigvals = eig(A);
@@ -242,27 +263,24 @@ if rank(Wo) < nx
 end
 
 % ------------------------------------------------------------------------
-% Save model
+% Add uncertainties using SIMULINK model
 
-model = struct();
+% Load simulink model with uncertainties and pass in parameters
+h = load_system('uav_model_uncertain');
+hws = get_param('uav_model_uncertain', 'modelworkspace');
 
-% Function to compute the rotation matrix
-model.FrameRot = @(pitch, roll, yaw) ...
-  subs(R, [phi, theta, psi], [pitch, roll, yaw]);
+hws.assignin('model', model);
+hws.assignin('perf', perf);
+hws.assignin('uncert', uncert);
 
-% Equations of non-linear model (algebraic)
-model.nonlinear = struct(...
-  'State', xi, ...
-  'Inputs', u, ...
-  'Dynamics', f ...
-);
+% Get uncertain model
+ulmod = linmod('uav_model_uncertain');
+usys = ss(ulmod.a, ulmod.b, ulmod.c, ulmod.d);
 
-% Linearized dynamics (numerical)
-model.linear = struct(...
-  'Nx', nx, 'Nu', nu, 'Ny', ny, ... % number of states, inputs, and outputs
-  'State', xi, 'Inputs', u, ... % state and input variables
-  'StateEq', xi_eq, 'InputEq', u_eq, ... % where the system was linearized
-  'StateSpace', sys ... % state space object
+% Save uncertain model
+model.uncertain = struct(...
+	'Simulink', ulmod, ...
+  'StateSpace', usys ...
 );
 
 end