Vehicle Speed and Position Estimation considering Microscopic Heterogeneous Car-Following Characteristics in Connected Vehicle Environments

Algorithm 1

Speed and position estimation for unsampled human vehicles.

(1)	%Step 1: Obtain the speed, position, and acceleration information of the vehicles within the detection range of CAV1 and CAV2.
(2)	Vehicle information:
(3)	Speed information:
(4)	Position information:
(5)	Acceleration information:
(6)	%Step 2: Calculate the speed correction factor, acceleration index correction factor, and headway distance correction factorat the current time.
(7)	While
(8)	Optimization Correction Factors :
(9)
(10)	Optimization Correction Factors and :
(11)
(12)	Output:
(13)	%Step 3: Estimating the position of the HV
(14)	Determine if there are any undetected HVs that need to be estimated:
(15)	Calculate the theoretical acceleration of:
(16)
(17)	If:
(18)	%There are HVs that have not been detected and need to be estimated.
(19)	While:
(20)	%Calculate the velocity of the HVs within the undetected range
(21)
(22)	%Calculating the vehicle spacing
(23)
(24)	%Calculate the position of the undetected HV.
(25)
(26)	Else:
(27)	Continue
(28)	Output: