www.gusucode.com > matlab编程实现平台的slam模拟器源码程序 > matlab编程实现平台的slam模拟器源码程序/ekf-slam-matlab-master/tests/medFilterScan.m
% Test ICP scan matching algorithm load medFilterScan.mat; rob = Robot; rob.R = [0; 0; 0]; rob.r = rob.R; rob.u = [2; pi/1.5]; % Set robot control to move in straight line sen = Sensor; sen.range = 30; sen.fov = 270*pi/180; axis_size = 7; sen.noise = [0.3; 3*pi/180]; % obs is an array of objects of type obstacle scan_data_1 = []; for i = 1:length(obs) scan_data_1_tmp = obs(i).getMeasured(sen, rob); scan_data_1 = [scan_data_1 scan_data_1_tmp]; end scan_data_1 = removeDuplicateLasers(scan_data_1); scan_data_1 = scan_data_1(2:3, :); % First add Gaussian white noise to scan v = repmat(sen.noise, 1, size(scan_data_1, 2)) .* randn(2,size(scan_data_1, 2)); scan_data_1 = scan_data_1 + v; scan_data_filt = scan_data_1; scan_data_1 = getInvMeasurement(scan_data_1); scan_data_filt(1,:) = medfilt1(scan_data_filt(1,:), 5); scan_data_filt(2,:) = medfilt1(scan_data_filt(2,:), 5); scan_data_filt = getInvMeasurement(scan_data_filt); p1 = rob.computeTriangle('true'); % Plot results figure('color', 'white') s1 = subplot(1,2,1); h1 = plot(p1(1,:), p1(2,:), 'b-', scan_data_1(1,:), scan_data_1(2,:), 'r+'); hold on for i = 1:length(obs) obs(i).plot(s1); end axis square axis([-axis_size axis_size -axis_size axis_size]) title('Raw noisy scan data') legend('wheelchair pose', 'scan points', 'obstacles') s2 = subplot(1,2,2); h2 = plot(p1(1,:), p1(2,:), 'b-', scan_data_filt(1,:), scan_data_filt(2,:), 'r+'); obs2 = obs; hold on for i = 1:length(obs) ob2graphics = copyobj(obs(i).graphics,s2); end axis square axis([-axis_size axis_size -axis_size axis_size]) title(['Scan data after' char(10) '1D median filtering']) %legend('scan before', 'scan after')