Layered SOTIF Analysis and 3σ-Criterion-Based Adaptive EKF for Lidar-Based Multi-Sensor Fusion Localization System on Foggy Days

Abstract

The detection range and accuracy of light detection and ranging (LiDAR) systems are sensitive to variations in fog concentration, leading to the safety of the intended functionality-related (SOTIF-related) problems in the LiDAR-based fusion localization system (LMSFLS). However, due to the uncontrollable weather, it is almost impossible to quantitatively analyze the effects of fog on LMSFLS in a realistic environment. Therefore, in this study, we conduct a layered quantitative SOTIF analysis of the LMSFLS on foggy days using fog simulation. Based on the analysis results, we identify the component-level, system-level, and vehicle-level functional insufficiencies of the LMSFLS, the corresponding quantitative triggering conditions, and the potential SOTIF-related risks. To address the SOTIF-related risks, we propose a functional modification strategy that incorporates visibility recognition and a 3σ-criterion-based variance mismatch degree grading adaptive extended Kalman filter. The visibility of a scenario is recognized to judge whether the measurement information of the LiDAR odometry is disturbed by fog. Moreover, the proposed filter is adopted to fuse the abnormal measurement information of the LiDAR odometry with IMU and GNSS. Simulation results demonstrate that the proposed strategy can inhibit the divergence of the LMSFLS, improve the SOTIF of self-driving cars on foggy days, and accurately recognize the visibility of the scenarios.

1. Introduction

Accurate localization information is crucial for autonomous vehicles and affects subsequent decision planning and control. Compared with a single sensor, multi-sensor fusion technology has many advantages, such as more channels to obtain information, less information uncertainty, and more convenience for fault detection [1,2]. Therefore, it is considered the first choice for autonomous vehicle localization technology. Light detection and ranging (LiDAR), based on the time-of-flight principle, is widely used in autonomous vehicle localization and environmental sensing. It has the advantages of a wide detection range, high accuracy, not being easily affected by ambient lighting conditions, and a 3D perception of the surrounding environment [3,4]. Therefore, the industry has widely adopted LiDAR-based multi-sensor fusion localization schemes, such as Google Waymo and Baidu Apollo.

However, adverse weather conditions, such as rain, snow, and fog, can degrade the detection performance of LiDAR and limit the all-weather operation of autonomous vehicles [5,6]. Previous studies [7,8] have shown that heavy rain interferes with LiDAR in a similar way to fog, while fog has a greater interference on LiDAR than rain or snow. Therefore, this study investigated the interference of fog on LiDAR.

The laser beam emitted by LiDAR is absorbed, scattered, and refracted by fog, which attenuates the range and increases the uncertainty of LiDAR detection. Fog interference on LiDAR causes the LiDAR-based multi-sensor fusion localization system (LMSFLS) to provide incorrect localization information to an autonomous vehicle, which ultimately affects the safety of the vehicle. Such safety problems are caused by the performance insufficiencies of LiDAR on foggy days and are not caused by faults in LiDAR. According to ISO 21448 [9], the safety of intended functionality (SOTIF) is defined as the “absence of unreasonable risk due to hazards resulting from functional insufficiencies of the intended functionality or its implementation.” Safety problems caused by functional insufficiencies of LiDAR on foggy days fall within the SOTIF domain.

Foggy conditions are a common occurrence in the real world, and LiDAR is susceptible to functional deficiencies caused by fog, which can lead to SOTIF-related risks of LMSFLS. However, due to weather constraints, it is challenging to quantitatively analyze the impact of fog interference on LMSFLS in natural environments. Therefore, the primary objectives of this study are to (1) utilize a fog simulation approach to quantitatively simulate and assess the impact of fog at varying concentrations on LMSFLS, (2) quantitatively analyze the simulation results to identify component-level, system-level, and vehicle-level functional insufficiencies of the LMSFLS, associated triggering conditions, and potential SOTIF-related harms on foggy days, and (3) propose a functional modification strategy to improve the SOTIF of LMSFLS under fog disturbance. The primary contributions of this research are:

• A layered quantitative SOTIF analysis method was proposed for the LMSFLS in foggy environments based on ISO 21448. The method includes static detection analysis for LiDAR and localization performance analysis for LMSFLS. Based on this, we identified the potential SOTIF-related harms and quantitative triggering conditions of LMSFLS on foggy days by quantitatively analyzing the component-level, system-level, and vehicle-level functional insufficiencies caused by fog in different concentrations.
• A functional modification strategy was proposed to address the SOTIF-related harms of LMSFLS. In this strategy, visibility recognition was first introduced to identify whether LiDAR odometry is interfered with by fog. Subsequently, the 3σ-criterion-based variance mismatch degree grading adaptive extended Kalman filter (3σ-VMDG-AEKF) was employed to accurately isolate abnormal measurement information in LiDAR odometry through sequential filtering and variance mismatch degree grading.

2. Related Work

Our research involved the effects of adverse weather on LiDAR, LiDAR-based multi-sensor localization methods, and SOTIF of the automated driving function. This section reviews the current status of these research areas.

2.1. Effects of Adverse Weather on LiDAR

In recent years, industries have paid increasing attention to the effects of adverse weather conditions on vehicle sensors. Some research results have been introduced in detail in [7,10,11]. Modeling and testing are the two main methods used to study the effects of fog on LiDAR. Modeling methods include physics-based and data-driven modeling, and testing methods include indoor and outdoor testing.

Several studies have been conducted on modeling based on physical mechanisms. The research in [8,12,13] modeled the interference of adverse weather on LiDAR according to its physical mechanism. Hahner et al. [14] modeled the effects of fog on LiDAR detection according to the physical mechanism of LiDAR pulse transmission and added the information on these effects to LiDAR point-cloud datasets in clear weather to generate LiDAR point-cloud datasets affected by fog. These datasets were then used to train a neural network to study the target detection performance of LiDAR under foggy conditions. Zhao et al. [15] proposed a data-oriented LiDAR model composed of geometric and physical models that modeled signal attenuation and unwanted raw data in rain, snow, and foggy weather. The mentioned studies mainly focused on the effects of fog on LiDAR using theoretical models, and experimental tests were used to verify the accuracy of the proposed models. However, other scholars have directly used experimental data to study the effects of fog on LiDAR. Kutila et al. [16], in the outdoor conditions of Sodankyla airport, conducted rain, snow, and fog attenuation tests on LiDAR and pointed out that LiDAR detection performance is sensitive to variations in fog concentration. Refs. [17,18] used the Cerema chamber to control the concentration of fog to test the effects of fog at different concentrations on different LiDAR, and they pointed out that increasing the transmit power of LiDAR can improve its detection range. In [19,20,21], the Cerema chamber test data were used to train machine-learning models and predict fog attenuation on LiDAR detection performance. These studies focused on the effects of fog on LiDAR detection performance or target recognition and provided essential references for our subsequent research.

2.2. LiDAR-Based Multi-Sensor Fusion Localization

LiDAR 3D point cloud feature extraction is a critical step in LiDAR localization, affecting localization accuracy and real-time performance [22]. Features such as road signs, lane markings, guard-rail reflectors, and road markings are used for high-precision localization in urban roads in [23,24,25,26]. Steink et al. [27] proposed a feature-extraction method based on geometric fingerprint technology from a point cloud. They applied this method to the IMU-LiDAR fusion localization system to achieve centimeter-level localization accuracy. Liu et al. [28] proposed a feature extraction network, YOLOv5-Tassel, based on YOLOv5 for extracting complex and variable features. Yin et al. [29] proposed a localization framework based on 3D LiDAR. LocNet was used in this framework for 3D point cloud feature recognition, improving localization efficiency and accuracy. Some studies in the context of LiDAR-based localization used map matching. Lu et al. [30] presented the L3-Net-based LiDAR localization system that achieved centimeter-level localization accuracy. Chen et al. [31] exploited range images generated from 3D LiDAR scans to address the problem of localizing mobile robots or autonomous cars in a map of a large-scale outdoor environment represented by a triangular mesh. Regarding fusion localization methods, Xiong et al. [32] proposed a robust estimation method for automated vehicle sideslip angle and attitude based on the parallel adaptive Kalman filter, which combined GNSS and INS. Xia et al. [33,34] estimated the yaw alignment error and velocity error of the vehicle using the Kalman filter and then employed the Consensus Kalman Filter to synthesize the vehicle kinematics and dynamics and estimate the vehicle’s heading error. These studies demonstrate that fusion methods based on Kalman filtering can accurately estimate the vehicle’s state parameters. Therefore, they have been widely adopted in lidar-based fusion localization systems. Zubača et al. [35] proposed an extended H∞ filter with an adaptive new information sequence to fuse the measurement information of LiDAR, IMU, and other dynamic sensors of vehicles to improve the robustness and accuracy of vehicle post-estimation. Maaref et al. [36] proposed a lane-level localization method that combines LiDAR odometry and cellular data pseudo-distance. However, these studies mainly focused on the means to improve the localization performance of LMSFLS in clear or rainy weather without considering the effects of foggy days. Therefore, in this study, we developed a foggy point cloud simulation method to introduce fog interference to the LMSFLS.

2.3. SOTIF of the Automated Driving Function

With the demands of SOTIF for autonomous vehicles, some SOTIF-related studies on automated driving functions have emerged. Different SOTIF analysis methods, which combined with system analysis theories such as Systems Theory Process Analysis (STPA) and Model-Based Systems Engineering (MBSE), were presented in [37,38,39,40], and these methods were applied to autonomousemergency braking (AEB) systems. Both [41] and [42] studied the SOTIF-related problems of lane-keeping assist (LKA) systems and proposed solutions. Guo et al. [43] studied the SOTIF-related problems of human misuse caused by driver distrust of autopilot systems. They proposed a path-planning method based on model predictive control, considering the degree of confidence. Huang et al. [44] proposed a public systematic identification method for triggering events owing to system performance limitations and human misuse. On this basis, they offered the safety analysis and verification procedures of SOTIF and applied these procedures to an L3 autonomous vehicle. Unlike the above studies, Wang et al. [45] proposed a robust non-fragile fault-tolerant control strategy as a risk reduction method to ensure SOTIF of the cooperative adaptive cruise control (CACC) under the conditions of system uncertainty, multi-source perturbations, and controller perturbations.

From the abovementioned studies, based on ISO 21448, scholars proposed corresponding SOTIF analysis processes for AEB, LKA, CACC, and other driving assistance functions by directly using system analysis theories such as STPA or MBSE. These processes focus on the identification of qualitative triggering conditions and human misuse. However, the quantitative SOTIF analysis methods for the SOTIF-related problems caused by fog in LMSFLS are lacking. Therefore, in this study, the component-level, system-level, and vehicle-level functional insufficiencies of LMSFLS caused by the fog will be quantitatively analyzed to identify the SOTIF-related harms and quantitative triggering conditions. Then, functional modification strategies for these SOTIF-related problems would be proposed to improve the SOTIF of LMSFLS on foggy days.

3. Fog Interference Simulation Method of LMSFLS

In this section, we explain in detail our approach for obtaining localization information with fog interference. After introducing the LMSFLS architecture with fog interference introduced in Section 3.1, we show how we generated foggy point clouds in Section 3.2.

3.1. The Architecture of LMSFLS with Fog Interference Introduced

The vehicle-mounted LMSFLS used in this study comprised LiDAR, INS, and GNSS sensors, and the fusion algorithm adopted was the extended Kalman filter (EKF). The information source for each sensor was as follows. The 360° circumnavigation LiDAR was installed on the top of the vehicle, and point clouds were generated by scanning a scenario. INS and GNSS obtained the required information directly from the vehicle.

We proposed an LMSFLS architecture, which introduced fog interference, as illustrated in Figure 1. First, clear point clouds of the scenario were generated using the LiDAR numerical model. Second, the clear point clouds and visibility were input into the foggy point-cloud generation model (FPCGM) to generate the foggy point clouds, which considered the attenuation and noise effects of fog for the scenario. Third, the foggy point clouds were input into the point-cloud registration module. Then the foggy point clouds after registration were input into LiDAR odometry to solve the pose information of the ego vehicle. Finally, we fused the measurement information from LiDAR odometry, INS pre-integration, and GNSS to get the estimation information of the ego vehicle. Through the above four steps, we introduced fog interference into the LMSFLS. Therefore, it can output both the detection information of LiDAR and the localization information of LMSFLS that have been hampered by fog.

A discrete-time state-space nonlinear model of the LMSFLS can be expressed as follows:

$$X_k=f\left(X_{k-1}\right)+W_{k-1}$$

(1)

$$Z_k=h\left(X_k\right)+M_k$$

(2)

where $f$ is the state transition function, $h$ is the observation function, $X={[p_{sv} v_{sv} {\phi }_{sv}]}^T$ is the state vector, $Z={[p_{ov} v_{ov} {\phi }_{ov}]}^T$ is the observation vector, in both $X$ and $Z$, $p={[p_x p_y p_z]}^T$ and $v={[v_x v_y v_z]}^T$ are the position and velocity in the navigation coordinate system, $\phi ={[{\phi }_x {\phi }_y {\phi }_z]}^T$ is the Euler angle of attitude in the vehicle coordinate system; $W$ is the process noise vector, $M$ is the observation noise vector, and $k$ is the time-step. The EKF algorithm is used as the data fusion algorithm, where INS pre-integration is treated as the prediction process, and the measurements from laser odometry and GNSS are regarded as the observation process. Please refer to Appendix A for details on LiDAR odometry, INS pre-integration, and data fusion.

3.2. Foggy Point Clouds Generation

Based on the existing research on LiDAR attenuation [46], physical [20], and noise [47] models, we propose FPCGM to generate fog point clouds. The specific process for converting clear point clouds into foggy point clouds is shown in Algorithm 1. Please refer to Appendix B for a detailed explanation of Algorithm 1.

Algorithm 1: Generation of foggy point clouds
1	Initialization:$\mathrm{Input} \mathrm{visibility} \mathrm{and} \mathrm{one} \mathrm{frame} \mathrm{of} \mathrm{clear} \mathrm{point} \mathrm{cloud} \mathrm{that} \mathrm{includes} n$ points
2	for	$i<n$ do
3		$\mathrm{Calculate} \gamma$ using (A9)
4		$\mathrm{Calculate} P_r^i$ using (A10)
5		$\mathrm{Calculate} {SNR}_i$ using (A17)
6		if	${SNR}_i>{SNR}_0$ then
7			$\mathrm{Calculate} {\sigma }_R^i$$\mathrm{and} \mathrm{add} \mathrm{it} \mathrm{on} x^i$
8		else
9			The $i^{\prime }th$ point is invalid
10		end if
11	end for
12	return	One frame of foggy point cloud

To verify the feasibility of algorithm 1, we input a frame of real foggy point clouds from the RADIATE [48] dataset into the FPCGM. Since this frame of fog point cloud contains over 4000 laser points, the generation of each laser point represents a simulation by FPCGM. Therefore, this verified FPCGM over 4000 times in this case. After reasonably setting the reflectance of the targets, the point cloud intensity distribution was calculated, and

Table 1. Lidar parameters.

Parameters	Descriptions	Values
$P_0$	Laser emitted energy	1.6 μJ
$\Delta S$	Spatial resolution	15 m
${\eta }_{st}$	Laser emitter efficiency	0.8
${\eta }_{sr}$	Laser receiver efficiency	0.3
$\eta$	Receiver quantum efficiency	0.1
$\theta$	Receiving telescope viewing angle	0.0003 rad
$A_R$	Receiver effective area	10 cm2
$∆\lambda$	Optical filter bandwidth	60 nm
$P_B$	Sky background radiation brightness	0.6 W/(m2·nm·sr)
$C_D$	Dark counting rate of the receiver	300

listed the parameter settings. Then the calculated point cloud intensity distribution was compared with the real point cloud intensity distribution. The results are shown in Figure 2 and Figure 3, and the data in the figure is normalized.

Figure 2 demonstrates the intensity distribution of the simulation and real results of foggy point clouds, and we can see that the distribution of point cloud intensity is similar in both the simulation and real results. Figure 3 shows the corresponding normalized value distribution. From it, we can also find both the simulation and real results are similar. So, the proposed FPCGM is feasible.

4. Fog Simulation-Based Layered SOTIF Analysis

In this section, we conducted a layered quantitative SOTIF analysis, as shown in Figure 4, to identify the SOTIF-related issues of LMSFLS in foggy environments. First, we performed a quantitative analysis of LiDAR static detection to assess the impact of fog on a single laser beam and a one-frame point cloud. Subsequently, we analyzed the effect of different concentrations of fog on the localization performance of the LMSFLS. Based on the analysis results, we identified the functional insufficiencies of LMSFLS at the component, system, and vehicle levels, along with the corresponding triggering conditions and potential SOTIF-related harms.

4.1. The Analysis for LiDAR Static Detection in Foggy Environments

The maximum detection range of vehicle-borne LiDAR in clear weather is generally 100–200 m, and the reflectance of the corresponding target object is 0.8–1. We assumed that the farthest detection range of the LiDAR was 120 m in clear weather, and the corresponding reflectance of the target was 0.8. When these two parameters were input into the FPCGM, the $SNR$ was 68.3 and ${\sigma }_R$ was 0.12 m under this condition. This $SNR$ can be used as the ${SNR}_0$ in Algorithm 1; that is, it is considered invalid if the $SNR$ of the point-cloud echo is less than 68.3, otherwise, it is retained.

According to [49], when the horizontal visibility is 1–10 km, it is light fog; when the horizontal visibility is 0.5–1 km, it is fog; and when the horizontal visibility is 0.2–0.5 km, it is heavy fog. First, we set the visibility to 10 km, 1 km, 0.8 km, 0.6 km, 0.5 km, 0.4 km, 0.3 km, and 0.2 km to simulate the interference of fog on the static detection of a single laser beam of LiDAR by using FPCGM. Subsequently, we set the visibility to 10 km, 1 km, 0.6 km, and 0.2 km and input one frame of clear point cloud, as shown in Figure 5, into FPCGM to simulate fog interference on clear point clouds. The LiDAR parameters are listed in Table 1, and the simulation results are shown in Figure 6 and Figure 7.

In Figure 6a, the intersection points of the curves and horizontal line are the $SNRs$ corresponding to the maximum detection ranges of the LiDAR echo signal under this visibility. Parts of the curves that are lower than the horizontal line are invalid. In Figure 6b, the intersection points of the curves and horizontal line are the values of ${\sigma }_R$ corresponding to the maximum detection ranges of the LiDAR echo signal under this visibility, and parts of the curves that are higher than the horizontal line are considered invalid. In Figure 7a–d, when the visibility is 10 km, 1 km, 0.6 km, and 0.2 km, the LiDAR receives 6, 4, 2, and 1 cluster of obstacle point clouds, respectively. The following conclusions can be drawn from Figure 6 and Figure 7.

• Under the condition that the visibility remains constant, the $SNR$ and ${\sigma }_R$ of the echo signals decreases and increases, respectively, with an increase in the LiDAR detection range.
• When the LiDAR detection range is constant, the $SNR$ decreases and ${\sigma }_R$ increases with a decrease in visibility. The smaller the visibility, the faster the decrease rate of $SNR$, and the quicker the increase rate of ${\sigma }_R$.
• With a reduction in visibility, the maximum detection range of LIDAR decreases. When the visibility was 10 km, 1 km, 0.8 km, 0.6 km, 0.5 km, 0.4 km, 0.3 km, and 0.2 km, the corresponding maximum detection ranges were 120 m, 88 m, 83 m, 76 m, 71 m, 65.5 m, 58 m, and 48 m.
• The number of point cloud clusters of obstacles in foggy point clouds decreases with reduced visibility.

These results indicate that the $SNR$ and ${\sigma }_R$ of the LiDAR echo signals are sensitive to variations in visibility, resulting in the reduction of maximum detection range and accuracy of LiDAR on foggy days. This will inevitably affect the localization performance of the LMSFLS. Therefore, it is necessary to analyze the effects of fog on the localization performance of the LMSFLS further.

4.2. The SOTIF Analysis for LMSFLS in Foggy Environments

• Simulation scenarios and parameters setting

An uneven arrangement of obstacles in a scenario affects the localization performance of the LiDAR odometry. To study the interference of fog on the LMSFLS more clearly, it is necessary to eliminate the effects of an uneven arrangement of obstacles. Therefore, this study used the driving scenario designer app of MATLAB to build straight and curved urban expressway scenarios with obstacles uniformly arranged on both sides of the roads, as shown in Figure 8 and Figure 9. The distance between obstacles in the scenarios is 75 m. The distance between the obstacles and the road is 40 m. When the visibility is less than or equal to 0.3 km, the maximum detection range of LiDAR is reduced by more than 50%, and the sensor is stopped immediately for safety reasons. Therefore, the effects of fog on the localization performance of LMSFLS for these two visibility types of 0.2 km and 0.3 km will not be discussed in a subsequent study. In the simulation, the vehicle ran in the middle lane of the right road at a constant speed of 90 km/h, and the total running distance was approximately 1 km. The visibility of the scenarios was set to 10 km, 1 km, 0.8 km, 0.6 km, and 0.4 km. The MATLAB simulation duration is related to the setting of road length, and the simulation ends when the vehicle runs to the end of the setting road. The parameters of the various sensors are listed in

Table 2. Sensors parameters.

Sensors	Parameters
INS Gyroscope	Noise: 0.573 deg/s Frequency: 100 Hz
INS Accelerometer	Noise: 0.1 m/s2 Frequency: 100 Hz
GNSS Position	Noise: 2 m Frequency: 10 Hz
GNSS Velocity	Noise: 0.1 m/s Frequency: 10 Hz
LiDAR	Max Range: 120 m Range Accuracy: 0.002 m Azimuth: 0.4 deg Elevation: 1.875 deg Azimuthal Limits: [−180° 180°] Elevation Limits: [−15° 15°] Frequency: 20 Hz

.

2.

Quantitative triggering condition analysis

Under the interference of fog at different concentrations in straight and curved urban expressway scenarios, the variation in LMSFLS localization performance is shown in Figure 10 and Figure 11, in which the localization error versus the running time of the vehicle is plotted. The localization errors were calculated in the vehicle coordinate system, as in reference [50]. From Figure 10a,b and Figure 11a,b, we can see that in the straight urban expressway scenario, with a decrease in visibility, the lateral localization error curves of the LMSFLS constantly fluctuate around 0 m. However, the longitudinal localization error curves diverged when the visibility was 0.6 km and 0.4 km. In the curved urban expressway scenario, with a decrease in visibility, the lateral and longitudinal localization error curves of the LMSFLS diverged when the visibility was 0.8 km, 0.6 km, and 0.4 km.

Therefore, it can be concluded that the quantitative triggering condition for SOTIF-related issues of LMSFLS in the straight urban expressway scenario is when the visibility is less than or equal to 0.6 km, and in the curved urban expressway scenario, it is when the visibility is less than or equal to 0.8 km.

3.

Functional insufficiencies analysis

To clarify the reasons for the divergence of localization errors, we analyzed the relationship between the increment of localization errors and LiDAR point clouds. We found that when the increment of localization errors increased, the obstacle point cloud detected by LiDAR was a feature-degraded point cloud similar to that in Figure 12. Inputting two consecutive frames of this type of point cloud into the LiDAR odometry for scan-to-scan matching can create an illusion of point cloud repetition patterns in the LiDAR odometry. When solving for the vehicle pose transformation, this type of point cloud can only provide constraints on the vehicle’s lateral degree of freedom but cannot provide constraints on the longitudinal and vertical rotational degrees of freedom for pose estimation algorithms. Therefore, it can be observed that in the straight urban expressway scenario, where the vehicle’s attitude does not undergo rotation, the lateral positioning accuracy of the fusion localization system does not diverge, while the longitudinal positioning accuracy diverges. This indicates that the longitudinal solution of the LiDAR odometry has degraded. In the curved urban expressway scenario, where the vehicle experiences rotation, the fusion localization system exhibits divergence in both lateral and longitudinal positioning accuracy, indicating that the overall solution of the LiDAR odometry has degraded. We statistically analyzed the relationship between visibility and the probability of degradation point clouds detected by LiDAR in straight and curved urban expressway scenarios, as shown in

Table 3. The probability of detecting feature degenerated point clouds.

Scenarios	Visibility (km)	Probability of Detecting Feature Degenerated Point Clouds (%)
Straight urban expressway	10.0	0
	1.0	0
	0.8	0
	0.6	6.7
	0.4	40.0
Curved urban expressway	10.0	0
	1.0	0.7
	0.8	4.6
	0.6	15.1
	0.4	40.8

. From it, we can see that the lower the visibility is, the higher the probability of LiDAR detecting feature degraded point clouds.

Based on the above analysis, in foggy straight and curved urban expressway scenarios, the component-level functional insufficiencies of LMSFLS are, respectively, the longitudinal and overall solution degradation of the LiDAR odometry. Then according to Figure 10 and Figure 11, the system-level functional insufficiencies of LMSFLS are, respectively, longitudinal localization divergence and longitudinal-lateral localization divergence. Finally, it can be inferred that the vehicle-level functional insufficiencies in the straight urban expressway scenario were unexpected acceleration and deceleration, and in the curved urban expressway scenario were unexpected acceleration, deceleration, and steering.

4.

SOTIF-related harms analysis

According to ISO 21448, a vehicle-level functional insufficiency corresponds to a hazardous behavior. When a hazardous behavior occurs and there is a condition that could cause a harm in the scenario, a hazardous event is formed. If the hazardous event is uncontrollable, harm will occur. In the straight urban expressway scenario of this study, if the hazardous behavior of unexpected acceleration or deceleration of the ego vehicle is triggered by fog with visibility less than 0.6 km and there are other vehicles in front or behind, a hazardous event of a collision between the ego vehicle and the front or rear vehicle occurs. Moreover, if there is no safety strategy to control the vehicle, the harm of collision between the ego vehicle and the front or rear vehicle will be caused. Similarly, in the curved urban expressway scenario, if the hazardous behavior of unexpected acceleration, deceleration and steering of the ego vehicle is triggered by fog with visibility less than 0.8 km and there are other vehicles around the ego vehicle, the harm of collision between the ego vehicle and surrounding vehicles or road infrastructure will be caused. A more detailed description is as follows: in the event of an unexpected acceleration and left steering of the ego vehicle, a harm of collision with a neighboring vehicle or road infrastructure in the front left may occur. Similarly, an unexpected acceleration and right steering of the ego vehicle may occur a harm of collision with a neighboring vehicle or road infrastructure in the front right. Moreover, if the ego vehicle unexpectedly decelerates and turns left, it may occur a harm of collision with a neighboring vehicle or road infrastructure in the rear left. Likewise, an unexpected deceleration and right turn of the ego vehicle may occur a harm of collision with a neighboring vehicle or road infrastructure in the rear right. The results of the SOTIF analysis are presented in

Table 4. Results of SOTIF analysis.

Scenarios	Triggering Condition	Functional Insufficiencies
		Component-Level	System-Level	Vehicle-Level
Straight urban expressway	Visibility is 0.6 km or less.	Solution degradation of longitudinal localization	Divergence of longitudinal localization.	Unintended acceleration or deceleration
Curved urban expressway	Visibility is 0.8 km or less.	Solution degradation of longitudinal and lateral localization	Divergence of longitudinal and lateral localization.	Unintended acceleration, deceleration, or steering
Scenarios	SOTIF-Related Harms
Straight urban expressway	Collision with the front neighboring vehicle. Collision with the rear neighboring vehicle.
Curved urban expressway	Collision with a neighboring vehicle or road infrastructure in the front left. Collision with a neighboring vehicle or road infrastructure in the front right. Collision with a neighboring vehicle or road infrastructure in the rear left. Collision with a neighboring vehicle or road infrastructure in the rear right.

.

5. Functional Modification Strategy

According to the SOTIF analysis results, whether in a straight or curved urban expressway scenario with a decrease in visibility, the LMSFLS will experience the functional insufficiency of localization divergence, leading to the potential SOTIF-related harm of collision with vehicles. Therefore, it is necessary to propose a functional modification strategy to ensure the stability of the LMSFLS and then improve the SOTIF.

5.1. Strategy Process

To address the SOTIF-related risks of the LMSFLS caused by fog, we proposed a functional modification strategy that was mainly divided into two steps. In the first step, the visibility of the current scenario is recognized, and whether the LiDAR odometry is disturbed by fog is judged according to the set visibility threshold $Vth$. In the second step, if LiDAR odometry is disturbed by fog, the 3σ-VMDG-AEKF is adopted to fuse the LiDAR odometry measurement information with GNSS and INS. The process of the functional modification strategy is shown in Figure 13.

5.2. Recognition of Visibility

It can be seen from previous analysis that the detection range and accuracy of LiDAR are susceptible to variations in visibility. Therefore, visibility is used to judge whether the measurement information of LiDAR odometry is disturbed by fog. Based on the echo point clouds from LiDAR, taking the natural logarithm of both sides of Equation (A10), we obtain the following result:

$${\gamma }^j=-\frac{1}{2x^j}\mathit{ln}\frac{{{(x^j)}^{2}P}_r^j}{C_L{\rho }_{tar}}$$

(3)

where superscript $j$ represents the $j^{\prime }th$ laser point. According to Equation (A9), we obtain

$$V^j=\frac{0.18126{\lambda }^2+0.13709\lambda +3.7502}{{\gamma }^j}$$

(4)

Using (3) and (4), we can calculate the visibility value recognized by each laser point in one frame foggy point cloud. The mean of these values is then calculated as the visibility of the current scenario. It is worth noting that the values of visibility recognized by the laser points with shorter ranges fluctuated wildly; therefore, the values of visibility recognized by the laser points with longer ranges were selected, and their mean was the recognized visibility of the current scenario. In this study, laser points with an $x$ greater than 30 m are used to recognize visibility. That is,

$$V=\frac{1}{N}{\sum }_{j=1}^N{V}^j$$

(5)

where $N$ is the number of laser points whose $x$ values are greater than 30 m.

After recognizing the visibility of the scenario, we can judge if the LiDAR odometry is disturbed by fog by setting a visibility threshold $V\mathrm{t}\mathrm{h}$. The analysis results in Table 3 show that when the visibility is less or equal to 0.8 km, the LiDAR odometry has a high probability of being disturbed. Therefore, the visibility threshold $Vdth$ was set to 0.8 km in this study.

5.3. 3σ-Criterion-Based Variance Mismatch Degree Grading Adaptive Extended Kalman Filter

After a sensor fault is identified, the most direct and effective way to handle it is to isolate the corresponding sensor so as not to affect the stability of the localization system. In this study, LiDAR odometry is disturbed by fog, and the measurement information deviates from the actual value in part or whole. If LiDAR odometry is directly isolated, it inevitably leads to inefficient use of its measurement information. Therefore, a method that can accurately identify and isolate the abnormal part in the measurement information is required to fuse the LiDAR odometry measurement information with GNSS and INS; to meet this requirement, the 3σ-VMDG-AEKF algorithm was proposed in this study.

$\epsilon$ is defined as the one-step prediction residual, which represents the degree of variance mismatch between the measured state of the LiDAR and the predicted state of the INS. For the $k^{\prime }th$ measurement, the $\epsilon$ of the $i^{\prime }th$ state is

$${\epsilon }_k^i=Z_k^i-H_k^i{\check{X}}_k,$$

(6)

where the “$\stackrel{ˇ}{}$” symbol represents the prediction, $Z_k^i$ is the $i^{\prime }th$ state in $Z_k$, $H_k=J\left(h\left({\check{X}}_k\right)\right)$, $J$ is the Jacobian matrix, and $H_k^i$ is the $i^{\prime }th$ row of $H_k$.

$\sigma$ is defined as the standard deviation of $\epsilon$. For the $k^{\prime }th$ measurement, the $\sigma$ of the $i^{\prime }th$ state is

$${\sigma }_k^i=\sqrt{H_k^i{\check{P}}_k^i{H_k^i}^T+R_k^i},$$

(7)

where $P$ is the mean square error matrix of $X$, ${\check{P}}_k^i$ is the $i^{\prime }th$ diagonal element of ${\check{P}}_k$, $R$ is the observation noise matrix, and $R_k^i$ is the $i^{\prime }th$ diagonal element of $R_k$.

$\widehat{C}$ is defined as the estimation of the noise matrix observed by LiDAR. According to Sage–Husa [51] filter, for the $k^{\prime }th$ measurement, the $i^{\prime }th$ diagonal element of $\widehat{C}$ is

$${\widehat{C}}_k^i=\left(1-{\beta }_k\right){\widehat{C}}_{k-1}^i+{\beta }_k{\epsilon }_k^i{\epsilon }_k^i,$$

(8)

where the “$\widehat{}$” symbol represents the estimation, ${\beta }_k=\frac{{\beta }_{k-1}}{{\beta }_{k-1}+b}$, ${\beta }_0=1$, and $0<b<1$. Here, $b$ is called the fading factor, and its value is usually 0.9–0.999.

When the $i^{\prime }th$ state in the observation vector of LiDAR is no variance mismatch, there is

$${\widehat{C}}_k^i\approx H_k^i{\check{P}}_k^i{H_k^i}^T+R_k^i.$$

(9)

When the $i^{\prime }th$ state in the observation vector of LiDAR is a mild variance mismatch, $R_k^i$ is considered to be biased; for (9) to be true, there is

$${\widehat{C}}_k^i-H_k^i{\check{P}}_k^i{H_k^i}^T={\alpha }_k^i{R}_k^i,$$

(10)

where $\alpha$ is the adaptive noise coefficient. For the $k^{\prime }th$ measurement, the $\alpha$ of the $i^{\prime }th$ state is

$${\alpha }_k^i=\frac{{\widehat{C}}_k^i-H_k^i{\check{P}}_k^i{H_k^i}^T}{R_k^i}.$$

(11)

Finally, we can adaptively compute the ${\widehat{X}}_k^i$ and ${\widehat{P}}_k^i$ using the following three equations:

$$K_k^i={\check{P}}_k^i{H_k^i}^T{\left(H_k^i{\check{P}}_k^i{H_k^i}^T+{\alpha }_k^i{R}_k^i\right)}^{-1}$$

(12)

$${\widehat{X}}_k^i={\check{X}}_k^i+K_k^i{\epsilon }_k^i$$

(13)

$${\widehat{P}}_k^i=\left(I-K_k^i{H}_k^i\right){\check{P}}_k^i$$

(14)

After recognizing that the LiDAR odometry is disturbed by fog, the $\epsilon$ and $\sigma$ of each element in the measurement state vector of LiDAR odometry are calculated sequentially. According to the 3$\sigma$ criterion in the Gaussian distribution, if $\left|\epsilon \right|<\sigma$, the element does not exhibit a variance mismatch. This element can be directly used for fusion. If $\sigma \le \left|\epsilon \right|<3\sigma$, then the element with a mild variance mismatch can be adaptively fused. If $\left|\epsilon \right|\ge 3\sigma$, then the element with a severe mismatch of variance should be isolated directly. However, in practical applications, the value of $\sigma$ may not be sufficiently accurate. We can modify $\sigma$ by setting $\sigma =a·\sigma$, where $0<a<1$. The specific process is shown in Algorithm 2.

Algorithm 2:$3\sigma$-VMDG-AEKF
1	Initialization: ${\check{X}}_k,Z_k,{\check{P}}_k,R_k$
2	for	$i\le m$
3		$\mathrm{Calculate} {\epsilon }_k^i$ using (6)
4		$\mathrm{Calculate} {\sigma }_k^i$ using (7)
5		if	$\left|{\epsilon }_k^i\right|<{\sigma }_k^i$
6			$\mathrm{Calculate} {\widehat{X}}_k^i$ and ${\widehat{P}}_k^i$ using (13) and (14)
7		else if	${\sigma }_k^i\le \left|{\epsilon }_k^i\right|<3{\sigma }_k^i$
8			$\mathrm{Calculate} {\alpha }_k^i$ using (11)
9			$\mathrm{Calculate} {\widehat{X}}_k^i$ and ${\widehat{P}}_k^i$ using (12)–(14)
10		else
11			${\widehat{X}}_k^i={\check{X}}_k^i,{\widehat{P}}_k^i={\check{P}}_k^i$
12		end if
13	end for
14	Return	${\widehat{X}}_k$ and ${\widehat{P}}_k$

6. Validation of Functional Modification Strategy

6.1. Design of Validation Schemes

We verified the effectiveness and visibility recognition performance of the proposed strategy in two quasi-real scenarios built after the Beijing Inner Ring Expressway. The straight urban expressway scenario was built as a part of the West North Fourth Ring Road. The curved urban expressway scenario was built after a part of the North East Third Ring Road. They are shown in Figure 14 and Figure 15.

In the two quasi-real scenarios, we set two types of visibility: constant and variable. The constant visibility is 0.4 km. The variation range of the variable visibility is 0.4–1 km, the visibility first decreasing from 1 km to 0.4 km, then stabilizing for a while, and finally increasing from 0.4 km to 1 km. This working condition simulates a vehicle moving from a fog area to a heavy fog area and then from a heavy fog area to a fog area. The operation conditions of the vehicle are the same as before. The comparison methods are EKF, AEKF, and EKF with fault diagnosis and isolation (EKF with FDI). The EKF is the filter we used for SOTIF analysis, which lacks adaptive and FDI capabilities. The AEKF is a filter that uses the Sage–Husa method for noise parameter adaptation. The EKF with FDI is a filter with FDI capabilities, where the FDI method is based on the state chi-square test. The parameters of the sensors are set in Table 1 and Table 2. The specific verification scheme is shown in

Table 5. The validation scheme of the functional modification strategy.

Validation Scenarios	Road Length	Visibility	Comparison Methods
Quasi-real straight urban expressway	750 m	Case 1: 0.4 km	EKF AEKF EKF with FDI
		Case 2: 0.4–1 km
Quasi-real curved urban expressway	700 m	Case 1: 0.4 km	EKF AEKF EKF with FDI
		Case 2: 0.4–1 km

.

6.2. Analysis of Simulation Results

Case 1: Visibility at 0.4 km

Figure 16 and Figure 17 show the localization performance of the proposed strategy and comparison methods in the quasi-real straight and curved urban expressway scenario at 0.4 km visibility.

As observed from Figure 16a–c, in the quasi-real straight urban expressway scenario, the longitudinal error curve of the EKF diverges rapidly. The longitudinal error curves of the AEKF and EKF with FDI increase gradually, and the performance of the EKF with FDI is superior to that of the AEKF. However, the longitudinal error of the proposed functional modification strategy is always approximately 0 m. Since there is no attitude change of the vehicle in the quasi-real straight urban expressway scenario, the feature degraded point clouds detected by LiDAR can provide constraints for lateral localization. So, the lateral error curves of the four methods are maintained at approximately 0 m without divergence. The advantages of the proposed functional modification strategy in this scenario are mainly reflected in the longitudinal localization. Moreover, the visibility of the current scenario can be recognized by our proposed functional modification strategy.

As observed in Figure 17a–c, the longitudinal and lateral error curves of EKF diverge rapidly in the quasi-real curved urban expressway scenario. The longitudinal and lateral error curves of AEKF and EKF with FDI rise gradually with the increase of simulation time, and the performance of EKF with FDI is better than that of AEKF. However, the longitudinal and lateral errors of the proposed functional modification strategy are always about 0 m. Furthermore, the proposed functional modification strategy can identify the visibility of the current scenario.

The above simulation results show that the proposed strategy can effectively improve the SOTIF of LMSFLS when the fog visibility is low and unchanged, and the performance is better than that of the comparison methods.

Case 2: Visibility at 0.4–1 km

Figure 18 and Figure 19 show the localization performance of the proposed strategy and comparison methods in the quasi-real straight and curved urban expressway scenario at variable visibility. From them, we can find that the variation of error curves of the four methods is similar to that of Case 1. It means that the proposed strategy can also effectively improve the SOTIF of LMSFLS when the fog visibility is low and changeable, and the performance is also better than the comparison methods.

According to the above two cases, the proposed functional modification strategy can recognize the visibility of scenarios and effectively suppress the divergence of the localization errors in both quasi-real straight and curved urban expressway scenarios under constant and variable visibility conditions. It improves the SOTIF of the LMSFLS caused by fog and eliminates the SOTIF-related risks. Compared with traditional methods, the proposed method has the advantages of higher localization accuracy and better stability.

According to the localization accuracy requirements for highway autonomous driving proposed in [52], we calculated the 95% accuracy and maximum localization error of the proposed method and three comparative methods under the aforementioned four sub-scenarios. The calculation results are shown in

Table 6. Analysis of localization accuracy of different methods.

Error Types	Requirements		Methods
			EKF	AEKF	EKF with FDI	Proposed Strategy
Longitudinal error (m)	Max	1.40	11.85	0.62	0.31	0.12
	95% accuracy	0.48	8.88	0.51	0.27	0.10
Lateral error (m)	Max	0.57	4.59	0.20	0.13	0.10
	95% accuracy	0.24	4.17	0.17	0.11	0.10

. It is worth noting that the 95% accuracy and maximum localization error corresponding to each method are the comprehensive results of the four sub-scenarios. It can be seen that the proposed strategy and EKF with FDI meet the localization accuracy requirements in a foggy environment, but the proposed strategy improves the longitudinal and lateral accuracy by approximately 61% and 23%, respectively, compared to the EKF with FDI. The longitudinal 95% accuracy of AEKF failed to meet the requirements, and EKF completely failed to achieve the required localization accuracy. This indicates that the proposed strategy has better localization accuracy while meeting the requirements.

7. Conclusions

In this study, the objective is to improve the SOTIF of autonomous vehicles in a foggy environment. Firstly, a layered SOTIF analysis method is proposed to quantitatively analyze the SOTIF-related issues of the LMSFLS. Subsequently, a fog visibility recognition method and a 3σ-VMDG-AEKF are proposed to ensure the localization performance of the LMSFLS. Finally, a virtual simulation platform is constructed to validate the effectiveness of the proposed functional modification strategies.

Some conclusions can be drawn. (1) The proposed layered SOTIF analysis method could identify the quantitative triggering conditions, functional insufficiencies, and SOTIF-related harms of LMSFLS on foggy days and solve the problem that it is difficult to perform quantitative SOTIF analysis on LMSFLS due to weather limitations. (2) The proposed visibility recognition method can identify the visibility of the scenarios to determine whether LiDAR odometry is disturbed by fog, and the proposed 3σ-VMDG-AEKF can effectively identify and isolate abnormal measurement information in LiDAR odometry to ensure the stability of LMSFLS.

In the future, real vehicle experiments will be considered to verify the effectiveness of the proposed functional modification strategy. We plan to integrate HD maps into this localization system and further study the HD map-based multi-sensor fusion localization systems on foggy days.