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%#codegen
  3    persistent first z1_dot phi01_dot phi02_dot phi03_dot delta_0 
  4
  5    if isempty(first)
  6        first = 0;
  7        
  8        %Initializing filters
  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    %Parameter estimates
 19    M_e = theta_hat(:,1:3);
 20    D_e = theta_hat(:,4:6);
 21    
 22    K_e = theta_hat(:,7:9);
 23
 24    %Initializing filter outputs
 25    z = zeros(3,1);
 26    phi1 = zeros(3,1);
 27    phi2 = zeros(3,1);
 28    phi3 = zeros(3,1);
 29    
 30    %Filter states
 31    z1 = phi0(1:2,:);
 32    phi01 = phi0(3:4,:);
 33    phi02 = phi0(5:6,:);
 34    phi03 = phi0(7:8,:);
 35    
 36    %Initializing derivative of theta estimate
 37    theta_hat_dot = zeros(3,6); 
 38    
 39    %Initializing normalizing term derivative
 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    %Adaptive gain
 64    Gamma = diag([800 1000]);
 65    
 66    gamma = 800;
 67    
 68    %Upper bounds for switching term
 69    M_0deu = [200 200 200]';
 70    M_0meu = [200 200 200]';
 71    
 72    M_0keu = [200 200 200]';
 73    
 74    %Lower bounds for switching term
 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        %Regressor
 87        phi = [phi1(i) phi2(i)]';
 88        
 89        %Parameter estimate
 90        theta_hat = [M_e(i,i) D_e(i,i)]';
 91        
 92        %Normalizing factor
 93        m = 1 + phi'*phi + m_s(i);
 94        
 95        mk = 1 + phi3(i)^2 + m_sk(i);
 96        
 97        %Estimate of error
 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        %Switching leakage for M_e
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        %Switching leakage for D_e
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        %Switching leakage for K_e
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