1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
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
|
