Some more cleanup and addition of a sample video in readme. A bug fix for demo video matlab script.