www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tools/invScanPoint.m
% Author: Jai Juneja (adapted from SLAM course by Joan Sola) % Date: 12/02/2013 % % For a range-bearing measurement y in the robot's local frame, determine % the corresponding point measured in the global frame. This first requires % a conversion of the range-bearing measurement into a cartesian point p_r % in the robot's frame. transToGlobal then transforms p_r to a point p_g in % the global frame. % % 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 [p_g, PG_r, PG_y] = invScanPoint(r, y) if nargout == 1 p_r = getInvMeasurement(y); % Convert range-bearing measurement to local point p_g = transToGlobal(r, p_r); else % Compute Jacobians: [p_r, PR_y] = getInvMeasurement(y); [p_g, PG_r, PG_pr] = transToGlobal(r, p_r); % From the chain rule, we deduce: PG_y = PG_pr * PR_y; end end