1function [m_s_dot, phi_dot, theta_hat_dot, theta_e_hat] = env_est(m_s, phi0, theta_hat, A01, B01, C01, A11, B11, C11, nu_r, F_T, F_C)
2
3 persistent first z1_dot phi01_dot phi02_dot delta_0
4
5 if isempty(first)
6 first = 0;
7
8
9 z1_dot = zeros(2,3);
10 phi01_dot = zeros(2,3);
11 phi02_dot = zeros(2,3);
12
13 delta_0 = 50;
14
15 end
16
17
18 M_e = theta_hat(:,1:3);
19 D_e = theta_hat(:,4:6);
20
21
22 z = zeros(3,1);
23 phi1 = zeros(3,1);
24 phi2 = zeros(3,1);
25
26
27 z1 = phi0(1:2,:);
28 phi01 = phi0(3:4,:);
29 phi02 = phi0(5:6,:);
30
31 phi = zeros(3,1);
32
33
34 theta_hat_dot = zeros(3,6);
35
36
37 m_s_dot = zeros(3,1);
38
39 for i=1:3
40
41 z1_dot(:,i) = A01*z1(:,i)+B01*(F_C(i));
42 z(i) = C01'*z1(:,i);
43
44 phi01_dot(:,i) = A11*phi01(:,i)+B11*nu_r(i);
45 phi1(i) = C11'*phi01(:,i);
46
47 phi02_dot(:,i) = A01*phi02(:,i)+B01*nu_r(i);
48 phi2(i) = C01'*phi02(:,i);
49
50 phi_n = [phi1(i) phi2(i)]';
51
52
53 m_s_dot(i) = -delta_0*m_s(i) + phi_n'*phi_n;
54
55 end
56
57
58 Gamma = diag([500 100]);
59
60
61 M_0deu = [5000 5000 8000]';
62 M_0meu = [3000 3000 5000]';
63
64
65 M_0del = [0 0 0]';
66 M_0mel = [10 10 10]';
67
68 w_0de = 1;
69 w_0me = 1;
70
71 for i=1:3
72
73
74 phi = [-phi1(i) -phi2(i)]';
75
76
77 theta_e_hat = [M_e(i,i) D_e(i,i)]';
78
79
80 m = 1 + phi'*phi + m_s(i);
81
82
83 z_hat = theta_e_hat'*phi;
84 eps = (z(i) - z_hat)/m;
85
86
87 if and(M_0mel(i) < M_e(i,i), M_e(i,i) < M_0meu(i))
88 w_sme = 0;
89 elseif or(and(M_0meu(i) <= M_e(i,i), M_e(i,i) <= 2*M_0meu(i)), and(M_0mel(i)/2 <= M_e(i,i), M_e(i,i) <= M_0mel(i)))
90 w_sme = (abs(M_e(i,i))/M_0meu(i) - 1)*w_0me;
91 else
92 w_sme = w_0me;
93 end
94
95
96 if and(M_0del(i) < D_e(i,i), D_e(i,i) < M_0deu(i))
97 w_sde = 0;
98 elseif or(and(M_0deu(i) <= D_e(i,i), D_e(i,i) <= 2*M_0deu(i)), and(M_0del(i)/2 <= D_e(i,i), D_e(i,i) <= M_0del(i)))
99 w_sde = (abs(D_e(i,i))/M_0deu(i) - 1)*w_0de;
100 else
101 w_sde = w_0de;
102 end
103
104 W_se = diag([w_sme w_sde]);
105
106
107 theta_hat_dot(i,i:3:end) = Gamma*eps*phi - Gamma*W_se*theta_e_hat;
108
109 end
110
111 phi_dot = [z1_dot; phi01_dot; phi02_dot];
112 theta_e_hat = [M_e; D_e];
113
114