-
Notifications
You must be signed in to change notification settings - Fork 0
/
Controller_Calc_W.m
104 lines (85 loc) · 2.06 KB
/
Controller_Calc_W.m
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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
close all
clear
clc
%% parameters
mq = 0.3812;
Ixx = 2.661e-5;
Iyy = 2.661e-5;
Izz = 2.661e-5;
k = 2e-6;
L = 0.035;
b = 7e-9;
kd = 0;
rho = 0.037;
r = 0.03;
z_offset = 0.02;
g = 9.8;
ss = diag([1, 1, 0.5]) * 2;
xd = 2;
yd = 2;
zd = 1;
%% Matrices
%states = [wx wy wz phi theta psi x y z vx vy vz]
A1 = [0 g 0
-g 0 0
0 0 0];
A = [zeros(3, 12);
eye(3), zeros(3, 9);
zeros(3, 9), eye(3);
zeros(3), A1, zeros(3, 6)];
B1 = [L, 0, -L, 0;
0, L, 0, -L;
b / k, -b / k, b / k, -b / k];
J = diag([Ixx, Iyy, Izz]);
B = [J^ - 1 * B1;
zeros(8, 4);
ones(1, 4) / mq] * 2 * k * eye(4) * ((mq * g)^0.5/2);
Q = diag([1, 1, 100, 1, 1, 100, 1, 1, 100, 1, 1, 10000]);
R = eye(4);
[K, S, E] = lqr(A, B, Q, R);
%open('quad_simulation_with_linearization_around_propeller_w.slx');
sim('quad_simulation_with_linearization_around_propeller_w.slx');
figure
subplot(2, 1, 1);
plot(Positions.Time, Positions.Data, 'LineWidth', 2);
hold on
plot(Angles.Time, Angles.Data, 'LineWidth', 2);
grid on
grid minor
xlabel('time (sec)');
ylabel('States');
legend('$x$', '$y$', '$z$', '$\phi$', '$\theta$', '$\psi$', 'Interpreter', 'latex')
subplot(2, 1, 2);
plot(Inputs.Time, Inputs.Data, 'LineWidth', 2);
grid on
grid minor
xlabel('time (sec)');
ylabel('Inputs');
legend('$u_1$', '$u_2$', '$u_3$', '$u_4$', 'Interpreter', 'latex')
%% simulation
Ts = 1e-4;
Tf = 30;
t = 0:Ts:Tf;
N = numel(t);
x = zeros(12, N);
u = zeros(4, N);
x(:, 1) = [0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0].';
for i = 1:N - 1
u(:, i) = -K * (x(:, i) - [0, 0, 0, 0, 0, 0, xd, yd, zd, 0, 0, 0].');
x(:, i + 1) = x(:, i) + Ts * (A * x(:, i) + B * u(:, i));
end
figure
subplot(2, 1, 1);
plot(t(1:round(N)), x([7, 8, 9, 1, 2, 3], 1:round(N)), 'LineWidth', 2);
grid on
grid minor
xlabel('time (sec)');
ylabel('States');
legend('$x$', '$y$', '$z$', '$\phi$', '$\theta$', '$\psi$', 'Interpreter', 'latex')
subplot(2, 1, 2);
plot(t(1:round(N)), u(:, 1:round(N)) + (mq * g / 4 / k)^0.5, 'LineWidth', 2);
grid on
grid minor
xlabel('time (sec)');
ylabel('Inputs');
legend('$u_1$', '$u_2$', '$u_3$', '$u_4$', 'Interpreter', 'latex')