1 files changed, 39 insertions, 0 deletions
diff --git a/uav_ctrl_lqr.m b/uav_ctrl_lqr.m
new file mode 100644
index 0000000..8063c64
--- /dev/null
+++ b/uav_ctrl_lqr.m
@@ -0,0 +1,39 @@
+% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich
+% 
+% Design a nominal controller for UAV.
+
+function [ctrl_lqr] = uav_ctrl_lqr(params, model)
+
+% ------------------------------------------------------------------------
+% Design a Kalman filter for state estimation
+
+G = model.linear.StateSpace
+[L, P, E] = lqe(G.A, G.
+
+% ------------------------------------------------------------------------
+% Design a nominal LQR controller
+
+% Define penalties according to following priorities
+
+q_pos = 1; % penalty on position
+q_vel = 10; % penalty on linear velocity
+q_ang = 10; % high penalty on angles
+q_rate = 10; % very high penalty on angular velocities
+
+r_ang = 1; % flap movement is cheap
+r_thrust = 10; % thrust is more expensive on the battery
+
+% 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]);
+
+% Compute controller using output lqr
+[K, S, poles] = lqry(model.linear.StateSpace, Q, R);
+
+% ------------------------------------------------------------------------
+% Save controller
+
+ctrl_lqr = struct('Name', 'LQR', 'K', K);
+
+end
+% vim: ts=2 sw=2 et: