-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathwing_rock_sim.m
143 lines (117 loc) · 3.87 KB
/
wing_rock_sim.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
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
clc;
Kp=1.5;
Kd=1.3;
ti=0;
tf=60;
dt=0.005;
Am=[[0 1];[-Kp -Kd]];
% First section of the code simply demonstates PD controller response
% to step inputs. This is the ref model that we wish to follow
phi_p_prev=[0;0]; % Initialised vector
phi_p_ref_store=[]; % matrix for storing phi and phidot (p) values as vector columns
phi_des_store=[];
for i=ti:dt:tf
if i>10 && i<20 % The desired signal we wish to track
phi_des=1;
phid_des=0;
phidd_des=0;
elseif i>30 && i<40
phi_des=-1;
phid_des=0;
phidd_des=0;
else
phi_des=0;
phid_des=0;
phidd_des=0;
end
phi_des_store=[phi_des_store,phi_des];
phi_p_ref_next=phi_p_prev+dt*(Am*phi_p_prev+[0;1]*...
(phidd_des+Kd*phid_des+Kp*phi_des));
phi_p_ref_store=[phi_p_ref_store,phi_p_prev];
phi_p_prev=phi_p_ref_next;
end
% for plotting reference signal, PD ref model response
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
time_vec=ti:dt:tf;
figure(1)
plot(time_vec,phi_des_store,'r--');
hold on;
grid on;
plot(time_vec,phi_p_ref_store(1,:));
xlabel('Time')
ylabel('ref signal and PD response')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% Actual model with adaptive control (using above control law as linear model)
phi_p_prev_2=[0;0]; % Initialised vector
phi_p_adaptive_store=[];
phi_des_store_2=[];
BW=2;
centers=10;
gain=200;
arbit_centres=repmat(linspace(-0.05,0.05,centers),[2,1]);
% repmat(linspace(3,5,10),[2,1]);
% center location change causes issue. So if domain of operation uncertain,
% issue arises. Shortcoming of classical MRAC can be seen by replacing
% value of arbit_centres with the above commented repmat command
% Also making vad=0, will show how badly PD response is when delta is
% present
basis=zeros(centers,1);
counter=0;
outputweight=zeros(centers,1);
B=[0;1];
Q=eye(2);
P=lyap(Am',Q); % Am'P+P*Am+Q = 0
delta_store=[]; % for plotting delta uncertainity
adaptive_store=[]; % for plotting adaptive element
for i=ti:dt:tf
counter=counter+1;
if i>10 && i<20
phi_des_2=1;
phid_des_2=0;
phidd_des_2=0;
elseif i>30 && i<40
phi_des_2=-1;
phid_des_2=0;
phidd_des_2=0;
else
phi_des_2=0;
phid_des_2=0;
phidd_des_2=0;
end
phi_des_store_2=[phi_des_store_2,phi_des_2];
%%%%%%%%%%%% adaptive element %%%%%%%%%%%%%
for m=1:centers
basis(m)=exp((-norm(arbit_centres(:,m)-phi_p_prev_2)^2)/(2*BW));
end
error=phi_p_ref_store(:,counter)-phi_p_prev_2;
outputweight=outputweight+dt*(-gain)*basis*error'*P*B;
vad=outputweight'*basis;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Uncertainity
delta=0.8+0.2314*phi_p_prev_2(1)+0.6918*phi_p_prev_2(2)-0.6245*abs(phi_p_prev_2(1))*phi_p_prev_2(2)...
+0.0095*abs(phi_p_prev_2(2))*phi_p_prev_2(2)+0.0214*(phi_p_prev_2(1))^3;
% Adaptive element countering uncertainity
phi_p_next_2=phi_p_prev_2+dt*([[0 1];[-Kp -Kd]]*phi_p_prev_2+[0;1]*...
(phidd_des_2+Kd*phid_des_2+Kp*phi_des_2)+[0;delta]+[0;-vad]);
%%%%%%%%%% For plotting later on %%%%%%%%%%
phi_p_adaptive_store=[phi_p_adaptive_store,phi_p_prev_2];
delta_store=[delta_store,delta];
adaptive_store=[adaptive_store,vad];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
phi_p_prev_2=phi_p_next_2;
end
figure(2)
time_vec=ti:dt:tf;
plot(time_vec,phi_des_store_2,'r--','LineWidth',2);
hold on
grid on;
plot(time_vec,phi_p_adaptive_store(1,:));
xlabel('time');
legend('Phi desired','Phi after countering delta with adaptive element')
figure(3)
plot(time_vec,delta_store,'r--','LineWidth',2);
hold on
grid on;
plot(time_vec,adaptive_store);
xlabel('time');
legend('delta','adaptive control element response')