1 Introduction
Global Positioning System (GPS) devices are widely used to track vehicles and other moving objects. The devices generate a time series of noisy measurements of position and velocity that can then be used in batch and online reconstruction of trajectories.
GPS receivers are used to obtain trajectory information for a wide variety of reasons. The TracMap company, located in New Zealand and USA, produces GPS display units to aid precision farming in agriculture, horticulture and viticulture. With these units, operational data are collected and sent to a remote server for further analysis. GPS units also guide drivers of farm vehicles to locations on the farm that require specific attention.
Given a sequence of position vectors in a tracking system, the simplest way of constructing the complete trajectory of a moving object is by connecting positions with a sequence of lines, known as linebased trajectory representation
(Agarwal et al., 2003). Vehicles with an omnidirectional drive or a differential drive can actually follow such a path in a driveandturn fashion, though it is highly inefficient (Gloderer and Hertle, 2010). This kind of nonsmooth motion can cause slippage and overactuation (Magid et al., 2006). By contrast, most vehicles typically follow smooth trajectories without sharp turns.Several methods have been investigated to incorporate smoothness constraints into trajectory reconstruction. One approach uses the minimal length path that is continuously differentiable and consists of line segments or arcs of circles, with no more than three segments or arcs between successive positions (Dubins, 1957). This method is called Dubins curve and has been extended to other more complex vehicle models but is still limited to line segments and arcs of circles (Yang and Sukkarieh, 2010). However, there are still curvature discontinuities at the junctions between lines and arcs, leading to yaw angle errors (Wang et al., 2017).
Spline methods have been developed to construct smooth trajectories. Magid et al. (2006) propose a pathplanning algorithm based on splines. The main objective of the method is the smoothness of the path, not a shortest or minimumtime path. Yu et al. (2004)
give a piecewise cubic reconstruction found by matching the observed position and velocity at the endpoints of each interval; this is essentially a Hermite spline. More generally, a Bspline gives a closedform expression for the trajectory with continuous second derivatives and goes through the position points smoothly while ignoring outliers
(Komoriya and Tanie, 1989; BenArieh et al., 2004). Bsplines are flexible and have minimal support with respect to a given degree, smoothness, and domain partition. Gasparetto and Zanotto (2007) use fifthorder Bsplines to model the overall trajectory, allowing one to set kinematic constraints on the motion, expressed as the velocity, acceleration, and jerk. In the context of computer (or computerized) numerical control (CNC), Erkorkmaz and Altintas (2001) presented a quintic spline trajectory generation algorithm connecting a series of reference knots that produces continuous position, velocity, and acceleration profiles. Yang and Sukkarieh (2010) proposed an efficient and analytical continuous curvature pathsmoothing algorithm based on parametric cubic Bézier curves that can fit sequentially ordered points.A parametric approach only captures features contained in the preconceived class of functions (Yao et al., 2005) and increases model bias. To avoid this problem, nonparametric methods, such as smoothing splines, have been developed (Craven and Wahba, 1978). Let be a sequence of observation times in the interval satisfying , and let be the associated position observations. Smoothing spline estimates of appear as a solution to the following minimization problem: find that minimizes the penalized residual sum of squares,
(1) 
for a prespecified value (Aydin and Tuzemen, 2012). In equation (1), the first term is a residual sum of squares and penalizes lack of fit. The second term is a roughness penalty term where the smoothing parameter varies from 0 to . (The roughness penalty term is a formalization of a mechanical device: if a thin piece of flexible wood, called a spline, is bent to the shape of the curve , then the leading term in the strain energy is proportional to (Green and Silverman, 1993).) The reconstruction cost, equation (1), is determined not only by its goodnessoffit to the data quantified by the residual sum of squares but also by its roughness (Schwarz, 2000). For a given , minimizing equation (1) will give the best compromise between smoothness and goodnessoffit. When
the reconstruction is a smooth interpolation of the observation points; when
the reconstruction is a straight line. Notice that the first term in equation (1) depends only on the values of at . Green and Silverman (1993) show that the function that minimizes the objective function for fixed values of is a cubic spline: an interpolation of points via a continuous piecewise cubic function, with continuous first and second derivatives. The continuity requirements uniquely determine the interpolating spline, except at the boundaries (Sealfon et al., 2005).Zhang et al. (2013) propose Hermite interpolation on each interval to fit position, velocity and acceleration with kinematic constraints. Their default trajectory formulation is a combination of several cubic splines on every interval or, alternatively, is a single function found by minimizing
(2) 
where is a smoothing parameter (Castro et al., 2006).
A conventional smoothing spline is controlled by one single parameter, which controls the smoothness of the spline on the whole domain. A natural extension is to allow the smoothing parameter to vary as a function of the independent variable, adapting to the change of roughness in different domains (Silverman, 1985; Donoho et al., 1995). The objective function is now of the form
(3) 
Similar to the conventional smoothing spline problem, one has to choose the penalty function . The fundamental idea of nonparametric smoothing is to let the data choose the amount of smoothness, which consequently decides the model complexity (Gu, 1998). When is constant, most methods focus on datadriven criteria, such as crossvalidation (CV), generalized crossvalidation (GCV) (Craven and Wahba, 1978) and generalized maximum likelihood (GML) (Wahba, 1985). Allowing the smoothing parameter to be a function poses additional challenges, though Liu and Guo (2010) were able to extend GML to adaptive smoothing splines.
In this paper, we propose a smoothing spline – which we name the Vspline – that is obtained from noisy paired position data and velocity data . In Section 2, the objective function for the Vspline is introduced that depends on velocity residuals , as well as position residuals , and a parameter that controls the degree to which the velocity information is used in the reconstruction. We show that the Vspline can be written in terms of modified Hermite spline basis functions. We also introduce a particular adaptive Vspline that seeks to control the impact of irregularly sampled observations and noisy velocity measurements. A crossvalidation scheme for estimating the Vspline parameters is given in section 3. Section 4 details the performance of the Vspline on simulated data based on the Blocks, Bumps, HeaviSine and Doppler test signals (Donoho and Johnstone, 1994). Finally, an application of the Vspline to a real 2dimensional dataset is given in section 5.
2 The Vspline
We consider the situation of paired position data and velocity data at a sequence of times satisfying . For , we define the objective function
(4) 
where , , , and we have chosen the penalty function to be a piecewise constant function, i.e for ,
(5) 
From now on, we will understand to be given by eq. (5) and we will often use to refer to the set of .
Theorem 1.
For , the objective function is uniquely minimized by a cubic spline, piecewise on the intervals , , and linear on and .
We term the minimizer of (4) the Vspline because it incorporates velocity information and because of its application to vehicle and vessel tracking. The proof of Theorem 1 is in Appendix B.
Remark: In the language of splines, the points are the interior knots of the Vspline, and are the exterior or boundary knots. Since the Vspline is linear on and , without loss of generality we let and from now on.
2.1 Constructing basis functions
It is convenient to construct basis functions from cubic Hermite splines (Hintzen et al., 2010). If and are the position and velocity at time , and and are the position and velocity at time , then for , the cubic Hermite spline is defined as
This implies that for an arbitrary interval , the relevant cubic spline basis functions are
(6)  
(7)  
(8)  
(9) 
Consequently, the cubic Hermite spline on an arbitrary interval with two successive points and is expressed as
(10) 
For Vsplines, a slightly more convenient basis is given by , where , , and for all ,
and
Any can then be represented in the form
(11) 
where are parameters.
2.2 Computing the Vspline
In terms of the basis functions in the previous section, the objective function (4) is given by
(12) 
where and are matrices with components
(13)  
(14) 
and is a matrix with components . In the following, we reserve the use of boldface for vectors and matrices.
The detailed structure of is considered in Appendix A. It is convenient to write , where . It is then evident that is a bandwidth four matrix.
Since equation (12) is a quadratic form in terms of , it is straightforward to establish that the objective function is minimized at
(15) 
which can be identified as a generalized ridge regression. The fitted Vspline is then given by
.The Vspline is an example of a linear smoother (Hastie et al., 2009). This is because the estimated parameters in equation (15) are a linear combination of and . Denoting by and the vector of fitted values and at the training points , we have
(16) 
(17) 
where and are smoother matrices that depend only on and . It is not hard to show that and are symmetric, positive semidefinite matrices. Note that .
Theorem 2.
If is a Vspline then, for almost all and , is continuous at the knots if and only if and , for all .
2.3 The adaptive Vspline
Until now, we have not explicitly considered the impact of irregularly sampled observations or of noisy measurements of velocity on trajectory reconstruction. In order to do this, it is instructive to evaluate the contribution to the penalty term from the interval . Using (10), it is relatively straightforward to show that
(18) 
where , and is the average velocity over the interval. The can be interpreted as the difference at time and respectively between the velocity implied by an interpolating Hermite spline and the velocity implied by a straight line reconstruction.
The contribution to the penalty term is then
(19) 
where . We call the quantity , the square of the discrepancy of the velocity on the interval .
As a consequence of (19), larger time intervals will tend to contribute less to the penalty term (other things being equal). But this is exactly when we would expect the velocity at the endpoints of the interval to provide less useful information about the trajectory over the interval. In the case when the observed change in position is small, i.e. when , overreliance on noisy measurements of velocity will result in “wiggly” reconstructions. In these two instances – graphically depicted in Figure 0(a) – we would like the Vspline to adapt and to favour straighter reconstructions; this is a deliberate design choice. We can achieve this by choosing
(20) 
where is a parameter to be estimated. The penalty term then takes a particularly compelling form: the contribution from the interval (19) is proportional to
(21) 
for all . We call the resulting spline the adaptive Vspline. The spline when , we call the nonadaptive Vspline.
3 Parameter selection and crossvalidation
The issue of choosing the smoothing parameter is ubiquitous in curve estimation and there are two different philosophical approaches to the problem. The first is to regard the free choice of smoothing parameter as an advantageous feature of the procedure. The second is to let the data determine the parameter (Green and Silverman, 1993). We prefer the latter and use data to train our model and find the best parameters. The most wellknown method for this is crossvalidation.
In standard regression, which assumes the mean of the observation errors is zero, the true regression curve has the property that if an observation is omitted at time point , the value is the best predictor of in terms of returning the least value of . We use this observation to motivate a leaveoneout crossvalidation scheme to estimate and for both the nonadaptive and the adaptive Vsplines.
Let be the minimizer of
(22) 
and define the crossvalidation score
(23) 
We then choose and that jointly minimize .
The following theorem establishes that we can compute the crossvalidation score without knowing the :
Theorem 3.
The crossvalidation score of a Vspline satisfies
(24) 
where is the Vspline smoother calculated from the full data set with smoothing parameter and , and , etc.
4 Simulation study
In this section, we give an extensive comparison of methods for regularly sampled time series data followed by simulation of irregularly sampled data. The comparison is based on the ability to reconstruct trajectories derived from Blocks, Bumps, HeaviSine and Doppler, four functions which were used in (Donoho and Johnstone, 1994, 1995; Abramovich et al., 1998) to mimic problematic features in imaging, spectroscopy and other types of signal processing.
Letting denote any one of Blocks, Bumps, HeavisSine or Doppler, we treat as the velocity of the trajectory , i.e. . Numerically, we calculate the position data via
(25) 
where . The observed position and velocity are then found by adding i.i.d. zeromean Gaussian noise:
(26) 
where , ,
is the standard deviation of the positions
, is the standard deviation of the velocities , and SNR is the signaltonoise ratio, which we take to be 3 or 7.4.1 Regularly sampled time series
We compare the performance of the adaptive Vspline with two wavelet transform reconstructions (Donoho and Johnstone, 1995; Abramovich et al., 1998), a spatially adaptive penalized spline known as the Pspline (Krivobokova et al., 2008; Ruppert et al., 2003), as well as the adaptive Vspline with and the nonadaptive Vspline. It is important to note that only the nonadaptive and adaptive Vsplines incorporate velocity information. For the wavelet transform approach, we use both the sure threshold policy and BayesThresh with levels . The Vspline parameters are obtained by minimizing the crossvalidation score (24). Following Nason (2010), we fix in the simulations.
4.1.1 Numerical Examples
Figures 2 to 5 give reconstructions of the trajectories based on the Blocks, Bumps, Heaviside and Doppler functions respectively, when SNR. Apart from the adaptive Vspline, we find all methods tend to oversmooth the reconstructions. Wavelet (sure) suffers from an occasional “highfrequency” breakdown and wavelet (BayesThresh) gives spurious fluctuations near the boundary knots. The Pspline gives a smoother reconstruction than wavelet reconstructions, but does not perform as well as the adaptive Vspline with . The value of incorporating velocity information can be seen in the performance of the nonadaptive and adaptive Vsplines. However, the nonadaptive Vspline can perform poorly when there are large absolute changes in velocity, e.g. in the Blocks and Bumps trajectories. Overall, the adaptive Vspline performs much better than the other methods and returns neartrue reconstructions.
Figure 6 illustrates the trajectory velocities obtained by taking the first derivative of the adaptive Vspline trajectory. It is clear that the Vspline incorporates velocity information in order to improve trajectory reconstruction at the expense of smooth velocity reconstruction.
4.1.2 Evaluation and residual analysis
To examine the performance of the adaptive Vspline, we compute the true mean squared error for each of the reconstructions:
TMSE  (27) 
The results are shown in Table 1. The adaptive Vspline returns the smallest true mean squared errors for the Blocks and Bumps trajectories. For the Heaviside and Doppler trajectories, only the nonadaptive Vspline shows similar performance. Further analysis also shows that the residuals from the Vsplines are not significantly correlated at any lag, as expected.
TMSE  SNR  Vspline  VS  VS  Pspline  W(sure)  W(Bayes) 

Blocks  7  1.75  54.25  28.68  54.76  201.02  182.12 
3  16.44  152.5  30.76  171.59  1138.08  712.36  
Bumps  7  1.64  23.44  21.10  24.21  71.71  69.26 
3  8.51  77.78  37.12  77.52  330.77  238.79  
HeaviSine  7  1.53  7.80  1.56  9.54  55.37  44.88 
3  8.21  33.56  8.49  34.26  240.72  110.49  
Doppler  7  1.51  6.67  1.08  8.26  14.87  12.01 
3  8.10  22.14  8.25  19.95  81.48  50.33 
Table 2 shows the ability of the adaptive Vspline to retrieve the true SNR, calculated by .
SNR  predefined value  generated  Vspline 

Blocks  7  6.9442  6.9485 
3  2.9761  2.9817  
Bumps  7  6.9442  6.9548 
3  2.9761  2.9953  
HeaviSine  7  6.9442  6.9207 
3  2.9761  2.9706  
Doppler  7  6.9442  6.8757 
3  2.9761  2.9625 
4.2 Irregularly Sampled Time Series Data
In this section, we show that the proposed Vspline has the ability to reconstruct the true trajectory even though the data is irregularly sampled. For each of the functions of the previous section, we set SNR to 7 and generate a mother simulation of length . We then obtain a regularly sampled daughter simulation of length 512 using the indices , and an irregularly sampled daughter simulation with 512 indices chosen at random.
Table 3 shows that the TMSE tends to increase when the data are irregularly sampled. In the case of Blocks and Bumps, the TMSE increases by a factor 3 or 4. However the ability to retrieve the true SNR does not appear to be affected; see Table 4.
The reconstructions themselves are shown in Figure 7. In the irregularly sampled cases, the reconstruction tends to smooth some features in the underlying trajectories. In Bumps and, to a lesser extent, in Doppler, the reconstruction occasionally adds in features. This occurs when there are no sampled data at times when the underlying velocity is changing rapidly.




TMSE  

Regular  Irregular  
Blocks  3.5197  10.8596 
Bumps  1.6662  6.2586 
HeaviSine  1.1275  1.1077 
Doppler  1.0101  1.7832 
SNR  Regular  Irregular 

Blocks  7.0479  6.8692 
Bumps  7.0241  7.1937 
HeaviSine  7.2367  6.8793 
Doppler  6.8692  7.3645 
5 Inference of Tractor Trajectories
Real world vehicle trajectories exhibit many of the features seen in the test trajectories considered in the previous section, particularly the Blocks, Bumps and HeaviSine trajectories. In this section, we extend the Vspline to more than one dimension and apply it to a real dataset, which was obtained from a GPS unit mounted on a tractor working in a horticultural setting. The data was irregularly recorded with highly variable time differences . The original dataset contains records of longitude, latitude, speed, bearing and the status of the tractor’s boom sprayer. These data were converted into northing and easting, and the velocities in those directions. The boom status, “up” or “down”, denotes the operational state of the tractor, and may indicate different types of trajectories.
5.1 The Vspline in dimensions
To generalize the Vspline to dimensions, we consider the situation preceding eq. (4) but where now . Then the function is a dimensional Vspline if it minimizes:
(28) 
where is the Euclidean norm in dimensions. For each direction , the fitted Vspline has the form , where
(29) 
The parameters and are estimated by minimizing the crossvalidation score:
(30) 
In what follows, we allow the nonadaptive and adaptive Vsplines to depend on the boom status. Specifically, letting denote boom “up”, denote boom “down”, and be the average velocity on the interval , the penalty term for the nonadaptive Vspline is
(31) 
and for the adaptive Vspline it is
(32) 
5.2 Trajectory reconstruction
It is instructive to first consider what happens when the northing and easting trajectories are reconstructed separately. To facilitate comparison with the wavelet approach we restrict attention to the first 512 observations. The data are shown in Figure 8. It is evident that the trajectory of the tractor is typically characterized by motion along rows with boom down, and tight turns at the ends of rows with boom up. In this section, we use to denote easting and to denote northing.
Figure 9 compares different methods for reconstructing the northing trajectory. The Pspline reconstruction fails due to instances where . Wavelet (sure) overshoots at turning points, as does the nonadaptive Vspline, though more smoothly. The nonadaptive Vspline overshoots because it is incorporating outdated velocity information during a period where the velocity is changing quickly. On the other hand, wavelet (BayesThresh), the adaptive Vspline with and the adaptive Vspline give acceptable results.
5.3 2Dimensional Trajectory
Figure 10 shows the full 2D adaptive Vspline reconstruction of the tractor trajectory for the first 512 observations, as well as the derived northing and easting trajectories. Also depicted by red dots of varying sizes are the sizes of the penalty terms . As expected, the penalty terms typically become large at turning points and before long waiting periods. A histogram of the and the implied penalty function are given in Figure 11. Figure 12 is a full reconstruction from the complete dataset.

6 Discussion
In this paper, a smoothing spline called the Vspline is proposed that minimizes an objective function which incorporates both position and velocity information. Given knots, the Vspline has degrees of freedom corresponding to cubic polynomials with their value and first derivative matched at the interior knots. The degrees of freedom are then fixed by position observations and velocity observations. Note that in the limit , the Vspline reduces to having degrees of freedom. (See Appendix C for details.) An adaptive version of the Vspline is also introduced that seeks to control the impact of irregularly sampled observations and noisy velocity measurements. In simulation studies, the Vspline shows improved true mean squared error in reconstructions.
In most work on polynomial smoothing splines, observation errors are assumed to be independent. However, this can be a questionable assumption in practice and it is known that correlation greatly affects the selection of smoothing parameters (Wang, 1998). Parameter selection methods such as generalized maximum likelihood (GML), generalized crossvalidation (GCV) typically underestimate smoothing parameters when data are correlated. To accommodate an autocorrelated error sequence, Diggle and Hutchinson (1989) extend GCV, and Wang (1998) extends GML and unbiased risk (UBR) to simultaneously estimate the smoothing and correlation parameters. Kohn et al. (1992) also propose an algorithm to evaluate crossvalidation functions when the autocorrelated errors are modelled by an autoregressive moving average. Here we show that there is a generalized crossvalidation scheme for the Vspline that is appropriate for correlated observation errors.
When observation errors are correlated (and considering splines in onedimension for simplicity), the Vspline minimizes
(33) 
where and are precision matrices, assumed known. The solution is , where
(34) 
The usual leaveoneout crossvalidation algorithm requires knowledge of the diagonal elements of the smoother matrices. GCV achieves computational savings and robustness by approximating , etc. (Syed, 2011). The natural extension of the GCV to the Vspline with correlated errors is
(35) 
One current drawback of the Vspline is that it is slower than other splinebased methods, however we have not yet optimized CV parameter estimation, particularly around solving for in (15). Future directions for the Vspline include application to ship tracking (Hintzen et al., 2010) and development of a fast online filtering mode.
Acknowledgements
This work was funded by grant UOOX1208 from the Ministry of Business, Innovation & Employment (NZ). GPS data was provided by TracMap (NZ).
Appendix A Penalty Matrix in (12)
We have , where . Thus is a bandwidth four symmetric matrix and its nonzero, upper triangular elements are
(36)  
(37)  
(38)  
(39)  
(40)  
(41)  
(42)  
(43)  
(44)  
(45) 
where and
Comments
There are no comments yet.