# Adaptive Smoothing V-Spline for Trajectory Reconstruction

Trajectory reconstruction is the process of inferring the path of a moving object between successive observation points. Widely separated points and measurement errors can cause traditional spline method to infer trajectories with sharp angles not typical of a moving object. Smoothing spline methods can efficiently build up a more smooth trajectory. In conventional smoothing splines, the objective function is augmented with a penalty term, which has a single parameter that controls the smoothness of reconstruction. Adaptive smoothing splines extend the single parameter to a function that can vary, hence the degree of smoothness can be different regions. A new method named the V-spline is proposed, which incorporates both location and velocity information but penalizes excessive accelerations. In the application of interest, the penalty term is also dependent on a known operational state of the object. The V-spline includes a parameter that controls the degree to which the velocity information is used in the reconstruction. In addition, the smoothing penalty adapts the observations are irregular in time. An extended cross-validation technique is used to find all spline parameters.

## Authors

• 3 publications
• 8 publications
• 4 publications
03/20/2018

### V-Splines and Bayes Estimate

Smoothing splines can be thought of as the posterior mean of a Gaussian ...
04/21/2020

### An Asympirical Smoothing Parameters Selection Approach for Smoothing Spline ANOVA Models in Large Samples

Large samples have been generated routinely from various sources. Classi...
03/01/2020

### Roughness Penalty for liquid Scorecards

A liquid scorecard has liquid characteristics, for which the characteris...
02/12/2020

### Asymptotics for M-type smoothing splines with non-smooth objective functions

M-type smoothing splines are a broad class of spline estimators that inc...
06/08/2021

### Spline Smoothing of 3D Geometric Data

Over the past two decades, we have seen an increased demand for 3D visua...
01/18/2022

### General P-splines for non-uniform B-splines

P-spline represents an unknown univariate function with uniform B-spline...
03/23/2022

### Adaptive Regularization of B-Spline Models for Scientific Data

B-spline models are a powerful way to represent scientific data sets wit...
##### This week in AI

Get the week's most popular data science and artificial intelligence research sent straight to your inbox every Saturday.

## 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 on-line 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 line-based trajectory representation

(Agarwal et al., 2003). Vehicles with an omnidirectional drive or a differential drive can actually follow such a path in a drive-and-turn fashion, though it is highly inefficient (Gloderer and Hertle, 2010). This kind of non-smooth motion can cause slippage and over-actuation (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 path-planning algorithm based on splines. The main objective of the method is the smoothness of the path, not a shortest or minimum-time 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 B-spline gives a closed-form expression for the trajectory with continuous second derivatives and goes through the position points smoothly while ignoring outliers

(Komoriya and Tanie, 1989; Ben-Arieh et al., 2004). B-splines are flexible and have minimal support with respect to a given degree, smoothness, and domain partition. Gasparetto and Zanotto (2007) use fifth-order B-splines 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 path-smoothing 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,

for a pre-specified 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 goodness-of-fit 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 goodness-of-fit. 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

 pn∑i=1|yi−f(ti)|2+(1−p)∫ba|f′′(t)|2dt, (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

 n∑i=1(yi−f(ti))2+∫baλ(t)(f′′(t))2dt. (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 data-driven criteria, such as cross-validation (CV), generalized cross-validation (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 V-spline – that is obtained from noisy paired position data and velocity data . In Section 2, the objective function for the V-spline 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 V-spline can be written in terms of modified Hermite spline basis functions. We also introduce a particular adaptive V-spline that seeks to control the impact of irregularly sampled observations and noisy velocity measurements. A cross-validation scheme for estimating the V-spline parameters is given in section 3. Section 4 details the performance of the V-spline on simulated data based on the Blocks, Bumps, HeaviSine and Doppler test signals (Donoho and Johnstone, 1994). Finally, an application of the V-spline to a real 2-dimensional dataset is given in section 5.

## 2 The V-spline

We consider the situation of paired position data and velocity data at a sequence of times satisfying . For , we define the objective function

 J[f]=1nn∑i=1(yi−f(ti))2+γnn∑i=1(vi−f′(ti))2+n∑i=0λi∫ti+1ti(f′′(t))2dt, (4)

where , , , and we have chosen the penalty function to be a piecewise constant function, i.e for ,

 λ(t)=λi,t∈[ti,ti+1). (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 V-spline 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 V-spline, and are the exterior or boundary knots. Since the V-spline 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

 f(s)=(2s3−3s2+1)y0+(s3−2s2+s)v0+(−2s3+3s2)y1+(s3−s2)v1.

This implies that for an arbitrary interval , the relevant cubic spline basis functions are

 h(i)00(t)=⎧⎨⎩2(t−titi+1−ti)3−3(t−titi+1−ti)2+1ti≤t

Consequently, the cubic Hermite spline on an arbitrary interval with two successive points and is expressed as

 f(i)(t)=h(i)00(t)yi+h(i)10(t)vi+h(i)01(t)yi+1+h(i)11(t)vi+1. (10)

For V-splines, a slightly more convenient basis is given by , where , , and for all ,

 N2i+1(t) =h(i)01(t)+h(i+1)00(t), N2i+2(t) =h(i)11(t)+h(i+1)10(t),

and

 N2n−1(t) ={h(n−1)01(t)if t

Any can then be represented in the form

 f(t)=2n∑k=1Nk(t)θk, (11)

where are parameters.

### 2.2 Computing the V-spline

In terms of the basis functions in the previous section, the objective function (4) is given by

 nJ[f](θ,λ,γ)=(y−Bθ)⊤(y−Bθ)+γ(v−Cθ)⊤(v−Cθ)+nθ⊤Ωλθ, (12)

where and are matrices with components

 [B]ij =Nj(ti)={1,j=2i−10,otherwise (13) ij =N′j(ti)={1,j=2i0,otherwise (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

 ^θ=(B⊤B+γC⊤C+nΩλ)−1(B⊤y+γC⊤v), (15)

which can be identified as a generalized ridge regression. The fitted V-spline is then given by

.

The V-spline 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

 ^f=B(B⊤B+γC⊤C+nΩλ)−1(B⊤y+γC⊤v)=:Sλ,γy+γTλ,γv (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 V-spline then, for almost all and , is continuous at the knots if and only if and , for all .

The proof of Theorem 2 is in Appendix C. The V-spline with and is simply the conventional smoothing spline.

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

 f′′(t)=1ti+1−ti{6(ε+i+ε−i)t−titi+1−ti−2(2ε+i+ε−i)}, (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

 4λi(ε+i)2+ε+iε−i+(ε−i)2ΔTi, (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 , over-reliance on noisy measurements of velocity will result in “wiggly” reconstructions. In these two instances – graphically depicted in Figure 0(a) – we would like the V-spline to adapt and to favour straighter reconstructions; this is a deliberate design choice. We can achieve this by choosing

 λi=ηΔTi¯v2i, (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

 (discrepancy in velocityaverage velocity)2 (21)

for all . We call the resulting spline the adaptive V-spline. The spline when , we call the non-adaptive V-spline.

## 3 Parameter selection and cross-validation

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 well-known method for this is cross-validation.

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 leave-one-out cross-validation scheme to estimate and for both the non-adaptive and the adaptive V-splines.

Let be the minimizer of

 1n∑j≠i(yj−f(tj))2+γn∑j≠i(vj−f′(tj))2+n−1∑i=1λi∫ti+1ti(f′′(t))2dt, (22)

and define the cross-validation score

 CV(λ,γ)=1nn∑i=1(yi−^f(−i)(ti,λ,γ))2. (23)

We then choose and that jointly minimize .

The following theorem establishes that we can compute the cross-validation score without knowing the :

###### Theorem 3.

The cross-validation score of a V-spline satisfies

 CV(λ,γ)=1nn∑i=1⎛⎜⎝yi−^f(ti)+γTii1−γVii(vi−^f′(ti))1−Sii−γTii1−γViiUii⎞⎟⎠2 (24)

where is the V-spline smoother calculated from the full data set with smoothing parameter and , and , etc.

The proof of Theorem 3 is in Appendix D.

## 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

 f(ti+1)=f(ti)+g(ti)+g(ti+1)2(ti+1−ti), (25)

where . The observed position and velocity are then found by adding i.i.d. zero-mean Gaussian noise:

 yi=f(ti)+ε(f)i,vi=g(ti)+ε(g)i, (26)

where , ,

is the standard deviation of the positions

, is the standard deviation of the velocities , and SNR is the signal-to-noise ratio, which we take to be 3 or 7.

### 4.1 Regularly sampled time series

We compare the performance of the adaptive V-spline with two wavelet transform reconstructions (Donoho and Johnstone, 1995; Abramovich et al., 1998), a spatially adaptive penalized spline known as the P-spline (Krivobokova et al., 2008; Ruppert et al., 2003), as well as the adaptive V-spline with and the non-adaptive V-spline. It is important to note that only the non-adaptive and adaptive V-splines incorporate velocity information. For the wavelet transform approach, we use both the sure threshold policy and BayesThresh with levels . The V-spline parameters are obtained by minimizing the cross-validation 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 V-spline, we find all methods tend to oversmooth the reconstructions. Wavelet (sure) suffers from an occasional “high-frequency” breakdown and wavelet (BayesThresh) gives spurious fluctuations near the boundary knots. The P-spline gives a smoother reconstruction than wavelet reconstructions, but does not perform as well as the adaptive V-spline with . The value of incorporating velocity information can be seen in the performance of the non-adaptive and adaptive V-splines. However, the non-adaptive V-spline can perform poorly when there are large absolute changes in velocity, e.g. in the Blocks and Bumps trajectories. Overall, the adaptive V-spline performs much better than the other methods and returns near-true reconstructions.

Figure 6 illustrates the trajectory velocities obtained by taking the first derivative of the adaptive V-spline trajectory. It is clear that the V-spline 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 V-spline, we compute the true mean squared error for each of the reconstructions:

 TMSE =1nn∑i=1(f(ti)−^f(ti))2. (27)

The results are shown in Table 1. The adaptive V-spline returns the smallest true mean squared errors for the Blocks and Bumps trajectories. For the Heaviside and Doppler trajectories, only the non-adaptive V-spline shows similar performance. Further analysis also shows that the residuals from the V-splines are not significantly correlated at any lag, as expected.

Table 2 shows the ability of the adaptive V-spline to retrieve the true SNR, calculated by .

### 4.2 Irregularly Sampled Time Series Data

In this section, we show that the proposed V-spline 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.

## 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 V-spline 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 V-spline in d-dimensions

To generalize the V-spline to -dimensions, we consider the situation preceding eq. (4) but where now . Then the function is a -dimensional V-spline if it minimizes:

 J[f]=1nn∑i=1∥yi−f(ti)∥22+γnn∑i=1∥vi−f′(ti)∥22+n∑i=0λi∫ti+1ti∥f′′(t)∥22dt, (28)

where is the Euclidean norm in -dimensions. For each direction , the fitted V-spline has the form , where

 (29)

The parameters and are estimated by minimizing the cross-validation score:

 CV(λ,γ)=1nn∑i=1∥∥ ∥ ∥∥yi−^f(ti)+γTii1−γVii(vi−^f′(ti))1−Sii−γTii1−γViiUii∥∥ ∥ ∥∥22 (30)

In what follows, we allow the non-adaptive and adaptive V-splines 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 non-adaptive V-spline is

 λi=biλd+(1−bi)λu, (31)

and for the adaptive V-spline it is

 λi={biηd+(1−bi)ηu}ΔTi¯v2i. (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 P-spline reconstruction fails due to instances where . Wavelet (sure) overshoots at turning points, as does the non-adaptive V-spline, though more smoothly. The non-adaptive V-spline 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 V-spline with and the adaptive V-spline give acceptable results.

### 5.3 2-Dimensional Trajectory

Figure 10 shows the full 2D adaptive V-spline 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 V-spline is proposed that minimizes an objective function which incorporates both position and velocity information. Given knots, the V-spline 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 V-spline reduces to having degrees of freedom. (See Appendix C for details.) An adaptive version of the V-spline is also introduced that seeks to control the impact of irregularly sampled observations and noisy velocity measurements. In simulation studies, the V-spline 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 cross-validation (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 cross-validation functions when the autocorrelated errors are modelled by an autoregressive moving average. Here we show that there is a generalized cross-validation scheme for the V-spline that is appropriate for correlated observation errors.

When observation errors are correlated (and considering splines in one-dimension for simplicity), the V-spline minimizes

 1n(y−f)⊤W1(y−f)+γn(v−f′)⊤W2(v−f′)+n−1∑i=1λi∫ti+1ti(f′′(t))2dt, (33)

where and are precision matrices, assumed known. The solution is , where

 ^θ=(B⊤W1B+γC⊤W2C+nΩλ)−1(B⊤W1y+γC⊤W2v). (34)

The usual leave-one-out cross-validation 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 V-spline with correlated errors is

 GCV(λ,γ)=(^f−y)⊤W1(^f−y)+2tr(γT)tr(I−γV)(^f−y)⊤W1/21(W1/22)⊤(^f′−v)+(tr(γT)tr(I−γV))2(^f′−v)⊤W2(^f′−v)(tr(I−S−tr(γT)tr(I−γV)U))2. (35)

One current drawback of the V-spline is that it is slower than other spline-based methods, however we have not yet optimized CV parameter estimation, particularly around solving for in (15). Future directions for the V-spline include application to ship tracking (Hintzen et al., 2010) and development of a fast on-line 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 non-zero, upper triangular elements are

 Ω(i)2i−1,2i−1 =∫ti+1tid2h(i)00(t)dt2d2h(i)00(t)dt2dt=12ΔT3i (36) Ω(i)2i−1,2i =∫ti+1tid2h(i)00(t)dt2d2h(i)10(t)dt2dt=6ΔT2i (37) Ω(i)2i−1,2i+1 =∫ti+1tid2h(i)00(t)dt2d2h(i)01(t)dt2dt=−12ΔT3i (38) Ω(i)2i−1,2i+2 =∫ti+1tid2h(i)00(t)dt2d2h(i)11(t)dt2dt=6ΔT2i (39) Ω(i)2i,2i =∫ti+1tid2h(i)10(t)dt2d2h(i)10(t)dt2dt=4ΔTi (40) Ω(i)2i,2i+1 =∫ti+1tid2h(i)10(t)dt2d2h(i)01(t)dt2dt=−6ΔT2i (41) Ω(i)2i,2i+2 =∫ti+1tid2h(i)10(t)dt2d2h(i)11(t)dt2dt=2ΔTi (42) Ω(i)2i+1,2i+1 =∫ti+1tid2h(i)01(t)dt2d2h(i)01(t)dt2dt=12ΔT3i (43) Ω(i)2i+1,2i+2 =∫ti+1tid2h(i)01(t)dt2d2h(i)11(t)dt2dt=−6ΔT2i (44) Ω(i)2i+2,2i+2 =∫ti+1tid2h(i)11(t)dt2d2h(i)11(t)dt2dt=4ΔTi (45)

where and