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.

Particle Cloud Pics

In order to get a better idea of whether the mean and standard deviation actually change based on a kidnapping event, I did what I should have done a while ago - I plotted the particle cloud poses for each timestep.  I excluded angle from the plots - mostly because plotting points in Matlab is something I know how to do right away, and plotting directional arrows would take some figuring out.

Here are some excerpts from the Major Kidnapping Event dataset, in which I drove (by hand) around the same route for 80 seconds, and at Timestep 70, forced the robot back to (0,0) in a Major Kidnapping Event.

Plots of Pose Estimates of AMCL's Particle Cloud:
Note: I should have included bearing and - more important - the weight assigned to each particle.


Time = 68 sec:

Time = 69 sec:

Time = 70 sec:

KIDNAPPING EVENT

(No significant change. Takes a couple seconds for the cloud to be aware of the problem)

Time = 71 sec:

Time = 72 sec:

Time = 73 sec:

FILTER SPREADS OUT

Time = 74 sec:

So, there's a clear dispersal of the poses at T=73 sec. 

Execution of Plan

Although nothing was difficult there was a lot of set-up code around this, and the usual mistakes and corrections. 

Here's an example of the data file generated with the new method:
https://drive.google.com/file/d/0BwpfRdaiQmbfQTh4Z3J0LUdCMHc/view?usp=sharing

My Matlab code for loading 5 such files:
https://drive.google.com/file/d/0BwpfRdaiQmbfRXpGS1VtZy04ZDg/view?usp=sharing

My Matlab code using the Neural Network Toolbox library:
https://drive.google.com/file/d/0BwpfRdaiQmbfZldKMTdPejJjY28/view?usp=sharing


Sadly, the results were poor:

New Plan

The new plan is to organize the inputs as follows:

particle_cloud_mean_pose_x_4 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_3 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_2 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_1 - particle_cloud_mean_pose_x_0
particle_cloud_mean_pose_x_0 - particle_cloud_mean_pose_x_0

particle_cloud_mean_pose_y_4
particle_cloud_mean_pose_y_3
particle_cloud_mean_pose_y_2
particle_cloud_mean_pose_y_1
particle_cloud_mean_pose_y_0

particle_cloud_mean_pose_qx_4
particle_cloud_mean_pose_qx_3
particle_cloud_mean_pose_qx_2
particle_cloud_mean_pose_qx_1
particle_cloud_mean_pose_qx_0

particle_cloud_mean_pose_qy_4
particle_cloud_mean_pose_qy_3
particle_cloud_mean_pose_qy_2
particle_cloud_mean_pose_qy_1
particle_cloud_mean_pose_qy_0

particle_cloud_mean_pose_qz_4
particle_cloud_mean_pose_qz_3
particle_cloud_mean_pose_qz_2
particle_cloud_mean_pose_qz_1
particle_cloud_mean_pose_qz_0

particle_cloud_mean_pose_qw_4
particle_cloud_mean_pose_qw_3
particle_cloud_mean_pose_qw_2
particle_cloud_mean_pose_qw_1
particle_cloud_mean_pose_qw_0

particle_cloud_stddev_x_4
particle_cloud_stddev_x_3
particle_cloud_stddev_x_2
particle_cloud_stddev_x_1
particle_cloud_stddev_x_0

particle_cloud_stddev_y_4
particle_cloud_stddev_y_3
particle_cloud_stddev_y_2
particle_cloud_stddev_y_1
particle_cloud_stddev_y_0

particle_cloud_stddev_qx_4
particle_cloud_stddev_qx_3
particle_cloud_stddev_qx_2
particle_cloud_stddev_qx_1
particle_cloud_stddev_qx_0

particle_cloud_stddev_qy_4
particle_cloud_stddev_qy_3
particle_cloud_stddev_qy_2
particle_cloud_stddev_qy_1
particle_cloud_stddev_qy_0

particle_cloud_stddev_qz_4
particle_cloud_stddev_qz_3
particle_cloud_stddev_qz_2
particle_cloud_stddev_qz_1
particle_cloud_stddev_qz_0

particle_cloud_stddev_qw_4
particle_cloud_stddev_qw_3
particle_cloud_stddev_qw_2
particle_cloud_stddev_qw_1
particle_cloud_stddev_qw_0

The new plan is to organize the OUTPUTS as a metric of error volume:

(AMCL_X - Gazebo_X) * (AMCL_Y - Gazebo_Y) * (AMCL_QX - Gazebo_QX) * (AMCL_QY - Gazebo_QY) * (AMCL_QZ - Gazebo_QZ) * (AMCL_QW - Gazebo_QW)

Note: the QX/QY/QZ/QW stuff is the angle in quaternion form.  A common angle is [0 0 0 1], and if AMCL and Gazebo poses ever align to that, the (0 - 0) in the resulting equation will cause the whole error metric to zero out.