VEHICLES, COMPUTER APPARATUSES AND METHODS FOR GENERATING A TRAJECTORY OF A VEHICLE ON A MAP

In one embodiment, a method of generating a trajectory of a vehicle on a map includes receiving map data of the map, wherein the map data includes a first road and a second road. The method further includes receiving odometry data from one or more sensors of the vehicle, the odometry data having an odometry uncertainty associated therewith, and receiving a plurality of vehicle location signals, each vehicle location signal having a location uncertainty associated therewith. The method also includes generating the trajectory of the vehicle on the first road or the second road of the map by providing as input to an optimizer the odometry uncertainty and the location uncertainty of the plurality of vehicle location signals as constraints.