![]() |
UpdateFilterPlotPURPOSE
Updates the plot of a filter with a new pose.
SYNOPSIS
function p=UpdateFilterPlot(F,step,p,redraw)
DESCRIPTION
Updates the plot of a filter with a new pose. Updates a filter plot with a new robot's pose: it extends the trajectory and displaces the ellipse representing the uncertainty on the robot's pose. The information used is that provided by the pose obtained at step 'step'. In general 'step' is the last execute simulation step but this function can be used to incrementaly display a previously estimated trajectory (see PlotFilter). Parameter 'p' is a plot structure as returned by InitPlotFilter. The structure is updated and returned to use it in subsequent calls to UpdateFilterPlot. If parameter redraw is true a drawnow command is issued at the end of the plot update. See also PlotFilter, InitPlotFilter. CROSS-REFERENCE INFORMATION
This function calls:
SOURCE CODE
0001 function p=UpdateFilterPlot(F,step,p,redraw) 0002 % Updates the plot of a filter with a new pose. 0003 % 0004 % Updates a filter plot with a new robot's pose: it extends the 0005 % trajectory and displaces the ellipse representing the uncertainty on 0006 % the robot's pose. 0007 % The information used is that provided by the pose obtained at step 0008 % 'step'. In general 'step' is the last execute simulation step but this 0009 % function can be used to incrementaly display a previously estimated 0010 % trajectory (see PlotFilter). 0011 % 0012 % Parameter 'p' is a plot structure as returned by InitPlotFilter. The 0013 % structure is updated and returned to use it in subsequent calls to 0014 % UpdateFilterPlot. 0015 % 0016 % If parameter redraw is true a drawnow command is issued at the end of 0017 % the plot update. 0018 % 0019 % See also PlotFilter, InitPlotFilter. 0020 0021 % Plot in the figure selected in the initialization 0022 h=gcf; 0023 if h~=p.h 0024 figure(p.h); 0025 end 0026 0027 % Get the curent pose: The pose for step 1 is the second pose since pose 1 0028 % is F.initialEstimation (i.e., it is set before starting the simulation) 0029 G=GetPoseEstimation(F,step); 0030 pm=get(G,'mean'); 0031 0032 % Replace the error ellipse with the most upt to date one 0033 p.poseXY=get(G,'ellipse'); 0034 set(p.pose,'xdata',p.poseXY(1,:),'ydata',p.poseXY(2,:)); 0035 uistack(p.pose,'top'); 0036 0037 % Replace the trace with the one including the new robot displacement 0038 p.traceX=[p.traceX pm(1)]; 0039 p.traceY=[p.traceY pm(2)]; 0040 set(p.trace,'xdata',p.traceX,'ydata',p.traceY); 0041 set(p.last,'xdata',[pm(1) pm(1)],'ydata',[pm(2) pm(2)]); 0042 0043 % Display the polygon representing the robot 0044 T=[[1 0 pm(1)];[0 1 pm(2)];[0 0 1]]*[[cos(pm(3)) -sin(pm(3)) 0];[sin(pm(3)) cos(pm(3)) 0];[0 0 1]]; 0045 rp=T*p.robotPoints; 0046 set(p.robot,'xdata',rp(1,:),'ydata',rp(2,:)); 0047 uistack(p.robot,'top'); 0048 0049 % Display everything 0050 if redraw 0051 drawnow; 0052 end 0053 0054 |