Initial self-navigation system

[whymarrh]

This PR is the implementation of our initial control system, as (is being) discussed in #75.

This PR includes:

• [x] An error function for the GPS values (using the haversine formula)
• Two PID controllers
  • [x] A controller for surge
  • [x] A controller for yaw
• [x] Wiring the PID controller outputs to the props

[arandell93]

Would it be appropriate to have @michaelabarnes check your work on this one?

[whymarrh]

This PR still has a few open tasks before it's ready to be merged.

That said, the bits that @michaelabarnes would be interested in are implemented.

[whymarrh]

Refs #83

[whymarrh]

I think this implementation should be ready. For each controller we can:

• Set initial gains (which are 1, currently)
• Define a error function e(y(t))
• Define a clamp function for controller output
• Adjust ∆t indirectly by adjusting the interval at which inputs are given to the controller (it calculates ∆t internally)
• Adjust gains (k_p, k_i, and k_d)

As for the distance and bearing calculations used as the error functions, I've tested their output with the following cases (from @michaelabarnes' notes).

Bearing (used for yaw):

Longitude1	Latitude1	Longitude2	Latitude2	Bearing (degrees)
18.25	-34.1	-25.056667	14.89	312.4833
124.783333	48.506667	-153.545	-27.463333	114.5923

Distance (used for surge):

Longitude1	Latitude1	Longitude2	Latitude2	Distance (metres)
18.25	-34.1	-25.056667	14.89	7117151.82
124.783333	48.506667	-153.545	-27.463333	11685167.01
-4.315	-47.17	2.355	-47.17	504052.997
-37.418333	39.273333	-15.458333	39.273333	1885640.085
-47.78	45.493333	-44.498333	45.493333	255777.597
-176.705	-32.956667	177.475	-32.956667	542946.6889

As a sanity check, I have the following three tests for the controller:

1. No input produces no output
2. Starting at the setpoint produces zero as its output
3. e(t) = 5, u(t) = 5

For the controller tests:

• The same error function is used for all tests: e(y(t)) = r(t) - y(t)
• The identity function is used for clamping (i.e. there is no clamping)
• The setpoint, r(t), is 10
• All of the gains are 1 and ∆t is 1

[whymarrh]

@arandell93 @michaelabarnes if either of you wanted to double-check my math that'd be great

[arandell93]

e(t) = 5, u(t) = 5

What is this test?

[whymarrh]

I've edited my comment to add the (missing) value of ∆t.

I was using the terminology from the diagram on Wikipedia (that was linked in #75):

The test is that a input of e(t) = 5 produces 5 as u(t).

[FifoIronton]

Looking back at how the bearing PID controller was implemented in the early labs of that mobile robotics course I looked at all those months ago, they do something like this for a P controller:

% heading error
e_h = theta_d-theta;
e_h = atan2(sin(e_h), cos(e_h));

% PID for heading
w = k_p*e_h; 

Where e_h is the heading error, theta is the bot's heading, theta_d is the desired heading, w is the angular velocity used to turn the bot, and k_p is the proportional gain.

Using atan2(sin(e_h),cos(e_h)) avoids all of the weirdness that comes from the fact that angles happen on a circle, while keeping w linearly correlated to the heading error. This also requires that bearing and stuff is in radians.

[whymarrh]

What is e_k in that example?

[FifoIronton]

Haha, the code originally had 'e_k' in place of 'e_h', i changed it because the k didn't stand for something obvious. I'll edit that last comment to fix it

[arandell93]

I don't follow the trig function applied here. What does it look like as a mathematical expression (as opposed to unicode)?

[FifoIronton]

More or less, it cuts out the problem that angles are on a circle. Look at the godawful image below:

You got a triangular robot pointing in the red direction θ, that wants to go _θd. Your angle error is not _θ - θd, because that would be much larger than the actual angular difference between these two angles.

There are a lot of ways to get around this, but the one presented here is to get the angular difference in the range (-pi,pi) by converting the angle into a point on the unit circle (y coordinate found with sin, x cooridinate using cos) and then turning it back into an angle using atan2, an arctan function that returns angles in that range. For comparison, the arctan function on your calculator only returns angles in the range (0,pi). I turned the diagram sideways because it makes the angles make more sense.

(I need to learn how to draw arcs)

Does this make any sense?

[arandell93]

get the angular difference in the range (-pi/2,pi/2)

Should this be [-pi, pi]?

I may be missing something here, but isn't it easier to just map[180, 360] to [-180, 0] after the subtraction of θ - θ_d? This would provide you with your error from [-180, 180]. In that way you get a sign which will represent the direction you must turn (negative means turn to starboard, positive means turn to port) and it avoids a whole bunch of math.

Edit: Since the result of our subtraction of θ - θ_d can be anywhere in the range {-360, 360}, for my note above to work we would need to also map [-180, -360] to [180, 0]

To summarize, we can do a piecewise function as follows:

'θ - θ_d' is in the range [-360, -180], map to [0, 180] 'θ - θ_d' is in the range [-180, 0], no mapping required 'θ - θ_d' is in the range [0, 180], no mapping required 'θ - θ_d' is in the range [180, 360], map to [-180, 0]

[FifoIronton]

Should this be [-pi, pi]?

Yeah, yeah it should.

The purpose of this is to do everything in a line and avoid if statements and all of that. It also makes heading estimation easier in the future if we're going to add that in (say our heading is based on more than just measurements, also on how much we expect it to turn), because if we estimate that we're turning there are rare opportunities that we could estimate our error to be outside of the range [-360,360].

Using sin, cos, and atan2 is slightly more robust than just mapping, although functionally identical for most of what we're doing. It wouldn't be a whole lot of work to do it with modulo functions too, this is just one implementation that we really can't screw up.

[whymarrh]

.@arandell93 what part of the the atan2 function do you find off-putting? The Wikipedia page has a section on its definition and computation that offers the following piecewise function:

If you compose atan2 with sine and cosine, as in atan2(sin(x), cos(x)), that boils down to shifts of x (i.e. x, x + π, x - π), shifts not unlike your piecewise function. As Nick said, atan2 is just one of the many ways to get the same result.

That's all to say that your proposed function is equivalent to the one Nick put forward.

Using atan2(sin(x), cos(x)):^[1]

Anthony's piecewise function:^[2]

Edit: composing atan2 with sine and cosine results in shifts of x, not shifts of tan(x)

[arandell93]

Well now I just feel dumb.

Let's note that the range goes from -3 to 3. We'll need that when tuning.

[whymarrh]

Let's note that the range goes from -3 to 3. We'll need that when tuning.

More specifically -π to π

[whymarrh]

Back on the topic of the PID controller (what's in this PR), I've created the following two tables to describe two of the tests I've implemented for the core controller logic/calculations:

System 1:

t	Δt	e(t)	Kp	Ki	Kd	u(t)
0	0	0	1	1	1	0
1	1	5	1	1	1	5
2	1	3	1	1	1	13
3	1	6	1	1	1	17

System 2, showing the behaviour of the default T_i value of 4⋅Δt:

t	Δt	e(t)	Kp	Ki	Kd	u(t)
0	0	0	1	0	0	0
1	1	5	1	1	1	5
2	1	6	1	1	1	16
3	1	7	1	1	1	24
4	1	8	1	1	1	33
5	1	9	1	1	1	38
6	1	0	1	1	1	33
7	1	0	1	1	1	17
8	1	1	1	1	1	10
9	1	4	1	1	1	6
10	1	3	1	1	1	12

Notes:

• I've tried to keep to the terminology from the image linked above
• I've skipped the desired (r) and measure (y) process variables and have just included the error values—I have other tests for the error functions

The tests (shown in 97b2eb5) input the listed error values over time and assert the correct outputs.

[whymarrh]

I'm not going to rewrite commits in this PR, so note that there's some flip-flopping on the go.

The two tables in my previous comment were seemingly incorrect (they derivative calculation they used calculated de(t) backwards—fixed in 06d1fb1). The new, hopefully correct, tables:

System 1:

t	Δt	e(t)	Kp	Ki	Kd	u(t)
0	0	0	1	1	1	0
1	1	5	1	1	1	15
2	1	3	1	1	1	9
3	1	6	1	1	1	23

System 2, showing the behaviour of the default T_i value of 4⋅Δt:

t	Δt	e(t)	Kp	Ki	Kd	u(t)
0	0	0	1	0	0	0
1	1	5	1	1	1	15
2	1	6	1	1	1	18
3	1	7	1	1	1	26
4	1	8	1	1	1	35
5	1	9	1	1	1	40
6	1	0	1	1	1	15
7	1	0	1	1	1	17
8	1	1	1	1	1	12
9	1	4	1	1	1	12
10	1	3	1	1	1	10

[whymarrh]

Another system (03573a9), this with a larger integral time (> 10) and different gains:

t	Δt	e(t)	Kp	Ki	Kd	u(t)
0	0	0	2	1	8	0
1	1	9	2	1	8	99
2	1	8	2	1	8	25
3	1	7	2	1	8	30
4	1	6	2	1	8	34
5	1	5	2	1	8	37
6	1	4	2	1	8	39
7	1	3	2	1	8	40
8	1	2	2	1	8	40
9	1	1	2	1	8	39
10	1	0	2	1	8	37

[whymarrh]

A note about the implementation of the controller: the error value at t = 0 is always ignored (assumed zero) and no output is produced (which is why I have it set to zero in all of the tables above). The 2nd time-step is the first time-step that produces controller output using its error and previous error (0) to calculate change in error (it is also the first time-step that can calculate Δt).

While this isn't theoretically accurate of a PID controller, this concession does simplify the implementation enough to warrant it (in my opinion). Where we're clamping the output of the controller I don't think it makes much of a difference anyhow (e.g. the initial error spike is limited).

[arandell93]

Where we're clamping the output of the controller I don't think it makes much of a difference anyhow (e.g. the initial error spike is limited).

Agreed. Our boat's response will also be sluggish enough to basically ignore a spike, even it if it was not clamped and even though it will inflate the integral term for the first Ti timesteps. It would ramp the motors up fast, but the boat won't suddenly jerk about and throw the system out of whack because it's a slow ass boat.