Covariance Spike Indicates Kidnapping (And Shows Minor Kidnapping Sim Didn't Work)

In looking at the particle cloud data some more, I started using the ROS tool rqt.  It's a GUI for viewing the contents of bag files. (To run rqt: in one console window, type in "roscore" and hit Enter. In a new console, type "rqt" and hit Enter.)


I was looking for the weights assigned to the particle cloud elements, and hoping I'd find them in the bag file of the AMCL data gathered while driving the route around the test Gazebo world.  I didn't find any messages about particle weights, but I was able to use rqt to view a property of the amcl_pose message that stores the covariance matrix for the particle cloud's poses. 

Turns out, for Major Kidnapping Events, the first element of the covariance matrix shows a big spike almost immediately:

The red line of the plot above is the marker for Time = 70, the time at which the robot was forced to (0,0) during navigation.

The covariance plot (first element of covariance, that is) for the same route WITHOUT a kidnapping event looks like this:

*************************************
These spikes occur in every Major Kidnapping dataset recorded with driving via the teleop method or the pid controller method.  They did not occur with the map_server method, but that dataset has a lot of little variation in the covariance plots, so the robot probably didn't know where it was well enough to have the kidnapping event affect the covariance.

Anyway, covariance seems like a signal worth pursuing as a way to check for Major Kidnapping.
*************************************

Further findings:


When I looked at the covariance plots for Minor Kidnapping Events, a spike wasn't easily identified.  Could it be that AMCL consistently recovered from the Minor Kidnapping act of changing the robot's current angle by 10 degrees? 

I checked out how well the AMCL pose estimates aligned to the Gazebo poses in Minor Kidnapping simulations.  Turns out, the poses are pretty close.

The red line in the figure above is the timestep at which the robot was kidnapped by +10 degrees, at 70 seconds.  As you can see, there is no significant peak after the kidnapping.

I did a comparison of the AMCL poses and the Gazebo poses over time.  In the plot below, the X's are close to one another and the Y's are close to eachother, too. 

This leads me to conclude that:

1. The minor kidnapping of 10 degrees is a "kidnapping" that AMCL is able to recover from.  Other variations on the change in the robot's orientation should be tested.

2. A lack of a covariance spike means a kidnapping event was not present.  However - what about the rise in the plot between 40 and 50?  Needs further attention.