-
Notifications
You must be signed in to change notification settings - Fork 1
/
load_experiment_constants.m
113 lines (86 loc) · 4.02 KB
/
load_experiment_constants.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
105
106
107
108
109
110
111
112
113
% 22 September 2015
% load constants for simulation
constants.avoid_switch = 'true';
constants.dist_switch = 'true';
constants.adaptive_switch = 'true';
% define constants/properties of rigid body
constants.m_sc = 1;
m_sc = constants.m_sc;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% INERTIA TENSOR
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% constants.J = [1.059e-2 -5.156e-6 2.361e-5;...
% -5.156e-6 1.059e-2 -1.026e-5;
% 2.361e-5 -1.026e-5 1.005e-2];
% Chris's Hexrotor inertia matrix
constants.J = [55710.50413e-7 , 617.6577e-7 , -250.2846e-7 ;...
617.6577e-7 , 55757.4605e-7 , 100.6760e-7 ;...
-250.2846e-7 , 100.6760e-7 , 105053.7595e-7];
% % % from Farhad ASME paper
% constants.J = [ 5.5711 0.0618 -0.0251; ...
% 0.06177 5.5757 0.0101;...
% -0.02502 0.01007 1.05053] * 1e-2;
% constants.J = diag([694 572 360]);
J = constants.J;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CONTROLLER
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% controller parameters
constants.G = diag([0.9 1 1.1]);
% con = -1+2.*rand(3,constants.num_con); % inertial frame vectors (3XN)
% from [1] U. Lee and M. Mesbahi. Spacecraft Reorientation in Presence of Attitude Constraints via Logarithmic Barrier Potentials. In 2011 AMERICAN CONTROL CONFERENCE, Proceedings of the American Control Conference, pages 450?455, 345 E 47TH ST, NEW YORK, NY 10017 USA, 2011. Boeing; Bosch; Corning; Eaton; GE Global Res; Honeywell; Lockheed Martin; MathWorks; Natl Instruments; NT-MDT; United Technol, IEEE. American Control Conference (ACC), San Fransisco, CA, JUN 29-JUL 01, 2011.
% con = [0.174 0 -0.853 -0.122;...
% -0.934 0.7071 0.436 -0.140;...
% -0.034 0.7071 -0.286 -0.983];
% column vectors to define constraints
% zeta = 0.7;
% wn = 0.2;
% constants.kp = wn^2;
% constants.zeta = 2*zeta*wn;
% constants.kp = 0.0424; % wn^2
% constants.kp = 0.4;
% constants.kv = 0.296; % 2*zeta*wn
constants.kp = 0.4;
constants.kv = 0.7; % 2*zeta*wn
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% CONSTRAINT
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
constants.sen = [1;0;0]; % body fixed frame
% define a number of constraints to avoids
% con = [0.174 0.4 -0.853 -0.122;...
% -0.934 0.7071 0.436 -0.140;...
% -0.034 0.7071 -0.286 -0.983];
% constants.con_angle = [40;40;40;20]*pi/180;
con = [1/sqrt(2);1/sqrt(2);0];
constants.con_angle = 12*pi/180;
constants.con = con./repmat(sqrt(sum(con.^2,1)),3,1); % normalize
constants.alpha = 8; % use the same alpha for each one
constants.num_con = size(constants.con,2);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ADAPTIVE CONTROL FOR DISTURBANCE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% disturbance terms
constants.W = eye(3,3);
constants.delta = [0.06;0.06;0.09];
constants.kd = 0.05; % adaptive controller gain term (rate of convergence)
constants.c = 0.1; % input the bound on C here
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% DESIRED/INITIAL CONDITION
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% define the initial state of rigid body
% R0 = ROT1(0*pi/180)*ROT2(45*pi/180)*ROT3(180*pi/180);
constants.q0 = [-0.188 -0.735 -0.450 -0.471];
% constants.qd = [-0.59 0.67 0.21 -0.38]; % from lee/meshbahi paper
constants.qd = [0 0 0 1];
% constants.R0 = quat2dcm(constants.q0)';
% constants.Rd = quat2dcm(constants.qd)';
% constants.R0 = ROT1(0*pi/180)*ROT3(90*pi/180); % avoid single constraint
% constants.Rd = ROT3(0*pi/180);
constants.R0 = ROT1(0*pi/180)*ROT3(90*pi/180); % avoid multiple constraints
constants.Rd = eye(3,3);
R0 = constants.R0;
w0 = zeros(3,1);
delta_est0 = zeros(3,1);
initial_state = [constants.R0(:);w0; delta_est0];
% simulation timespan
tspan = linspace(0,10,1000);