Vehicle pose estimation and elevation mapping based on error model

In the forward elevation map reconstruction of multi-axle emergency rescue vehicles, measurement noises from the LiDAR reduced the accuracy of vehicle pose estimation, further leading to distortions in the elevation map. To address this issue, a vehicle pose estimation and elevation mapping method based on an uncertainty error model was proposed, which provided a modeling of the covariance propagation mechanism from state estimation to map construction. Based on this uncertainty error model, the fusion weight between the state prior of inertial-measurement-unit and the LiDAR observation model in the iterated error state Kalman filter was optimized, thereby enhancing the accuracy of vehicle pose estimation. Utilizing the improved pose estimation method and the elevation uncertainty model, the fusion and update of elevation information were achieved. Validation results on public datasets, simulation platforms, and real vehicle experiments show that the proposed method reduces the pose estimation error and improves the elevation mapping accuracy.