% 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: