www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/scanPoint.m
% Author: Jai Juneja (adapted from SLAM course by Joan Sola) % Date: 12/02/2013 % % For a point p_g in the global frame, determine the range-bearing % measurement y in the robot's local frame. This first requires a % transformation of the point p_g to the robot's local frame to give p_r. % This is then converted to a range and bearing vector with getMeasurement. % % Inputs: % r = [x y alpha]' : Robot frame % p_g = [pg_x pg_y]' : Point in global frame % % Outputs: % y = [d a] : Range-bearing sensor measurement % Optional: % Y_r : Jacobian of y wrt. r % Y_pg : Jacobian of y wrt. p_g function [y, Y_r, Y_pg] = scanPoint(r, p_g) if nargout == 1 p_r = transToLocal(r, p_g); y = getMeasurement(p_r); % Obtain range-bearing measurement y of p_r else % Compute Jacobians: [p_r, PR_r, PR_pg] = transToLocal(r, p_g); [y, Y_pr] = getMeasurement(p_r); % From the chain rule, we deduce: Y_r = Y_pr * PR_r; Y_pg = Y_pr * PR_pg; end end