function [xy_debias,P_debias] = debias_lerro(rp_meas,R,rp_true)
% DEBIAS_LERRO - Converting polar measurments to Cartesian
% measurements by applying the non-linear transformation and
% debiasing it according to Lerro & Bar-Shalom. 
%
%   [xy_debias,P_debias] = debias_lerro(rp_meas,R,rp_true)
%
%   optional input 'rp_true': 
%   if supplied 'truth' debiasing based on the true position
%   otherwise   'measured'  debiasing based on the measured position
%
% Dirk Tenne 03/28/2002
% CoDE
% http://code.eng.buffalo.edu/tracking
%
  if nargin > 2
    method = 'truth';
  else 
    method = 'measured';
  end

 xy = rp_meas;
 xy_debias = xy;
 P_debias = R;
 
 xy(1) = rp_meas(1)*cos(rp_meas(2));
 xy(2) = rp_meas(1)*sin(rp_meas(2));
  
 phi_dev = sqrt(R(2,2));
 r_dev = sqrt(R(1,1));
  switch method
    case 'truth'
     r_t = rp_true(1);
     phi_t = rp_true(2);
     mu_t = [r_t*cos(phi_t)*(exp(-phi_dev^2/2)-1); ... 
	     r_t*sin(phi_t)*(exp(-phi_dev^2/2)-1)];
     R_t(1,1) = r_t^2*exp(-phi_dev^2)*((cos(phi_t))^2* ...
              (cosh(phi_dev^2)-1)+(sin(phi_t))^2*sinh(phi_dev^2))+ ...
	 r_dev^2*exp(-phi_dev^2)*((cos(phi_t))^2* ...
	 cosh(phi_dev^2)+(sin(phi_t))^2*sinh(phi_dev^2));
     
     R_t(2,2) = r_t^2*exp(-phi_dev^2)*((sin(phi_t))^2* ...
             (cosh(phi_dev^2)-1)+(cos(phi_t))^2*sinh(phi_dev^2)) + ...
             r_dev^2*exp(-phi_dev^2)*((sin(phi_t))^2* ...
             cosh(phi_dev^2)+(cos(phi_t))^2*sinh(phi_dev^2));
     R_t(1,2) = sin(phi_t)*cos(phi_t)*exp(-2*phi_dev^2)*(r_dev^2+ ...
             r_t^2*(1-exp(phi_dev^2)));
     R_t(2,1) = R_t(1,2);
 
     debias = mu_t;
     P_debias = R_t;
   case 'measured'
     r_m = rp_meas(1);
     phi_m = rp_meas(2);
     mu_a = [r_m*cos(phi_m)*(exp(-phi_dev^2)-exp(-phi_dev^2/2)); ... 
	    r_m*sin(phi_m)*(exp(-phi_dev^2)-exp(-phi_dev^2/2))];
     sphi = phi_dev^2;
     sr = r_dev^2;
     R_a(1,1) = r_m^2*exp(-2*sphi)*(cos(phi_m)^2*(cosh(2*sphi)-cosh(sphi))+...
        sin(phi_m)^2*(sinh(2*sphi)-sinh(sphi))) + ...
             sr*exp(-2*sphi)*(cos(phi_m)^2*(2*cosh(2*sphi)-cosh(sphi))+ ...
                              sin(phi_m)^2*(2*sinh(2*sphi)-sinh(sphi)));
     R_a(2,2) = r_m^2*exp(-2*sphi)*(sin(phi_m)^2*(cosh(2*sphi)-cosh(sphi))+...
                             cos(phi_m)^2*(sinh(2*sphi)-sinh(sphi)))+...
             sr*exp(-2*sphi)*(sin(phi_m)^2*(2*cosh(2*sphi)-cosh(sphi))+...
                        cos(phi_m)^2*(2*sinh(2*sphi)-sinh(sphi)));
     R_a(1,2) = sin(phi_m)*cos(phi_m)*exp(-4*sphi)*(sr+(r_m^2+sr)* ...
						    (1-exp(sphi))); 
     R_a(2,1) = R_a(1,2);

     debias = mu_a;
     P_debias = R_a;
   otherwise
    error(['Unknown flag ''',method,'''!'])
  end

  xy_debias = xy + debias;
  