Angdiff issues

[joeycaley]

Hello,

I've run into a couple of issues with the angdiff function while playing around with the particle filter example.

First, in RandomPath.m on line 198, the heading error is always returned as the "long way around". This is because the angles in angdiff are flipped, with the goal heading being subtracted from the robot heading rather than the proper goal heading minus the robot heading. So for example, if the robot has a heading of 0 and it wants to be pointed at pi/3, the angdiff call of angdiff(goal_heading, driver.veh.x(3)), as per MATLAB's documentation, calculates driver.veh.x(3) - goal_heading. This returns a negative error, driving the robot down to turn the opposite direction. Once it reaches the antiparallel heading (-2*pi/3), it stabilizes as the error will always drive the long way around due to the [pi, -pi] limit of angdiff. To fix this, angdiff just needs to be called in the opposite order angdiff(driver.veh.x(3), goal_heading) so that it computes the error signal properly as goal_heading - driver.veh.x(3).

The error in the particle filter class is much simpler. angdiff must have 2 vectors of the same size, which presents issues when comparing vectors and scalars. Whenever angdiff is called to provide something like the residual between the expected measurement heading and the measured measurement heading for all the particles, the measured measurement heading (or whatever is the scalar value) just needs to turned into a vector with something like x_scalar*ones(n_particles,1).