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%#codegen
  3    persistent first z1_dot phi01_dot phi02_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        
 13        delta_0 = 50;
 14        
 15    end
 16    
 17    %Parameter estimates
 18    M_e = theta_hat(:,1:3);
 19    D_e = theta_hat(:,4:6);
 20
 21    %Initializing filter outputs
 22    z = zeros(3,1);
 23    phi1 = zeros(3,1);
 24    phi2 = zeros(3,1);
 25    
 26    %Filter states
 27    z1 = phi0(1:2,:);
 28    phi01 = phi0(3:4,:);
 29    phi02 = phi0(5:6,:);
 30    
 31    phi = zeros(3,1);
 32    
 33    %Initializing derivative of theta estimate
 34    theta_hat_dot = zeros(3,6); 
 35    
 36    %Initializing normalizing term derivative
 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        %m_s_dot(i) = -delta_0*m_s(i) + z(i)^2 + phi1(i)^2 + phi2(i)^2;
 53        m_s_dot(i) = -delta_0*m_s(i) + phi_n'*phi_n;
 54
 55    end
 56    
 57    %Adaptive gain
 58    Gamma = diag([500 100]);
 59    
 60    %Upper bounds for leakage term
 61    M_0deu = [5000 5000 8000]';
 62    M_0meu = [3000 3000 5000]';
 63    
 64    %Lower bounds for leakage term
 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        %Regressor
 74        phi = [-phi1(i) -phi2(i)]';
 75        
 76        %Parameter estimate
 77        theta_e_hat = [M_e(i,i) D_e(i,i)]';
 78
 79        %Normalizing factor
 80        m = 1 + phi'*phi + m_s(i);
 81        
 82        %Estimate of error
 83        z_hat = theta_e_hat'*phi;
 84        eps = (z(i) - z_hat)/m;
 85        
 86        %Switching leakage for M_e
 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        %Switching leakage for D_e
 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