![\"\"](\"https://user-images.githubusercontent.com/7933764/128525861-3b529b21-e5bb-49ca-8ca6-60229b4ade32.png\")
Key points of the solution:
\nInput Data
\nDid not use Phone_derived.csv. There were some missing data in Phone_derived.csv files. So the author calculated the satellite position and velocity directly from the RINEX navigation file. Baseline position and IMU data are also not used.
\nFactor Graph Optimization
\nFactor graphs are a class of graphical models in which there are variables and factors. The variables represent unknown quantities in the problem, and the factors represent functions on subsets of the variables. Edges in the factor graph are always between factors and variables, and indicate that a particular factor depends on a particular variable.here
The core of the approach was to use a global optimization method based on Factor Graph. Several optimization methods have been used, with good results. Factor graph optimization is a method that can apply a variety of complex nonlinear constraints and simultaneously optimize all state variables（the entire driving trajectory）. There are many outliers in the various constraints（edges in the graph）, but a robust optimization technique eliminates the need to manually set the outlier threshold parameters.
\nFactor Graph Structure
\nThe author tried many different graph structures and finally performed Graph optimization using the following Factor graph.
The graph node𝑋𝑖 presents the state variable of different moments, and the edge connecting two nodes presents the error function 𝑒(⋅); each edge corresponds to a single observation 𝑍𝑖. In the graph, error function 𝑒(⋅) represents probabilistic constraints applied to the state at the specified time-step. Optimization factor graph can be written as follows.
\n![\"\"](\"https://user-images.githubusercontent.com/7933764/128522724-ad708687-61ec-4416-b622-c972b451a8a0.png\")
Here, Ω𝑖 is the information matrix（inverse of covariance matrix）which determines the accuracy of the observation 𝑍𝑖. We defined the following as nodes（estimated states）of the graph.
\n![\"\"](\"https://user-images.githubusercontent.com/7933764/128524196-b6843c78-dfe3-4850-88ed-9fada7becea2.png\")
![\"\"](\"https://user-images.githubusercontent.com/7933764/128524319-f1c6a8fe-0f7e-4417-b702-a9cf815aa466.png\")
where 𝑟 and 𝑟˙ is 3D position/velocity in earth-centered earth-fixed（ECEF）coordinate. 𝑡 and 𝑡˙ represent the receive clock bias and drift in each GNSS signals.
\nHere, 𝑠 in the graph is a switchable constraint, which is a state that takes a variable between 0 and 1. The value of switchable constraint is estimated simultaneously by optimization. On the edge of an outlier, the switchable constraint is automatically optimized to 0 and acts like a weight for the observed value. The optimization problem can be described as follows.
\n![\"\"](\"https://user-images.githubusercontent.com/7933764/128525345-2cb74f7a-5a74-4fa4-8c30-1d73a0828ecf.png\")
where the last term in the above equation is the anchor factor of the switch to prevent the switch state from going to all zeros.
\nPseudorange Factor
\nIn the Pseudorange Factor, the observation is the difference between the pseudorange and the reference station's pseudorange, which removes the all bias error component in GNSS measurements（excluding multipath errors）. Theoretically, the residuals of the single differenced pseudorange have a Gaussian distribution with zero mean value if receiver clock error is compensated. This single-difference pseudorange improves the absolute position accuracy. Using this differential GNSS technique, the aurhor did not need any bias correction for the final position.
Doppler/ADR Factor
\nThe pseudorange rate（Doppler shift）of the smartphone was noisier than the aurhor expected. Doppler factor is used instead only when the ADR factor is not available.
Motion Factor
\nThe motion factor uses the estimated velocity and clock drift to add constraints between neighboring nodes. It simply adds a constraint so that the integral of the velocity and clock drift is equal to the difference between the neighboring states.
Pseudo-Position Factor
\nThe pseudo-position factor was used only for the downtown area, and since the ground truth was traveling along the same path as the test data, the points on the ground truth path closest to the estimated trajectory were extracted and added to the graph as pseudo position constraints with appropriate covariance.
Baseline Improvement
\nThey computed velocities in ENU coordinate system between consecutive timestamps using Acummulated Delta Range, pseudoragnes, sattelite positions and reproduced baseline. After that, they removed outliers and applied smoothing to predictions
\nThen they trained convolutional neural network using velocities from previous step, IMU data and baseline, which also predicted velocities in ENU coordinates. Model achieved mean absolute error around 0.25m/s .
\nAs a last step, they applied Weighted Least Squares to combine relative predictions with data from derived files. For each timestamp it solved the following optimization problem
\nargmin_{x, b_{-k}, …, b_k} \\sum_{i=-k}^k \\sum_{j=1}^{n_i} w_{ij} (|| s_{ij} - \\Delta x_i - x|| + b_i -p_{ij})^2
\nwhere
k - parameter controlling size of the window.
\nx - phone position at timestamp t
\nb_i - phone clock bias at timestamp t + i
\nn_i - number of pseudorange measurements at timestamp t + i
\nw_{ij} - inverse of j-th pseudorange measurement uncertainty multiplied by 1 + 0.25|i|
\ns_{ij} - j-th satellite position at timestamp t + i
\n\\Delta x_{i} - model's prediction of relative position between timestamp t + i and t
\np_{ij} - corrected j-th pseudorange measurement at timestamp t + i
\nParameter k = 10 worked best for me, so the window size was 21. This model had 24 unknowns - 3 for position and 1 for clock bias for each timestamp. It had 6 times more parameters than WLS for single timestamp, but 21 times more measurements. The downside was, that any errors in relative positions affected performance of this solution.
Baseline Improvement in Downtown Area
\nAs all the road segments from downtown areas in the test data were also present in ground truth data, they applied the following algorithm for each timestamp:
select all points from ground truth file closer to the baseline prediction than 30 meters - let's call them candidate points
\nfor each candidate point x_j estimate clock bias: b_j = \\mathrm{median}i ( || s_i - x_j || - p_i )\nfind k points with the lowest values of\n\\sum{i=1}^{n} w_i | || s_i - x_j || - b_i - p_i |
and return their medoid.
\nIt was important to use median instead of mean to compute clock bias and to use the absolute value of residuals instead of squares because it helped with very noisy measurements in the downtown area.
Postprocessing
\nAfter that, they applied the following steps to improve predictions:
Error correction model - it predicted the difference between current predictions and ground truth. It was a simple 1D Conv Neural Network. Features for this model were based on current predictions and imu.
\nAggregating predictions when speed is zero.
\nAggregating predictions from different phones.
Use of sensor data
\nBased on acceleration, gyro, and geomagnetic sensors, I estimated vehicle orientation and acceleration and added acceleration and lateral velocity terms to the cost function of subsequent optimizations. This idea may have had a little effect after the introduction of velocity estimates.
Velocity estimation using doppler shift
\nThe vehicle speed was estimated from the pseudo-range rate by the least-squares method and used for subsequent optimization-based smoothing.
Optimization-based smoothing
\nFirst, the dynamics of the vehicle is expressed by the following state equation.
\n![\"\"](\"https://user-images.githubusercontent.com/309785/128366897-369cc75b-46e9-419b-8f35-d0e884e9ff27.png\")
where 𝜙 and 𝜓 are latitude and longitude, respectively.
\nThen, by vertically arranging the states 𝑋 and inputs, jerks, 𝑈 at each time, the equality constraints on state transitions can be described as follows,
\n$$A X + B U = 0$$
\nIn addition, by determining the observation matrix 𝐶 according to the observation time based on Hermitian interpolation, the vector 𝑌, which is a vertical sequence of latitude and longitude at each observation time, can be written as follows,
\n$$Y = C X$$
\nThe optimization evaluation function 𝐽 is set as the sum of the squares of the jerk 𝑈 and the position estimation error, as follows,
\n$$J = U^T R U + (\\hat{Y} - C X)^T L (\\hat{Y} - C X)$$
\nFinally, the position estimation is formulated as the following quadratic programming problem.
\n![\"\"](\"https://user-images.githubusercontent.com/309785/128366891-08f79ee0-881e-4a6a-b9a5-887270cc83fb.png\")
Based on this method, various information can be integrated by adding velocity error, acceleration error, cost related to lateral velocity, etc. to the cost function.
\nThe important point is to perform the above optimization by using the positions of all smartphones in one driving data at the same time. This method is clearly more accurate than the method of making estimates for each smartphone and calculating their average.
\nPreprocess
\nThe author predicted anomalies (distance from ground truth) and speed from IMU data and lightGBM then replaced anomalies above a threshold with linearly interpolated values.
Kalman smoothing
\nApplied two patterns of Linear Kalman Smoothing:
AccumulatedDeltaRange
\nUsing the GNSS Analysis app from google, the author was able to obtain observations using the AccumulatedDeltaRange for some of the data.
\nImplemented a forward hatch filter and a backward hatch filter and averaged them after applying Kaman smoothing.
\n prev_smooth = lat_lngs[0]\n res = [prev_smooth]\n for raw, delta_adr, delta_t in zip(lat_lngs[1:, :], delta_adrs[1:, :], delta_ts[1:]):\n if not all(np.isnan(delta_adr)):\n smooth = 1 / M * raw + (M - 1) / M * (prev_smooth + delta_adr * delta_t)\n else:\n smooth = raw\n prev_smooth = smooth\n res.append(prev_smooth)\n res = np.array(res)\nWeighted Phone Mean
\nAfter applying the hatch filter, they weighted average latitude and longitude using the weights of the phone optimized by GroupKFold as well as M.
\nSome of the millisSinceGpsEpoch had a little gap depending on the phone, so the authors used linear interpolation to fill it.
Snap2grid
\nIn the downtown area, the authors performed a discrete optimization using linearly interpolated ground truth.
\nTo prevent overfitting, the ground truth in itself was not included in the target to be snapped.
\nThe objective function is as follows, and they used a greedy search to find the optimal path.
cost = distance2cur + distance2prev * alpha
\nStay point mean
\nUsing the predicted value of the speed of LightGBM, they took the average for points below the threshold.
XGBoost stacked ensemble
\nCorrected the PrMs using other device data, gives about 10-40 cm improved baseline. Used the trick: the amount of correction of PrM was used as weights in WLS algorithm
Computing Velocity
\nTranslated WlsPvt.m from gps-measurement google tool in python using PseudorangeRateMetersPerSecond and the results was 0.08Mps mean average accurate. When the translation algorithm failed the author used the baseline speedMps
ADR relative position
\nUsed derived files public version Android GPS tools.
Remove outliers
\nUsed some outlier removal methods from public notebooks but also included: If there were two or more consecutive outliers remove and interpolate it.
Mixing signals from multiple devices
\nIt turns out then median averaging is better than mean, also variance among all epochs was pretty similar so the author windowed signal (10-30 epochs adjusted by speed Category and region).
Mean By Stop
\nUsed velocity Mps with threshold adjusted about 0.5-1.
kNNHeight
\nThe author mapped the latitude and longitude from the ground truth to the correct altitude using KNN.
Outlier Detection
\nThe author estimated what point to consider as an outlier using the relative coordinates of the surrounding about 50 seconds as a feature.
Postprocesses
\nRelpos Outlier Detection
\nDetects relative position outliers as well as absolute coordinates to improve accuracy.
KalmanFilter
\nKalman filter considers relative and absolute positions.
SateliteMean
\nAveraging phone with 1/(psuedorangesigma)**2 as weights
Snap to Grid
\nIn SJC, the authors snap the prediction to the nearest neighbor of ground_truth.
Satellite Selection
\nExcluding satellites that were a source of error, from the least-squares calculation.
\nUse the elevation angle for filter condition: Signals from satellites with low elevation angles are excluded because they are strongly affected by various errors.
Carrier Smoothing
\nPseudorange smoothing with Accumulated Delta Range (ADR).
Relative Position Estimation
\nIMU Data
\nRelative positions were missing in some places the author combined IMU sensor data and author used lightGBM with lag and rolling features of the IMU and vehicle velocity.
Excluding high-speed points
\nExclude points that have a very large distance from the previous and next point.
** Post-processing using Kalman Smoothing**
\nWas done in a public notebook, the author used it also.
Area Grouping
\nThe author usewd KNN and implemented grouping based on the degree of path matching with the train where each collection was divided into five groups, and the hyperparameters and order of processing were adjusted for each group.
Outlier Correction
\nnotebook
Kalman Smoothing
\nApplied linear interpolation to keep epoch width constant: notebook
Phone Mean
\nnotebook 1
\nnotebook 2
Phones Removal
\nnotebook
Snap to Grid
\nThey applied to snap to grid only downtown area and the difficult areas.
Position estimation by imu data
\nRelative position correction was performed using IMU data for the only downtown area using LightGBM with GroupKFold(group=\"phone\") CV and the features(shift range is -30~30) & aggregate features(mean, std, max, min, median, skew, kart).
Key points of the solution:
\nInput Data
\nDid not use Phone_derived.csv. There were some missing data in Phone_derived.csv files. So the author calculated the satellite position and velocity directly from the RINEX navigation file. Baseline position and IMU data are also not used.
\nFactor Graph Optimization
\nFactor graphs are a class of graphical models in which there are variables and factors. The variables represent unknown quantities in the problem, and the factors represent functions on subsets of the variables. Edges in the factor graph are always between factors and variables, and indicate that a particular factor depends on a particular variable.here
The core of the approach was to use a global optimization method based on Factor Graph. Several optimization methods have been used, with good results. Factor graph optimization is a method that can apply a variety of complex nonlinear constraints and simultaneously optimize all state variables（the entire driving trajectory）. There are many outliers in the various constraints（edges in the graph）, but a robust optimization technique eliminates the need to manually set the outlier threshold parameters.
\nFactor Graph Structure
\nThe author tried many different graph structures and finally performed Graph optimization using the following Factor graph.
The graph node𝑋𝑖 presents the state variable of different moments, and the edge connecting two nodes presents the error function 𝑒(⋅); each edge corresponds to a single observation 𝑍𝑖. In the graph, error function 𝑒(⋅) represents probabilistic constraints applied to the state at the specified time-step. Optimization factor graph can be written as follows.
\n
Here, Ω𝑖 is the information matrix（inverse of covariance matrix）which determines the accuracy of the observation 𝑍𝑖. We defined the following as nodes（estimated states）of the graph.
\n
where 𝑟 and 𝑟˙ is 3D position/velocity in earth-centered earth-fixed（ECEF）coordinate. 𝑡 and 𝑡˙ represent the receive clock bias and drift in each GNSS signals.
\nHere, 𝑠 in the graph is a switchable constraint, which is a state that takes a variable between 0 and 1. The value of switchable constraint is estimated simultaneously by optimization. On the edge of an outlier, the switchable constraint is automatically optimized to 0 and acts like a weight for the observed value. The optimization problem can be described as follows.
\n
where the last term in the above equation is the anchor factor of the switch to prevent the switch state from going to all zeros.
\nPseudorange Factor
\nIn the Pseudorange Factor, the observation is the difference between the pseudorange and the reference station's pseudorange, which removes the all bias error component in GNSS measurements（excluding multipath errors）. Theoretically, the residuals of the single differenced pseudorange have a Gaussian distribution with zero mean value if receiver clock error is compensated. This single-difference pseudorange improves the absolute position accuracy. Using this differential GNSS technique, the aurhor did not need any bias correction for the final position.
Doppler/ADR Factor
\nThe pseudorange rate（Doppler shift）of the smartphone was noisier than the aurhor expected. Doppler factor is used instead only when the ADR factor is not available.
Motion Factor
\nThe motion factor uses the estimated velocity and clock drift to add constraints between neighboring nodes. It simply adds a constraint so that the integral of the velocity and clock drift is equal to the difference between the neighboring states.
Pseudo-Position Factor
\nThe pseudo-position factor was used only for the downtown area, and since the ground truth was traveling along the same path as the test data, the points on the ground truth path closest to the estimated trajectory were extracted and added to the graph as pseudo position constraints with appropriate covariance.
Baseline Improvement
\nThey computed velocities in ENU coordinate system between consecutive timestamps using Acummulated Delta Range, pseudoragnes, sattelite positions and reproduced baseline. After that, they removed outliers and applied smoothing to predictions
\nThen they trained convolutional neural network using velocities from previous step, IMU data and baseline, which also predicted velocities in ENU coordinates. Model achieved mean absolute error around 0.25m/s .
\nAs a last step, they applied Weighted Least Squares to combine relative predictions with data from derived files. For each timestamp it solved the following optimization problem
\nargmin_{x, b_{-k}, …, b_k} \\sum_{i=-k}^k \\sum_{j=1}^{n_i} w_{ij} (|| s_{ij} - \\Delta x_i - x|| + b_i -p_{ij})^2
\nwhere
k - parameter controlling size of the window.
\nx - phone position at timestamp t
\nb_i - phone clock bias at timestamp t + i
\nn_i - number of pseudorange measurements at timestamp t + i
\nw_{ij} - inverse of j-th pseudorange measurement uncertainty multiplied by 1 + 0.25|i|
\ns_{ij} - j-th satellite position at timestamp t + i
\n\\Delta x_{i} - model's prediction of relative position between timestamp t + i and t
\np_{ij} - corrected j-th pseudorange measurement at timestamp t + i
\nParameter k = 10 worked best for me, so the window size was 21. This model had 24 unknowns - 3 for position and 1 for clock bias for each timestamp. It had 6 times more parameters than WLS for single timestamp, but 21 times more measurements. The downside was, that any errors in relative positions affected performance of this solution.
Baseline Improvement in Downtown Area
\nAs all the road segments from downtown areas in the test data were also present in ground truth data, they applied the following algorithm for each timestamp:
select all points from ground truth file closer to the baseline prediction than 30 meters - let's call them candidate points
\nfor each candidate point x_j estimate clock bias: b_j = \\mathrm{median}i ( || s_i - x_j || - p_i )\nfind k points with the lowest values of\n\\sum{i=1}^{n} w_i | || s_i - x_j || - b_i - p_i |
and return their medoid.
\nIt was important to use median instead of mean to compute clock bias and to use the absolute value of residuals instead of squares because it helped with very noisy measurements in the downtown area.
Postprocessing
\nAfter that, they applied the following steps to improve predictions:
Error correction model - it predicted the difference between current predictions and ground truth. It was a simple 1D Conv Neural Network. Features for this model were based on current predictions and imu.
\nAggregating predictions when speed is zero.
\nAggregating predictions from different phones.
Use of sensor data
\nBased on acceleration, gyro, and geomagnetic sensors, I estimated vehicle orientation and acceleration and added acceleration and lateral velocity terms to the cost function of subsequent optimizations. This idea may have had a little effect after the introduction of velocity estimates.
Velocity estimation using doppler shift
\nThe vehicle speed was estimated from the pseudo-range rate by the least-squares method and used for subsequent optimization-based smoothing.
Optimization-based smoothing
\nFirst, the dynamics of the vehicle is expressed by the following state equation.
\n
where 𝜙 and 𝜓 are latitude and longitude, respectively.
\nThen, by vertically arranging the states 𝑋 and inputs, jerks, 𝑈 at each time, the equality constraints on state transitions can be described as follows,
\n$$A X + B U = 0$$
\nIn addition, by determining the observation matrix 𝐶 according to the observation time based on Hermitian interpolation, the vector 𝑌, which is a vertical sequence of latitude and longitude at each observation time, can be written as follows,
\n$$Y = C X$$
\nThe optimization evaluation function 𝐽 is set as the sum of the squares of the jerk 𝑈 and the position estimation error, as follows,
\n$$J = U^T R U + (\\hat{Y} - C X)^T L (\\hat{Y} - C X)$$
\nFinally, the position estimation is formulated as the following quadratic programming problem.
\n
Based on this method, various information can be integrated by adding velocity error, acceleration error, cost related to lateral velocity, etc. to the cost function.
\nThe important point is to perform the above optimization by using the positions of all smartphones in one driving data at the same time. This method is clearly more accurate than the method of making estimates for each smartphone and calculating their average.
\nPreprocess
\nThe author predicted anomalies (distance from ground truth) and speed from IMU data and lightGBM then replaced anomalies above a threshold with linearly interpolated values.
Kalman smoothing
\nApplied two patterns of Linear Kalman Smoothing:
AccumulatedDeltaRange
\nUsing the GNSS Analysis app from google, the author was able to obtain observations using the AccumulatedDeltaRange for some of the data.
\nImplemented a forward hatch filter and a backward hatch filter and averaged them after applying Kaman smoothing.
\n prev_smooth = lat_lngs[0]\n res = [prev_smooth]\n for raw, delta_adr, delta_t in zip(lat_lngs[1:, :], delta_adrs[1:, :], delta_ts[1:]):\n if not all(np.isnan(delta_adr)):\n smooth = 1 / M * raw + (M - 1) / M * (prev_smooth + delta_adr * delta_t)\n else:\n smooth = raw\n prev_smooth = smooth\n res.append(prev_smooth)\n res = np.array(res)\nWeighted Phone Mean
\nAfter applying the hatch filter, they weighted average latitude and longitude using the weights of the phone optimized by GroupKFold as well as M.
\nSome of the millisSinceGpsEpoch had a little gap depending on the phone, so the authors used linear interpolation to fill it.
Snap2grid
\nIn the downtown area, the authors performed a discrete optimization using linearly interpolated ground truth.
\nTo prevent overfitting, the ground truth in itself was not included in the target to be snapped.
\nThe objective function is as follows, and they used a greedy search to find the optimal path.
cost = distance2cur + distance2prev * alpha
\nStay point mean
\nUsing the predicted value of the speed of LightGBM, they took the average for points below the threshold.
XGBoost stacked ensemble
\nCorrected the PrMs using other device data, gives about 10-40 cm improved baseline. Used the trick: the amount of correction of PrM was used as weights in WLS algorithm
Computing Velocity
\nTranslated WlsPvt.m from gps-measurement google tool in python using PseudorangeRateMetersPerSecond and the results was 0.08Mps mean average accurate. When the translation algorithm failed the author used the baseline speedMps
ADR relative position
\nUsed derived files public version Android GPS tools.
Remove outliers
\nUsed some outlier removal methods from public notebooks but also included: If there were two or more consecutive outliers remove and interpolate it.
Mixing signals from multiple devices
\nIt turns out then median averaging is better than mean, also variance among all epochs was pretty similar so the author windowed signal (10-30 epochs adjusted by speed Category and region).
Mean By Stop
\nUsed velocity Mps with threshold adjusted about 0.5-1.
kNNHeight
\nThe author mapped the latitude and longitude from the ground truth to the correct altitude using KNN.
Outlier Detection
\nThe author estimated what point to consider as an outlier using the relative coordinates of the surrounding about 50 seconds as a feature.
Postprocesses
\nRelpos Outlier Detection
\nDetects relative position outliers as well as absolute coordinates to improve accuracy.
KalmanFilter
\nKalman filter considers relative and absolute positions.
SateliteMean
\nAveraging phone with 1/(psuedorangesigma)**2 as weights
Snap to Grid
\nIn SJC, the authors snap the prediction to the nearest neighbor of ground_truth.
Satellite Selection
\nExcluding satellites that were a source of error, from the least-squares calculation.
\nUse the elevation angle for filter condition: Signals from satellites with low elevation angles are excluded because they are strongly affected by various errors.
Carrier Smoothing
\nPseudorange smoothing with Accumulated Delta Range (ADR).
Relative Position Estimation
\nIMU Data
\nRelative positions were missing in some places the author combined IMU sensor data and author used lightGBM with lag and rolling features of the IMU and vehicle velocity.
Excluding high-speed points
\nExclude points that have a very large distance from the previous and next point.
** Post-processing using Kalman Smoothing**
\nWas done in a public notebook, the author used it also.
Area Grouping
\nThe author usewd KNN and implemented grouping based on the degree of path matching with the train where each collection was divided into five groups, and the hyperparameters and order of processing were adjusted for each group.
Outlier Correction
\nnotebook
Kalman Smoothing
\nApplied linear interpolation to keep epoch width constant: notebook
Phone Mean
\nnotebook 1
\nnotebook 2
Phones Removal
\nnotebook
Snap to Grid
\nThey applied to snap to grid only downtown area and the difficult areas.
Position estimation by imu data
\nRelative position correction was performed using IMU data for the only downtown area using LightGBM with GroupKFold(group=\"phone\") CV and the features(shift range is -30~30) & aggregate features(mean, std, max, min, median, skew, kart).