function [psi_rq,psi_rd,theta_psi_r,h_out] = aci_fe(i_sq,i_sd,u_sq,u_sd,h_in,p)
% This function estimates the rotor flux of Induction machine and its angle
%
% Inputs:  
%       i_sq = Stationary q-axis stator current (amp)
%       i_sd = Stationary d-axis stator current (amp)
%       u_sq = Stationary q-axis stator voltage (volt)
%       u_sd = Stationary d-axis stator voltage (volt)
%       h_in = [theta_psi_r_prev; psi_rD_i_prev; psi_sd_v_prev; psi_sq_v_prev;
%               ui_sd_prev; ui_sq_prev; e_sd_prev; e_sq_prev]
%       p = [T; Rs; Rr; Ls; Lr; Lm; Kp; Ti; Ib; Vb]
% Outputs:
%       psi_rq = Stationary q-axis rotor flux linkage (volt.sec/rad)
%       psi_rd = Stationary d-axis rotor flux linkage (volt.sec/rad)
%       theta_psi_r = Synchronously rotating rotor flux angle between 0-2*pi (rad)
%       h_out = [theta_psi_r_curr; psi_rD_i_curr; psi_sd_v_curr; psi_sq_v_curr;
%                ui_sd_curr; ui_sq_curr; e_sd_curr; e_sq_curr]
% where 
%		  T = sampling period (sec)
%       Rs = Stator resistance (ohm)
%       Rr = Rotor resistance referred to stator (ohm)
%       Ls = Stator self inductance (H)
%       Lr = Rotor self inductance referred to stator (H)
%       Lm = Magnetizing inductance (H)
%       Kp = Proportional gain
%       Ti = Integral time or reset time (sec)
%       theta_psi_r_prev,theta_psi_r_curr = Previous and current rotor flux angle (rad)
%       psi_rD_i_prev,psi_rD_i_curr = Previous and current rotor flux (volt.sec)
%       psi_sd_v_prev,psi_sd_v_curr = Previous and current stator flux (volt.sec)
%       psi_sq_v_prev,psi_sq_v_curr = Previous and current stator flux (volt.sec)
%       ui_sd_prev,ui_sd_curr = Previous and current integral term voltage (volt)
%       ui_sq_prev,ui_sq_curr = Previous and current integral term voltage (volt)
%       e_sd_prev,e_sd_curr = Previous and current back emf (volt)
%       e_sq_prev,e_sq_curr = Previous and current back emf (volt)

% Defining constants based on machine parameters
Tr = p(5)/p(3);     % Tr = Lr/Rr, Rotor time constant; depending on Rr
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        
% Constants 
K1_fe = Tr/(Tr+p(1)); 
K2_fe = p(1)/(Tr+p(1)); 
K3_fe = p(6)/p(5);
K4_fe = (p(4)*p(5)-p(6)*p(6))/(p(5)*p(6));   
K5_fe = p(9)*p(2)/p(10); 
K6_fe = p(10)*p(1)/(p(6)*p(9)); 
K7_fe = p(5)/p(6);   
K8_fe = (p(4)*p(5)-p(6)*p(6))/(p(6)*p(6));
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               
% Name the variables
i_qs_fe = i_sq;
i_ds_fe = i_sd;
u_qs_fe = u_sq;
u_ds_fe = u_sd;
theta_r_fe = h_in(1);
flx_dr_e = h_in(2);
psi_ds_fe = h_in(3);
psi_qs_fe = h_in(4);
ui_ds = h_in(5);
ui_qs = h_in(6);
emf_ds = h_in(7);
emf_qs = h_in(8);

Kp_fe = p(7);
Ki_fe = p(8)/p(1);

% Park transformation on the measured stator current
    theta_e = 2*pi*theta_r_fe;   
    i_ds_e = i_qs_fe*sin(theta_e)+i_ds_fe*cos(theta_e);

% The current model section (Classical Rotor Flux Vector Control Equation) 
	flx_dr_e = K1_fe*flx_dr_e + K2_fe*i_ds_e;	

% Inverse park transformation on the rotor flux from the current model
   flx_dr_s = flx_dr_e*cos(theta_e);
   flx_qr_s = flx_dr_e*sin(theta_e);  

% Compute the stator flux based on the rotor flux from current model
   flx_ds_s = K3_fe*flx_dr_s + K4_fe*i_ds_fe;	
   flx_qs_s = K3_fe*flx_qr_s + K4_fe*i_qs_fe;

% Conventional PI controller section 
   error =  psi_ds_fe - flx_ds_s;
   ucomp_ds = Kp_fe*error + ui_ds;
   ui_ds = Kp_fe*Ki_fe*error + ui_ds;    
   
   error =  psi_qs_fe - flx_qs_s;
   ucomp_qs = Kp_fe*error + ui_qs;
   ui_qs = Kp_fe*Ki_fe*error + ui_qs;    

% Compute the estimated stator flux based on the integral of back emf		
   emf_old = emf_ds;
   emf_ds = u_ds_fe - ucomp_ds - K5_fe*i_ds_fe;
   psi_ds_fe = psi_ds_fe + K6_fe*(0.5)*(emf_ds + emf_old); 

   emf_old = emf_qs;
   emf_qs = u_qs_fe - ucomp_qs - K5_fe*i_qs_fe;
   psi_qs_fe = psi_qs_fe + K6_fe*(0.5)*(emf_qs + emf_old); 

% Compute the estimated rotor flux based on the stator flux from the integral of back emf
   psi_dr_fe = K7_fe*psi_ds_fe - K8_fe*i_ds_fe;  
   psi_qr_fe = K7_fe*psi_qs_fe - K8_fe*i_qs_fe;  

% Compute the rotor flux angle
   theta_r_fe = rem(2*pi+atan2(psi_qr_fe,psi_dr_fe),2*pi)/(2*pi); 
 
% Rename variables    
psi_rd = psi_dr_fe;
psi_rq = psi_qr_fe;
theta_psi_r = theta_r_fe;    
   
% History update
h_out = [theta_r_fe; flx_dr_e; psi_ds_fe; psi_qs_fe; ui_ds; ui_qs; emf_ds; emf_qs];