% 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