Add the bearing and range based EKF and PF demos; Fix a number of bugs.