1function [m_s_dot, m_sk_dot,phi_dot, theta_hat_dot, D_e, M_e, K_e, phi1, phi2] = env_est(m_s, m_sk, phi0, theta_hat, A01, B01, C01, A11, B11, C11, eta_p, nu_r, F_T, F_C)
2
3 persistent first z1_dot phi01_dot phi02_dot phi03_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 phi03_dot = zeros(2,3);
13
14 delta_0 = 3;
15
16 end
17
18
19 M_e = theta_hat(:,1:3);
20 D_e = theta_hat(:,4:6);
21
22 K_e = theta_hat(:,7:9);
23
24
25 z = zeros(3,1);
26 phi1 = zeros(3,1);
27 phi2 = zeros(3,1);
28 phi3 = zeros(3,1);
29
30
31 z1 = phi0(1:2,:);
32 phi01 = phi0(3:4,:);
33 phi02 = phi0(5:6,:);
34 phi03 = phi0(7:8,:);
35
36
37 theta_hat_dot = zeros(3,6);
38
39
40 m_s_dot = zeros(3,1);
41 m_sk_dot = zeros(3,1);
42
43 for i=1:3
44
45 z1_dot(:,i) = A01*z1(:,i)+B01*(F_C(i)-F_T(i));
46 z(i) = C01'*z1(:,i);
47
48 phi01_dot(:,i) = A11*phi01(:,i)+B11*nu_r(i);
49 phi1(i) = C11'*phi01(:,i);
50
51 phi02_dot(:,i) = A01*phi02(:,i)+B01*nu_r(i);
52 phi2(i) = C01'*phi02(:,i);
53
54 phi03_dot(:,i) = A01*phi03(:,i)+B01*eta_p(i);
55 phi3(i) = C01'*phi03(:,i);
56
57 m_s_dot(i) = -delta_0*m_s(i) + z(i)^2 + phi1(i)^2 + phi2(i)^2;
58
59 m_sk_dot(i) = -delta_0*m_sk(i) + z(i)^2 + phi3(i)^3;
60
61 end
62
63
64 Gamma = diag([800 1000]);
65
66 gamma = 800;
67
68
69 M_0deu = [200 200 200]';
70 M_0meu = [200 200 200]';
71
72 M_0keu = [200 200 200]';
73
74
75 M_0del = [-200 -200 -200]';
76 M_0mel = [10 20 20]';
77
78 M_0kel = [5 5 5];
79
80 w_0de = 1;
81 w_0me = 1;
82 w_0ke = 1;
83
84 for i=1:3
85
86
87 phi = [phi1(i) phi2(i)]';
88
89
90 theta_hat = [M_e(i,i) D_e(i,i)]';
91
92
93 m = 1 + phi'*phi + m_s(i);
94
95 mk = 1 + phi3(i)^2 + m_sk(i);
96
97
98 z_hat = theta_hat'*phi;
99 eps = (z(i) - z_hat)/m;
100
101 zk_hat = K_e(i,i)*phi3;
102 epsk = (z(i)-zk_hat)/mk;
103
104
105 if and(M_0mel(i) < M_e(i,i), M_e(i,i) < M_0meu(i))
106 w_sme = 0;
107 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)))
108 w_sme = (abs(M_e(i,i))/M_0meu(i) - 1)*w_0me;
109 else
110 w_sme = w_0me;
111 end
112
113
114 if and(M_0del(i) < D_e(i,i), D_e(i,i) < M_0deu(i))
115 w_sde = 0;
116 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)))
117 w_sde = (abs(D_e(i,i))/M_0deu(i) - 1)*w_0de;
118 else
119 w_sde = w_0de;
120 end
121
122
123 if and(M_0kel(i) < K_e(i,i), K_e(i,i) < M_0keu(i))
124 w_ske = 0;
125 elseif or(and(M_0keu(i) <= K_e(i,i), K_e(i,i) <= 2*M_0keu(i)), and(M_0kel(i)/2 <= K_e(i,i), K_e(i,i) <= M_0kel(i)))
126 w_ske = (abs(K_e(i,i))/M_0keu(i) - 1)*w_0ke;
127 else
128 w_ske = w_0ke;
129 end
130
131 W_s = diag([w_sme w_sde]);
132
133 theta_hat_dot(i,i:3:6) = Gamma*eps*phi - Gamma*W_s*theta_hat;
134
135 theta_hat_dot(i,i+6) = gamma*epsk*phi3(i) - gamma*w_ske*K_e(i,i);
136
137 end
138
139 phi_dot = [z1_dot; phi01_dot; phi02_dot; phi03_dot];
140
141