1 files changed, 37 insertions, 0 deletions
diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m
new file mode 100644
index 0000000..d46a1ef
--- /dev/null
+++ b/uav_ctrl_nominal.m
@@ -0,0 +1,37 @@
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% 
+% Design a nominal controller for UAV.
+
+function [ctrl] = uav_ctrl_nominal(params, model)
+
+% ------------------------------------------------------------------------
+% Design a nominal LQR controller
+
+% Define penalties according to following priorities
+
+q_pos = 10; % penalty on position
+q_vel = 1; % penalty on linear velocity
+q_ang = 100; % high penalty on angles
+q_rate = 1000; % very high penalty on angular velocities
+
+r_ang = 1; % flap movement is cheap
+r_thrust = 10; % thrust is more expensive on the battery
+
+nx = model.linear.Nx;
+nu = model.linear.Nu;
+
+% LQR design matrices
+Q = kron(diag([q_pos, q_vel, q_ang, q_rate]), eye(3));
+R = diag([r_ang, r_ang, r_ang, r_ang, r_thrust]);
+N = zeros(nx, nu); % no cross terms
+
+% Compute controller
+[K, S, poles] = lqr(model.linear.StateSpace, Q, R, N);
+
+% ------------------------------------------------------------------------
+% Save controller
+
+ctrl = struct();
+ctrl.K = K;
+
+end
+\ No newline at end of file