www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/getMeasurement.m

    %   d = sqrt(px^2 + py^2) + rd : rd = measurement noise
%   a = atan(py/px) + ra

function [y, Y_x] = getMeasurement(x)

    px = x(1, :);
    py = x(2, :);
    
    d = sqrt(px.^2 + py.^2);
    a = atan2(py, px);
    
    y = [d; a];
    
    if nargout > 1 % We want Jacobians (only works for single point)
        Y_x = [...
            px/(px^2 + py^2)^(1/2),     py/(px^2 + py^2)^(1/2);
            -py/(px^2*(py^2/px^2 + 1)), 1/(px*(py^2/px^2 + 1))];
    end

end