Institut de Robòtica i Informàtica Industrial

ComputeNEES

PURPOSE ^

Quantifies the consitency of a filter.

SYNOPSIS ^

function n=ComputeNEES(F,T)

DESCRIPTION ^

 Quantifies the consitency of a filter.

 Quantifies the filter consistency using the NEES criterion at a 95%
 confidence level.
 Outputs above 1 indicate an inconsistent filter.

CROSS-REFERENCE INFORMATION ^

This function calls:
  • get Get function for robots.
  • get Get function for trajectories.
  • size Number of poses in a trajectory.
  • GetPoseEstimation Estimation of a given pose.
  • get Get function for EKF filters.
  • GetPoseEstimation Estimation of a given pose.
  • State2Step Returns the step corresponding to a state.
  • get Generic get function for filters.
  • size Size of the state estimated in the filter
  • GetPoseEstimation Estimation of a given pose.
  • get Get function for TRO filters.
  • get Get function for BTrees.
  • get Get function for PoseData objects.
  • get Get function for Gaussians.
  • double Converts an interval to a double.
  • size Size (rows/columns) of an interval matrix.
  • double Converts a double
  • get Get function for poses.
  • size Number of parameters of the pose.
  • Pose Generic pose constructor.
  • get Generic get for relative positioning sensors
  • get Generic get for sensors
  • size Number of readings stored in the Sensor.
This function is called by:
  • Simulation Simulates a robot performing Pose SLAM.

SOURCE CODE ^

0001 function n=ComputeNEES(F,T)
0002 % Quantifies the consitency of a filter.
0003 %
0004 % Quantifies the filter consistency using the NEES criterion at a 95%
0005 % confidence level.
0006 % Outputs above 1 indicate an inconsistent filter.
0007 %
0008    
0009 alpha=0.95;
0010 
0011 if true
0012   % Get the ground truth for the poses actually stored in the filter
0013   ne=size(F);
0014   dim=get(F,'dim');
0015   nStates=get(F,'nStates');
0016   
0017   e=zeros(ne,1);
0018   mu=get(F,'mu');
0019   ind=1:dim;
0020   for i=1:nStates
0021     e(ind)=double(Pose(mu(ind))-T{State2Step(F,i)});
0022     ind=ind+dim;
0023   end
0024   
0025   Lambda=get(F,'Lambda');
0026   
0027   m=e'*Lambda*e;
0028   
0029   n=m/chi2inv(alpha,ne);
0030 else
0031   dim=get(F,'dim');
0032   nStates=get(F,'nStates');
0033   step=State2Step(F,nStates);
0034   
0035   ep=GetPoseEstimation(F,step); %estimated pose
0036   
0037   e=double(Pose(get(ep,'mean'))-T{step});
0038   Lambda=get(ep,'invCovariance');
0039   
0040   m=e'*Lambda*e;
0041   n=m/chi2inv(alpha,dim);
0042 end


Institut de Robòtica i Informàtica Industrial

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