Yukari 4 and nowcasting

I have been convinced to do Yukari 4, by of all people, my lovely wife Kellie. I have a theory about what is wrong, and how to fix it with one wire and a software change.

What I think is going wrong is that there is too much lag between the time that the GPS calculates a fix and the machine is able to react to it. There is also an element of lag between the time the robot passes the waypoint and it gets the next GPS point.

Let's tell a story. One day, Yukari the robot is driving along, heading for the first waypoint. In fact, it's not headed for the first waypoint at all, but a point beyond the waypoint. Yukari knows about turning radii and imperfections in measurements, and never wants to steer to a point within its minimum turning radius. Therefore it aims for a point 10 meters beyond the waypoint, but switches to the next waypoint when it passes the first waypoint (not the target 10 meters beyond).

Yukari passes a waypoint at 18:01:32.16UTC. At this point in time, it is on the line perpendicular to the baseline running through the waypoint. However, the next GPS fix isn't until 18:01:33.00. Since Yukari is trucking along at 3m/s, it has gone more than 2m past the waypoint before it gets a waypoint fix. It doesn't receive the message from the GPS for another 0.2 seconds, and therefore goes another 0.6m past the waypoint. Yukari doesn't know that the GPS reading has been delayed, so it doesn't take this into account. It thinks that when it receives the GPS message, that the message is current. By this point, it might be in the fence.

Yukari, being a dumb robot, doesn't notice, and therefore turns a whole 3m late. By this time, it may be in the fence.

Yukari 4 is a much smarter robot. First, it pays attention to the PPS pulse from the GPS. As a result, it knows when the GPS reading takes effect. Second, it remembers the last several fixes. It draws a straight line through the fixes, predicts when it will pass the waypoint, and turns at that point.

More details. Previously I just wanted to record PPS because it was interesting to precisely synchronize things, but I didn't have any operational controlling reason to do so. Now I do. We put a wire from the GPS PPS to the Loginator AD5 pin, which is in turn in capture mode on timer 0, channel 2. We will track the last several GPS fixes, including their times. We can fit through the points. It won't be just a normal 2D fit, but rather two 2D fits, one through the time points and the northing, and one through the time and easting. These two lines can be considered a single parametric curve with time as the parameter.

Once we have these two points, we can find out when the dot product is zero:

The predicted position at any time is:
\[x(t)=m_xt+b_x\]
\[y(t)=m_yt+b_y\]

Or in vector form:

\[\vec{r}(t)=\vec{m}t+\vec{b}\]

The vector from the robot to the waypoint is:

\[\vec{r}_{robot}=\vec{i}(x_w-x(t))+\vec{j}(y_w-y(t))\]

The vector from the base waypoint to the current waypoint is a constant \(\vec{r}_{baseline}\), so the dot product is:

\[\begin{eqnarray}
\vec{r}_{robot}\cdot\vec{r}_{baseline}&=0&=&x_{baseline}(x_w-x(t))+y_{baseline}(y_w-y(t))\\
  & &=&x_{baseline}(x_w-(m_xt+b_x))+y_{baseline}(y_w-(m_yt+b_y))\\
  & &=&x_{baseline}(x_w-m_xt-b_x)+y_{baseline}(y_w-m_yt-b_y)\\
  & &=&x_{baseline}x_w-x_{baseline}m_xt-x_{baseline}b_x+y_{baseline}y_w-y_{baseline}m_yt-y_{baseline}b_y\\
  & &=&x_{baseline}x_w-x_{baseline}b_x+y_{baseline}y_w-y_{baseline}b_y-x_{baseline}m_xt-y_{baseline}m_yt\\
  & &=&x_{baseline}x_w-x_{baseline}b_x+y_{baseline}y_w-y_{baseline}b_y-(x_{baseline}m_x+y_{baseline}m_y)t\\
  & &=&x_{baseline}x_w+y_{baseline}y_w-x_{baseline}b_x-y_{baseline}b_y-(x_{baseline}m_x+y_{baseline}m_y)t\\
  & &=&r_{baseline}\cdot\vec{w}-\vec{r}_{baseline}\cdot\vec{b}-(r_{baseline}\cdot\vec{m})t\end{eqnarray}\]
\[\begin{eqnarray}
(r_{baseline}\cdot\vec{m})t&=&r_{baseline}\cdot\vec{w}-\vec{r}_{baseline}\cdot\vec{b}\\
                                            t&=&\frac{r_{baseline}\cdot\vec{w}-\vec{r}_{baseline}\cdot\vec{b}}{r_{baseline}\cdot\vec{m}}\\
\end{eqnarray}\]

This is the predicted time of waypoint passage. We can watch for this time, and if the actual time is greater than this, we change to the next waypoint. Might as well use floating point t() rather than integer TC which rolls over every minute. We watch for this time every time around loop(), not just every $GPRMC sentence. I have seen a similar technique referred to as "nowcasting", or in other words using data from the past to estimate the current state. It's not a forecast, because the estimation time (I keep wanting to type "prediction") is the present, not the future.