fitlinepolar.m

用于移动机器人SLAM的仿真工具箱（CAS Robot Navigation Toolbox）。由Kai Arras编写。可迅速构建移动机器人仿真平台。

%FITLINEPOLAR Fit alpha,r line to points in polar coordinates.
%   [X,C] = FITLINEPOLAR(P) fits a line in the weighted least squares
%   sense to points P in polar coordinates minimizing perpendicular
%   errors from the points to the line. Uncertainties in rho are
%   assumed and propagated through the fit expressions yielding the
%   parameter covariance matrix C. P is a nx3 matrix with n>=3 with
%   theta in the first column, rho in the second column and sigma rho
%   in the third column. The function returns the parameter vector
%   X = [alpha; r] and the covariance matrix C = [saa sar; sar srr].
%
%   Reference (contains derivation of fit expressions):
%      K.O. Arras, "Feature-Based Robot Navigation in Known and Unknown
%      Environments", Ph.D. dissertation, Nr. 2765, Swiss Federal Insti-
%      tute of Technology Lausanne, Autonomous Systems Lab, June 2003.
%
%   See also REGRESS.

% v.1.0, ~1996, Kai Arras, IfR-ETHZ: Main idea from diploma thesis
% v.1.1, ~2000, Kai Arras, ASL-EPFL: vectorization
% v.1.2, Dec.2003, Kai Arras, CAS-KTH: toolbox version

function [p,C] = fitlinepolar(x);

[n,m] = size(x);
if n >= 3,
  
  % Transform polar to cartesian
  costvec = cos(x(:,1));
  sintvec = sin(x(:,1));
  xy(:,1) = x(:,2).*costvec;
  xy(:,2) = x(:,2).*sintvec;
  
  sum_weights = sum(x(:,3).^-2);
  xmw = sum(x(:,3).^-2 .* xy(:,1)) / sum_weights;
  ymw = sum(x(:,3).^-2 .* xy(:,2)) / sum_weights;
  
  % alpha
  nom   = -2*sum(x(:,3).^-2.* (xy(:,1) - xmw)   .* (xy(:,2) - ymw)    );
  denom =    sum(x(:,3).^-2.*((xy(:,2) - ymw).^2 - (xy(:,1) - xmw).^2));
  alpha = 0.5 * atan2(nom,denom);
  
  % r
  r = xmw*cos(alpha) + ymw*sin(alpha);
  
  % Eliminate negative radii
  if r < 0,
    alpha = alpha + pi;
    r = -r;
  end;
  p(1,1) = alpha;
  p(2,1) = r;
  
  % Computing the WEIGHTED COVARIANCE MATRIX C %
  N = nom; D = denom;
  dr_dalpha = ymw*cos(alpha) - xmw*sin(alpha);
  w_i = x(:,3).^-2;

  % sigma_aa vectorized %
  dN_drhoi_v = 2*w_i.*(xmw*sintvec + ymw*costvec - x(:,2).*sin(2*x(:,1)));
  dD_drhoi_v = 2*w_i.*(xmw*costvec - ymw*sintvec - x(:,2).*cos(2*x(:,1)));
  sigma_aa_v = sum((D*dN_drhoi_v - N*dD_drhoi_v).^2.*x(:,3).^2) / (2*(N^2 + D^2))^2;
    
  % sigma_rr vectorized %
  dalpha_drhoi_v = (D*dN_drhoi_v - N*dD_drhoi_v)./(2*(N^2 + D^2)); 
  sigma_rr_v = sum((dalpha_drhoi_v.*dr_dalpha + w_i.*cos(x(:,1)-alpha)/sum_weights).^2.*x(:,3).^2);
  
  % sigma_ar vectorized %
  dr_drhoi_v = dalpha_drhoi_v*dr_dalpha + w_i.*cos(x(:,1)-alpha)/sum_weights;
  sigma_ar_v = sum(dalpha_drhoi_v.*dr_drhoi_v.*x(:,3).^2);

  C(1,1) = sigma_aa_v;
  C(1,2) = sigma_ar_v;
  C(2,1) = sigma_ar_v;
  C(2,2) = sigma_rr_v;
else
  p = []; C = [];
  disp('fitlinepolar: Too few points.');
end;