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