Institut de Robòtica i Informàtica Industrial

UpdateFilterPlot

PURPOSE ^

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:
  • get Get function for robots.
  • get Get function for trajectories.
  • GetPoseEstimation Estimation of a given pose.
  • get Get function for EKF filters.
  • GetPoseEstimation Estimation of a given pose.
  • get Generic get function for filters.
  • GetPoseEstimation Estimation of a given pose.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • set Set functino for PoseData objects.
  • get Get function for Gaussians.
  • get Get function for poses.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
This function is called by:

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


Institut de Robòtica i Informàtica Industrial

Generated on Fri 24-Jul-2009 12:32:50 by m2html © 2003