Input: |
Positioning data of the target vehicle: |
GPS(X, Y), WiFi(X, Y), CN(X, Y); |
Positioning data of its neighbor: |
GPSnei(X, Y), WiFinei(X, Y), CNnei(X, Y); |
Distance between the target vehicle and its neighbor: D |
Output: |
Position estimation of the target vehicle: ; |
Begin |
if the positioning data and distance are available then |
procedure FLM |
if GPS(X, Y) = WiFi(X, Y) = CN(X, Y) then |
set = = ; |
else |
compute s as (1); // the outlier rating |
compute e as (2); // the absolute error |
if s and e are not null then |
compute w as (3); // the importance degree |
if wx is less than th then |
set wx = 0, //x is GPS, WiFi, and CN; |
end if |
end if |
if , , and are not null then |
compute Target(X, Y) as (5); // composite position: |
end if |
end procedure |
end if |
if w and Target(X, Y) are available then |
procedure FKF |
//iterate at each sampling time (T) |
compute Cm(k) as (17); |
compute R(k) as (18) with (31); |
compute Q as (23); |
compute Z(k) as (8); |
compute as (15); |
end procedure |
end if. |
Return |
End |