ODOMETRY AND LOW-COST SENSOR FUSION IN TMM DATASET

The aim of this study is to identify the most powerful motion model and filtering technique to represent an urban terrestrial mobile mapping (TMM) survey and ultimately to obtain the best representation of the car trajectory. The authors want to test how far a motion model and a more or less refined filtering technique could bring benefits in the determination of the car trajectory. To achieve the necessary data for the application of the motion models and the filtering techniques described in the article, the authors realized a TMM survey in the urban centre of Turin by equipping a vehicle with various instruments: a low-cost action-cam also able to record the GPS trace of the vehicle even in the presence of obstructions, an inertial measurement system and an odometer. The results of analysis show in the article indicate that the Unscented Kalman Filter (UKF) technique provides good results in the determination of the vehicle trajectory, especially if the motion model considers more states (such as the positions, the tangential velocity, the angular velocity, the heading, the acceleration). The authors also compared the results obtained with a motion model characterized by four, five and six states. A natural corollary to this work would be the introduction to the UKF of the photogrammetric information obtained by the same camera placed on board the vehicle. These data would permit to establish how photogrammetric measurements can improve the quality of TMM solutions, especially in the absence of GPS signals (like urban canyons).


INTRODUCTION
The majority of vehicles today have GPS instruments, digital maps or speedometers on board to measure their velocity.These instruments are characterized by low precision and are not able to integrate pieces of information from the different sources.The integration of the information obtained by these instruments with their accuracy and other visual data about the motion model could define a vehicle's future position more accurately (Lytrivis et al., 2010).The application fields of these studies are many: e.g.driver assistance systems, start-stop systems and adaptive cruise control (Schubert et al., 2008).The Kalman filter, or its more modern derivatives, is the most commonly employed method to combine information about position, velocity, function of position and velocity and filtering techniques (Hartikainen and Särkkä, 2011).
The data filtering may take different form: if the stochastic model is represented by a Gaussian and the state equations between two successive epochs may be considered linear by leaving out the second order effects, a more suitable method is the Extended Kalman Filter (EKF).Alternatively, if these conditions are not verified, for example without the introduction of big approximations, it is possible to use other techniques such as the Unscented Kalman Filter (UKF).
However it is essential to adopt a motion model (Li and Jilkov, 2003) that can include in the state parameters and in the observation equations the measurements effectively realized during the survey.

CASE STUDY
In order to obtain an adequate dataset for the application of motion models and different filtering techniques, the authors realized a TMM survey in the urban centre of Turin.Therefore the path is characterized by rectilinear segments spaced out by curves of up to 90 degrees, changes of velocity, some departures and stops and urban canyons, as shown in Figure 1.With the aim of placing the instrumentation on the vehicle, the authors equipped its baggage rack with a structure reminiscent of a "cross".This is the name used (also in this article) to refer to this particular object.
The cross is able to host the whole instrumentation in known and unmovable positions.
In order to know the position of each instrument with a millimetric accuracy in respect of origin (O) and a direction (OA in Figure 2), the measurements were performed and adjusted depending on a tridimensional schema using a commercial software.The authors equipped the vehicle with various instruments: a low-cost action-cam (position V1 in Figure 2, in orange in Figure 3) able to record the trace of the vehicle even in presence of obstructions, since it has an internal highly sensitive GPS; an inertial measurement system (INS, position E in Figure 2, in blue in Figure 3), which is useful for obtaining the reference trajectory especially in the absence of GPS signals; and a DMI (distance measurement indicator, located on the vehicle as shown in Figure 4).The Garmin Virb Elite Action-Cam 1080p HD used for the survey is able to acquire HD video (with 1920*1080 pixel resolution) and high velocity frames (25 fps), as well as to provide the GPS locations of the frames, while maintaining a low cost.In fact, even if the camera crosses into an obstructed area, its high sensibility GPS is able to determine its position: for this reason it is a very useful instrument, especially in urban canyons or near high buildings or trees.Moreover, the presence of the integrated GPS guarantees easy synchronization of the time scale of the camera with the UTC time.
Another instrument on board is the INS SBG Ekinox-D, a navigation grade sensor characterized by an internal GNSS receiver with two antennas (in yellow in Figure 3).The use of this INS does not want to contradict the idea of analyzing an unfavourable scenario to understand how the use of different instruments can really improve the positioning solution, but it is necessary to define the "true trajectory" and it also constitutes one of the ways to interface with the DMI.
The authors decided to use the less precise GPS data (in fact are code points positions) acquired by the action-cam with the aim of testing how far a motion model and a more or less refined filtering technique could bring benefits in the determination of the vehicle trajectory and the position of the perspective centres of the camera.
Lastly, a DMI (Pegasem WSS) is also located on the vehicle, which permits to record vehicle speed information (and obviously the epochs of acquisition) and measurements of distance.The authors used these speeds information, and for this reason the instrument could be defined speedometer, but for brevity they use the term DMI.
The DMI permits the introduction of velocities into the analysis and also caters for the typical deficiencies of the motion models: e.g.failure to consider stops at traffic lights or zebra crossings, which are typically present in an urban road.
The DMI permits to "adjust" these models, realized for a continuous trend of the vehicle also to a discontinuous trend.
Obviously both camera and DMI were calibrated (Angelats and Colomina, 2014) before the survey, the first using the calibration tool of the commercial software Matlab® (Heikkilä and Silvén, 1997;Zhang, 1999), the second in the Laboratory of Topography of Politecnico di Torino, with the aim to determine the so-called "odometer gain" (in others words: the scale value of DMI). Figure 5 shows the instruments used for this latest calibration.The instruments used for the calibration of the DMI were a lathe, a speedometer and a chronometer, and obviously the value of measure of the circumference of the vehicle (shown in Figure 5).The instruments were used to define the correspondence between the mean value of velocity recorded by the DMI, when the odometer gain is in the amount of one pulse/m and the number of turns per second performed by the lathe.
The authors considered different measurements respectively corresponding more or less to three, five and seven turns per second of the lathe, and for each one they defined the mean value of velocity recorded by the DMI.This value was then divided by the specific number of turns per second corresponding to the specific measurement session.Finally, the results obtained by each measurement session were averaged (Table 1), for the purpose of defining the most reliable value useful for the determination of the scale of the DMI.The final mean value of velocity related to turns per second, adopted for the determination of the scale value of DMI, was 256.85.This value, divided by the circumference of the wheel of the vehicle, permitted the determination of the scale value of the DMI (equation 1), used in the successive surveys:

135.90
(1) The DMI data are recorded at a rate of about one second, but their acquisition times are not synchronized with the GPS camera positions: for this reason, resampling of the DMI data was necessary.The authors opted for a spline interpolation, finer than to the linear one.The residuals obtained by the linear interpolation with respect to the spline were at most 8 cm/s.

MOTION MODELS
It should be pointed out that each motion model is characterized by hypotheses and simplified assumptions, leaving out some errors, deliberately or otherwise.These un-modelled or causal effects relapse into the stochastic part of modern filters.We must remember that a simplified model, i.e. an "unpolished" model, even though it apparently works well, will be able to balance out only in a small part the simplified assumptions introduced in the motion errors.
Different motion models (Yuan et al., 2014) were studied for the analysis of the case study.Linear models are the least complex and assume the vehicle velocity or its acceleration as constant: for this reason they are respectively called Constant Velocity or Constant Acceleration models (Schubert et al., 2008).Obviously their major advantage is the linearity of the state transition equation, but these models suppose a "straight" motion without changes of direction or curves, which are always present in an urban path.
Figure 6.Scale of complexity of the motion models Therefore, more complex curvilinear models were adopted to describe the case study in a realistic way.These latest models could be classified using their unknown or constant parameters: the simpler is the so-called Constant Turn Rate and Velocity Model (CTRV), very useful in the description of the trajectories of aeroplanes; the second is the Constant Turn Rate and Acceleration (CTRA) (Altendorfer, 2009).
Besides yielding the best results in the definition of the vehicle trajectory and a realistic description of the motion, the CTRA method is also the only curvilinear method able to consider change of direction, the presence of rectilinear and curvilinear paths, and therefore passages in clothoids.
Figure 7 shows the variables used in the different motion models which are described in the next paragraphs., , , where x, y = positions of the vehicle v x , v y = constant velocities in x and y directions This is a linear model and the state transition equation is shown in equation 3: where T = time interval between two successive epochs x(t), y(t) = positions in the t-time != states at i-time (t + T).

Motion model with 5 states: CTRV
In this model the states are five: two positions, the tangential velocity, the direction angle θ and the rotation angle ω.
, , %, , & Here both the tangential velocity and the rotation angle are constants, in fact we have:

Motion model with 6 states: CTRA
This model "come from" the CTRV and includes also the acceleration of the vehicle.
As the acceleration, also the rotation angle is constant: & !& .

KF and EKF
The hypotheses of the Kalman filter (hereafter called "KF" for brevity; Kalman, 1960;Einicke, 2012) are: the system evolves linearly in the time; the measure equations are linear; both the state equations than the measure equations are normally distributed and uncorrelated.
In a lot of situations it is possible to linearize the measure equations and, if these are sufficiently precise, it is possible to consider these as normally distributed without falling into error and obtaining imprecise, less reasonable or divergent results.Instead, less frequently the state equations are linear and indeed usually are differential equations.However, if the time intervals between the measures are evaluated as "little" (a more detailed explication would request more space), it is usually possible to write non-linear state equations and then linearize these equations using a matrix of partial derivatives.
The initial state parameters, being non-linear equationsgenerally positions, velocities, accelerations, angular velocities, etc.-are obtained by the motion and measure equations of the previous epoch.
In this situation the filtering technique isn't known as Kalman Filter (KF), but is called Extended Kalman Filter (EKF).
The clearest difference is represented by the computation of the update of the state parameters, not by the classical computation of the parameters.
After the linearization of the state equations, the structure of the EKF follows the typical rules of the more traditional KF.
The KF is a recursive filter characterized by two steps: the filtering and the smoothing.The first step is divided into two parts: a time-update of the state equations (for this reason this passage is known as predictive estimation, or "prediction") and a following time-update of the measure equations (a process known as "correction").
At the successive epoch, in the EKF, the estimation just filtered is updated and then becomes the new starting point for the prediction of the successive instant.
In conclusion, the EKF permits the update of both the solution of the problem and the variance matrix.

UKF
The principal limitations that characterize both the KF and the EKF are the necessary linearization of the non-linear equations (Wan and Van der Merwe, 2000; Van der Merwe and Wan, 2001) and the hypothesis of the normality of the measures.Thanks to the use of the Unscented Kalman Filter, it is possible to remove the first hypothesis and maintain the second, though with minor consequences (Tsogas et al., 2011).
The data filtering related to dynamic non-linear systems has in the recent years become a fascinating study area, considering that a lot of approaches are developed with the aim of solving the problem.The Unscented Kalman Filter is based on unscented transformation, that is a mechanism for the propagation of the mean and the covariance, using non-linear transformations (Xiong et al., 2006;Terejanu et al., 2007).The state vector is not unique, but instead constitutes a reduced number of state vectors (points in a space of 2L+1 dimensions, where L is the dimension of the variance-covariance matrix of the states).These state vectors (called sigma points; Van der Merwe, 2004;Zoeter et al., 2004) are accurately chosen to approximate both the 2L+1 dimensional mean, and the variance-covariance matrix of the variable computed a posteriori, which is still Gaussian, with a second order accuracy (the mean and the variance).
The improvement in respect of the EKF is clear if we consider that EKF permits only the obtainment of the first order of accuracy (we only know the mean value: the variancecovariance matrix is only obtained for a more or less linear rule and it refers to the only average value).Furthermore with the UKF it is not necessary to build the Jacobean matrix, and so in this respect the computational commitment is reduced.
The application fields for this method are various, for example, the fusion of the output data of different types of sensor; the determination of the position; or the training of a neural network (Haykin, 2001).Therefore the literature suggests that this method is able to obtain better results than the EKF (Gustafsson and Hendeby, 2012), so the authors decided to apply the UKF to their case study.

RESULTS
The following tables and figures show the results obtainable using the two types of UKF method: one is the UKF notaugmented, described in the previous paragraphs, and the second method is the UKF augmented, that differs from the previous method because of its use of a greater number of states.This greater number of states is due to the fact that the state and measure equations are considered as non-linear with respect to the noise, which generally describes a real situation (Rutten, 2013).The number of states is not casual, it is justified by the introduction of the analysis of the noise that troubles the states (Guzzi, 2012).The biggest advantage of the augmented method is the possibility of considering the influence of the noise of the measures in the computation, but its biggest disadvantage is represented by a computational commitment and by the possibility of not getting a positive weight matrix.The latter situation causes an inapplicability of the Cholesky factorization (a fundamental step in the UKF computational process) and the consequent end of the computation.In order to solve these problems, some authors proposed different alternative solutions to the factorization (Rutten, 2013).
In relation to both the procedures for implementation of the UKF method, whatever the number of states under examination, it is important define two parameters: the coefficient "α" (equation 10), which takes into account the spread of the sigma points (Turner and Rasmussen, 2010) and is generally inversely proportional to the considered number of states, and "β", which considers the a priori knowledge of the sigma points distribution (for Gaussian distribution generally is assumed equal to two).
α9 0,1] (10) In fact, for the case study, it was possible to observe that the assumption of values of α about its lower limit usually determined the impossibility of applying Cholesky factorization, and so the crash of the method.
Another element to take into account as input is the variancecovariance matrix of the process: the adjustment of this matrix depends on the case study, in particular on the number of states used.As an example the equation 11 and equation 12, show the variance-covariance matrix used for the motion model characterized by four states: : ; where where q = standard deviation of the process T = time interval between successive epochs.
In Obviously to compare these data, the authors applied the specific level-arm between the GPS+DMI data and the reference trajectory, using the heading extracted from the velocity vector.
In the related graphs, it is also possible to observe the trajectories: the reference in black; the trajectories obtained by the UKF filtering and smoothing applied to the GPS and DMI data in blue and green respectively, and the GPS data of the camera in red.
All the results shown in the next tables and graphs were obtained thanks to a script realized in Matlab® by the authors.
In Table 2 are shown the results related to the case study with 4 states ( , , , ).

N° states UKF Augmented
1.80 7.99 4.16 Table 2. Delta mean and delta standard deviation between INS and UKF augmented or not-augmented applied to the motion model with 4 states The Figure 8 show the trajectory obtainable by the UKF, using four states: In Table 3 are shown the same results of Table 1, but related to the motion model with 5 states ( , , %, , &).
N° states UKF Augmented UKF not-augmented 2.16 5.29 1.63 Table 3. Delta mean and delta standard deviation between INS and UKF augmented or not-augmented applied to the motion model with 5 states Figure 9 show the trajectory obtainable by the UKF methods considering the motion model characterized by 5 states: Figure 10 shows also the comparison between the tangential velocity of DMI (in red), and the values obtained with UKF filtering (blue) or smoothing (green) augmented or notaugmented.With the aim of showing the results in a clearer scenario and to underline that a bigger number of states guarantees the best results in the definition of the trajectory, the authors show in Figure 13 the trajectories resulting from the UKF notaugmented method (with four, five and six states) in the urban canyon of Turin.They prefer to report the not-augmented results in a satellite view, because the differences in the various motion models in the definition of the final smoothed trajectory are clearer in this type of analysis than in the UKF augmented.In fact, the "worst" trajectory corresponds to the least number of states: it is the motion model with four states represented in red.
Here we can see that the trajectory is sinuous (that may be because of good UKF smoothing), but this trajectory is not always correctly located on the road: after the curves especially the positions are placed on trees or on the buildings.An improvement is determined by the introduction of another state: the trajectory in orange corresponds to the motion model with five states, which is a little bit more realistic than the previous because it also follows the road after a curve.However, it is the last analysis with the motion model based on six states that gives the best results: even if the final trajectory seems to be quite similar to the previous case, a visual analysis revels that after the curve this one is located in a more correct position than the other.

CONCLUSION
The results shown in the previous tables suggest that the UKF methods (especially the not-augmented), associated with a reduced number of states, are not able to consider all of the physics entities that have a role in the specific case study, and determines "dirty" results in the delta planimetric mean.In contrast, if the number of states considered is bigger, we can see an improvement (that is, a reduction) of the same value of the delta planimetric mean.At the same time, the values of the delta planimetric SQM are almost unvaried, even if the number of states is augmented.
Moving from the most restricted number of states towards the biggest, we can see an improvement in the delta planimetric mean value for the UKF augmented, but the SQM does not change substantially.
Observing in a more detailed manner the case study characterized by five states, it possible to see that the augmented method apparently determines more noisy results, since it is characterized by a SQM 50 cm bigger than the equivalent not-augmented method.However it is important also to consider the delta planimetric mean, which is equal to 4.56 m for the augmented method and 5.29 m for the other: there is a worsening of about 70 cm between the two UKF filtering techniques.
With respect to the results shown in the previous paragraph, it is possible to conclude that if we consider a reduced number of states, the not considered entities in the analysis try to influence the results negatively, in particular "dirtying" the mean value; otherwise, if the number of states augments, it is possible to see a general improvement (a reduction) of the value of the planimetric mean.It can therefore be concluded that the UKF augmented achieves better results than the not-augmented.
Following the integration of the GPS and DMI data, the available dataset permits another type of integration: GPS data with photogrammetric information (De Agostino et al., 2011).
Both the GPS data and the frames are obtained from the lowcost camera used for the survey.The aim of this different type of integration is to see if the photogrammetric information is useful to improve the solution of positioning when there are not other technologies available.
Obviously it was possible using a software able to simulate the situation represented by the case study (Taglioretti and Manzino, 2014), but in order to achieve this goal, the authors initially use a commercial software (frequently used in the field of photogrammetric research), able automatically to discover a lot of tie points (TP) between the frames and to compute the orientation between successive frames.The latest goal is the introduction of attitude parameters between the state parameters for the estimation of the camera coordinates with the respective errors of measure.
The authors decided to illustrate the results of a commercial software to permit an initial comparison with the values obtained using the UKF methods.This is because the authors are currently in a phase of implementation of a proprietary software: this software will be able to integrate GPS data with photogrammetric information, and will be the subject of future work.
It is important to underline that these results are obtained using a sort of "loosely coupled" integration.The authors introduced in the commercial software the real values of position and accuracy of the camera obtained by the UKF methods, but they not identified any significant changes in respect of the introduction of the accuracy of the camera position.For this reason they believe more interesting understand how the errors change in respect of the reference trajectory modifying this parameter.They chose a value of camera accuracy equal to a low 1 meter, up to 3 meters and they decided to study the situation characterized by five states, because it represents a sort of "compromise" in the number of state at disposal.The aim was to understand how photogrammetric measurements can improve positioning, in particular depending on the specific quality of the data available (and consequently depending on the camera quality) (Taglioretti et al., 2015).
The results obtained with the commercial software and compared with INS trajectory, are shown in the next Table 5.

INS-CASE
[m] 1 2.44 1.42 2 2.57 2.11 3 5.63 3.20 Table 5.The results obtained by the commercial software These values suggest that a loosely coupled integration permits an improvement of the mean value, but it is not sufficient to guarantee a real improvement in the positioning solution in respect of results obtained by the UKF.In fact the results shown in Table 5 obtained with the camera accuracy equal to two, show that there is an improvement in the mean value, but the SQM is quite similar to the previous case shown in Table 3.For this reason, a more complete tightly-coupled integration is necessary (Cazzaniga et al., 2007) that permits the obtainment of better results, and which the authors will follow when developing their software.

Figure 1 .
Figure 1.In red: vehicle's trajectory, in green: portion of trajectory analyzed

Figure 2 .
Figure 2. Schema of the positions of the various instruments on the cross

Figure 3 .
Figure 3. Instruments on board

Figure 5 .
Figure 5. On the left: the DMI during calibration; on the right: the measure of the circumference of the wheel of the vehicle

Figure 7 .
Figure 7. Variables in use in the motion models 3.1 Motion model with 4 states This model considers the planimetric positions of the vehicle and its velocities in these directions.
where x, y = positions of the vehicle v = tangential velocity % = direction angle & = rotation angle The equation 5 shows the state transition equation: interval between successive epochs.
the next tables, the authors show the means and the respective standard deviations (SQM) obtained by comparing the 2D positions of the reference INS (in black in the next figures) with the planimetric trajectories achieved by the UKF methods (augmented and not-augmented) applied to the GPS+DMI data.In these tables are shown the results obtained by the comparison between the reference and the smoothed values.

Figure 8 .
Figure 8.On the left: UKF augmented trajectory with 4 states; on the right: UKF not-augmented trajectories with 4 states

Figure 9 .
Figure 9. On the left: UKF augmented trajectory with 5 states; on the right: UKF not-augmented trajectories with 5 states The trajectory shown in Figure 9 are quite the same.

Figure 10 .
Figure 10.On the left the UKF augmented and on the right the not-augmented tangential velocities with 5 states