-
Notifications
You must be signed in to change notification settings - Fork 0
/
lqr_controller.m
75 lines (67 loc) · 1.87 KB
/
lqr_controller.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
clear variables;
clc;
% Define values of system variables
% variable unit
M = 1000; % kg
m1 = 100; % kg
m2 = 100; % kg
l1 = 20; % m
l2 = 10; % m
g = 9.8; % m/s^2
% Define system matrices
a = [0 1 0 0 0 0;
0 0 -g*m1/M 0 -g*m2/M 0;
0 0 0 1 0 0;
0 0 -(M*g + m1*g)/(M*l1) 0 -m2*g/(M*l1) 0;
0 0 0 0 0 1;
0 0 -m1*g/(M*l2) 0 -(M*g + m2*g)/(M*l2) 0];
b = transpose([0 1/M 0 1/(l1*M) 0 1/(l2*M)]);
% Considering output vector to be: x(t), theta1(t), theta2(t)
c = [1 0 0 0 0 0;
0 0 1 0 0 0;
0 0 0 0 1 0];
% We only have one input parameter: Force, F, along positive x-direction
d = transpose([1 0 0]);
% State weights matrix
% These weights were found using iterating over various values
q = [5 0 0 0 0 0;
0 0 0 0 0 0;
0 0 5000 0 0 0;
0 0 0 0 0 0;
0 0 0 0 5000 0;
0 0 0 0 0 0];
r = 0.001;
% Get LQR parameters
[k, s, p] = lqr(a, b, q, r);
% Verify stability using lyapunov indirect method
% All poles should lie on the left-half plane for the system to be
% controllable
poles_a = eig(a);
a_e = a-b*k;
poles_a_e = eig(a_e);
% Define linear system model for LQR controller
system = ss(a-b*k, b, c, d);
tspan = 0:0.1:100;
initial = [5 0 deg2rad(20) 0 deg2rad(37) 0];
% Get system response for linear model
[t,q1] = ode45(@(t,q)linear(t,q,-k*q),tspan,initial);
figure(1);
hold on
plot(t,q1(:,1),'r')
plot(t,q1(:,3),'g')
plot(t,q1(:,5),'b')
ylabel('x(t), theta1(t), theta2(t)')
xlabel('time')
title('Linear system response')
legend('x(t)','theta1(t)','theta2(t)')
% Get system response for non-linear model
[t,q2] = ode45(@(t,q)non_linear(t,q,-k*q),tspan,initial);
figure(2);
hold on
plot(t,q2(:,1),'r')
plot(t,q2(:,3),'g')
plot(t,q2(:,5),'b')
ylabel('x(t), theta1(t), theta2(t)')
xlabel('time')
title('Non-Linear system response')
legend('x(t)','theta1(t)','theta2(t)')