System for fusing two or more versions of map data based on spatial Kalman filtering

1 . A system for fusing two or more versions of map data together to create fused map data, the system comprising:

one or more central computers storing the two or more versions of map data and ground truth map data where each version of the map data represents a predefined geofenced area, wherein the one or more central computers execute instructions to:

receive road network data representing a road network for the predefined geofenced area, a first set of map data points, and a second set of map data points, wherein the road network data includes a discrete random curve that represents lane markings, and wherein the discrete random curve includes a plurality of state vectors that are each defined by a respective location and tangent angle;

determine a signed distance, wherein the signed distance includes a first perpendicular distance measured from a respective first map data point to the discrete random curve and a second perpendicular distance measured from a respective second map data point to the discrete random curve;

estimate a position for the state vectors of the discrete random curve based on the signed distance and the tangent angle by minimizing a spatial Kalman filter cost function;

execute a Kalman smoothening function to estimate the position and the tangent angle for the state vectors that are part of the discrete random curve, wherein the state vectors each represent a map point of the fused map data;

detect a data point representing a new state vector that is introduced to the discrete random curve;

in response to detecting the data point, estimate an updated state vector, where the updated state vector indicates respective values for the position and a tangent angle of a new data point, wherein the updated state vector is expressed as:

s

ˆ

k

=

arg

⁢

min

s

k

(

d

k

-

〚

μ

v

)

〛

T

⁢

∑

v

-

1

(

d

k

-

μ

v

)

+

wherein ŝ k represents the updated state vector, k represents a total number of state vectors, represents a predicted state, d k represents a matrix indicating a lateral distance and a longitudinal distance for the first map data point and a lateral distance and a longitudinal distance for the second map data point, μ v represents a bias of measurement noise,

∑

v

-

1

represents a covariance of the measurement noise, and represents a covariance matrix; and

fuse together the two or more versions of map data to create the fused map data, wherein the fused map data is based on more than one unique source of data and includes a greater precision and accuracy when compared to the original versions of map data alone.

2 . The system of claim 1 , wherein the discrete random curve is one of the following: a polyline and a Markovian random curve.

3 . The system of claim 1 , wherein the spatial Kalman filter cost function is expressed as:

l

ˆ

1

:

k

=

min

l

ˆ

1

:

k

⁢

∑

i

=

1

k

⁢

(

d

i

-

μ

v

)

T

⁢

∑

v

-

1

⁢

(

d

i

-

μ

v

)

+

❘

"\[LeftBracketingBar]"

ϕ

i

-

ϕ

i

-

1

❘

"\[RightBracketingBar]"

2

q

u

2

wherein {circumflex over (l)} 1:k represents an estimated position of the updated state vector s k , k represents a total number of state vectors, d i represents the signed distance for the first map data point and the second map data point, μ v represents a bias of measurement noise, q u represents a variance of additive white Gaussian noise,

∑

v

-

1

represents a covariance of the measurement noise, and φ i represents the tangent angle.

4 . The system of claim 3 , wherein the one or more central computers execute instructions to:

receive ground truth map data;

execute one or more machine learning algorithms to determine a first bias and a first variance of the first set of map data points and a second bias and a second variance of the second set of map data points; and

execute an iterative algorithm to remove outliers from the first map data point, the second map data point, and the ground truth map data.

5 . The system of claim 4 , wherein the covariance of the measurement noise is determined based on the ground truth map data.

6 . The system of claim 4 , wherein the iterative algorithm is a random sample consensus (RANSAC) voting algorithm.

7 . The system of claim 6 , wherein the bias of the measurement noise is determined based on the first bias of the first set of map data points and the second bias of the second set of map data points.

8 . The system of claim 1 , wherein the Kalman smoothing function is expressed as:

l

ˆ

1

:

N

=

min

l

ˆ

1

:

N

⁢

∑

i

=

1

N

⁢

(

d

i

-

μ

v

)

T

⁢

∑

v

-

1

⁢

(

d

i

-

μ

v

)

+

❘

"\[LeftBracketingBar]"

ϕ

i

-

ϕ

i

-

1

❘

"\[RightBracketingBar]"

2

q

u

2

wherein {circumflex over (l)} 1:N represents an estimated position of the updated state vector s k , N denotes a total number of state vectors plus one, d i represents the signed distance for the first map data point and the second map data point, μ v represents a bias of measurement noise, q u represents a variance of additive white Gaussian noise,

∑

v

-

1

represents a covariance of the measurement noise, and φ i represents the tangent angle.

9 . The system of claim 1 , wherein the two or more versions of map data are each based on one or more of the following: global positioning system (GPS) data, perception data, a high-speed vehicle telemetry (HSVT) source, satellite image data, and data collected from survey vehicles.

10 . A method for fusing two or more versions of map data together, the method comprising:

receiving, by one or more central computers, road network data representing a road network for a predefined geofenced area, a first set of map data points, and a second set of map data points, wherein the road network data includes a discrete random curve that represents lane markings, and wherein the discrete random curve includes a plurality of state vectors that are each defined by a respective location and tangent angle;

determining, by the one or more central computers, a signed distance, wherein the signed distance includes a first perpendicular distance measured from a respective first map data point to the discrete random curve and a second perpendicular distance measured from a respective second map data point to the discrete random curve;

estimating, by the one or more central computers, a position for the state vectors of the discrete random curve based on the signed distance and the tangent angle by minimizing a spatial Kalman filter cost function;

executing a Kalman smoothening function to estimate the position and the tangent angle for the state vectors that are part of the discrete random curve, wherein the state vectors each represent a map point of a fused map data;

detecting, by the one or more central computer, a data point representing a new state vector that is introduced to the discrete random curve;

in response to detecting the data point, estimating an updated state vector, where the updated state vector indicates respective values for a position and the tangent angle of a new data point, wherein the updated state vector is expressed as:

s

ˆ

k

=

arg

⁢

min

s

k

(

d

k

-

〚

μ

v

)

〛

T

⁢

∑

v

-

1

(

d

k

-

μ

v

)

+

wherein ŝ k represents the updated state vector, k represents a total number of state vectors, represents a predicted state, d k represents a matrix indicating a lateral distance and a longitudinal distance for the first map data point and a lateral distance and a longitudinal distance for the second map data point, μ v represents a bias of measurement noise,

∑

v

-

1

represents a covariance of the measurement noise, and represents a covariance matrix; and

fusing together the two or more versions of map data to create the fused map data, wherein the fused map data is based on more than one unique source of data and includes a greater precision and accuracy when compared to the original versions of map data alone.

11 . A system for fusing two or more versions of map data together to create fused map data, the system comprising:

one or more central computers storing the two or more versions of map data and ground truth map data, wherein each version of the map data represents a predefined geofenced area, the one or more central computers executing instructions to:

receive road network data representing a road network for the predefined geofenced area, a first set of map data points, and a second set of map data points, wherein the road network data includes a discrete random curve that represents lane markings, and wherein the discrete random curve includes a plurality of state vectors that are each defined by a respective location and tangent angle;

determine a signed distance, wherein the signed distance includes a first perpendicular distance measured from a respective first map data point to the discrete random curve and a second perpendicular distance measured from a respective second map data point to the discrete random curve;

estimate a position for the state vectors of the discrete random curve based on the signed distance and the tangent angle by minimizing a spatial Kalman filter cost function, wherein the spatial Kalman filter cost function is expressed as:

l

ˆ

1

:

k

=

min

l

ˆ

1

:

k

⁢

∑

i

=

1

k

⁢

(

d

i

-

μ

v

)

T

⁢

∑

v

-

1

⁢

(

d

i

-

μ

v

)

+

❘

"\[LeftBracketingBar]"

ϕ

i

-

ϕ

i

-

1

❘

"\[RightBracketingBar]"

2

q

u

2

wherein {circumflex over (l)} 1:k represents an estimated position of an updated state vector s k , k represents a total number of state vectors, d i represents the signed distance for the first map data point and the second map data point, μ v represents a bias of measurement noise, q u represents a variance of additive white Gaussian noise,

∑

v

-

1

represents a covariance of the measurement noise, and φ i represents the tangent angle;

execute a Kalman smoothening function to estimate the position and the tangent angle for the state vectors that are part of the discrete random curve, wherein the state vectors each represent a map point of the fused map data; and

fuse together the two or more versions of map data to create the fused map data, wherein the fused map data is based on more than one unique source of data and includes a greater precision and accuracy when compared to the original versions of map data alone.

12 . The system of claim 11 , wherein the one or more central computers execute instructions to:

receive ground truth map data;

execute one or more machine learning algorithms to determine a first bias and a first variance of the first set of map data points and a second bias and a second variance of the second set of map data points; and

execute an iterative algorithm to remove outliers from the ground truth map data.

13 . The system of claim 12 , wherein the covariance of the measurement noise is determined based on the ground truth map data.

14 . The system of claim 12 , wherein the iterative algorithm is a random sample consensus (RANSAC) voting algorithm.

15 . The system of claim 11 , wherein the one or more central computers execute instructions to:

detect a data point representing a new state vector that is introduced to the discrete random curve; and

in response to detecting the data point, estimate an updated state vector, where the updated state vector indicates respective values for a position and the tangent angle of a new data point.

16 . The system of claim 11 , wherein the updated state vector is expressed as:

s

ˆ

k

=

arg

⁢

min

s

k

(

d

k

-

〚

μ

v

)

〛

T

⁢

∑

v

-

1

(

d

k

-

μ

v

)

+

wherein ŝ k represents the updated state vector, represents a predicted state, d k represents a matrix indicating a lateral distance and a longitudinal distance for the first map data point and a lateral distance and a longitudinal distance for the second map data point, and represents a covariance matrix.

17 . The system of claim 11 , wherein the Kalman smoothing function is expressed as:

l

ˆ

1

:

N

=

min

l

ˆ

1

:

N

⁢

∑

i

=

1

N

⁢

(

d

i

-

μ

v

)

T

⁢

∑

v

-

1

⁢

(

d

i

-

μ

v

)

+

❘

"\[LeftBracketingBar]"

ϕ

i

-

ϕ

i

-

1

❘

"\[RightBracketingBar]"

2

q

u

2

wherein {circumflex over (l)} 1:N represents an estimated position of the updated state vector s k and N denotes the total number of state vectors plus one.