Maximum Mixture Correntropy Criterion-Based Variational Bayesian Adaptive Kalman Filter for INS/UWB/GNSS-RTK Integrated Positioning

Abstract

The safe operation of unmanned ground vehicles (UGVs) demands fundamental and essential requirements for continuous and reliable positioning performance. Traditional coupled navigation systems, combining the global navigation satellite system (GNSS) with an inertial navigation system (INS), provide continuous, drift-free position estimation. However, challenges like GNSS signal interference and blockage in complex scenarios can significantly degrade system performance. Moreover, ultra-wideband (UWB) technology, known for its high precision, is increasingly used as a complementary system to the GNSS. To tackle these challenges, this paper proposes a novel tightly coupled INS/UWB/GNSS-RTK integrated positioning system framework, leveraging a variational Bayesian adaptive Kalman filter based on the maximum mixture correntropy criterion. This framework is introduced to provide a high-precision and robust navigation solution. By incorporating the maximum mixture correntropy criterion, the system effectively mitigates interference from anomalous measurements. Simultaneously, variational Bayesian estimation is employed to adaptively adjust noise statistical characteristics, thereby enhancing the robustness and accuracy of the integrated system’s state estimation. Furthermore, sensor measurements are tightly integrated with the inertial measurement unit (IMU), facilitating precise positioning even in the presence of interference from multiple signal sources. A series of real-world and simulation experiments were carried out on a UGV to assess the proposed approach’s performance. Experimental results demonstrate that the approach provides superior accuracy and stability in integrated system state estimation, significantly mitigating position drift error caused by uncertainty-induced disturbances. In the presence of non-Gaussian noise disturbances introduced by anomalous measurements, the proposed approach effectively implements error control, demonstrating substantial advantages in positioning accuracy and robustness.

1. Introduction

Accurate and reliable location-based services are essential components of emerging applications such as intelligent transportation systems (ITSs) and the internet of things (IoT) [1,2]. Furthermore, unmanned ground vehicles (UGVs) are extensively employed in both civilian and military applications, such as smart warehousing [3] and emergency rescue [4], owing to their low cost, mobility, and high adaptability. These widespread applications necessitate precise and reliable position estimation, thereby significantly driving the rapid growth in demand for advanced navigation solutions. To accomplish this objective, significant attention has been focused on the integration of multi-source heterogeneous sensors within UGVs. This approach exploits their complementary characteristics and enhances the capability for reliable navigation across various scenarios, particularly in light of the inherent limitations associated with individual sensors [5].

Specifically, the global navigation satellite system (GNSS) delivers global, continuous, and all-weather positioning services, meeting the demands of autonomous navigation systems over large-scale areas. Concurrently, GNSS real-time kinematic (RTK) technology offers centimeter-level precision with rapid convergence in real time, and has increasingly become a critical component in UGV applications [6]. However, in complex scenarios such as urban canyons, underground spaces, or indoor–outdoor transition zones, GNSS positioning performance degrades due to satellite signal blockage or denial. The integration of an inertial navigation system (INS) with the GNSS enhances the device’s positioning capabilities. An INS can deliver high-frequency pose prediction between consecutive time instances, thereby enhancing GNSS reliability and enabling continuous position estimation. Unfortunately, when GNSS signals lose lock, the accuracy of reliable position solution is constrained. Specifically, the integration of a low-cost micro-electronic-mechanical-system (MEMS) inertial measurement unit (IMU) into GNSS processing leads to rapid accumulation of IMU errors. To address this challenge, it is essential to incorporate additional redundant measurements from other sensors in complex scenarios where GNSS measurements are sparse, in order to mitigate or alleviate position errors.

Recently, the extensive adoption of ultra-wideband (UWB) technology in wireless communication has positioned it as a highly attractive complementary solution to GNSS signals. Compared to other wireless communication technologies, UWB technology has gained traction in smart terminals (e.g., Xiaomi and iPhone) due to its high-precision ranging capabilities and moderate cost. UWB, functioning as a carrierless spread-spectrum communication technology, employs non-sinusoidal narrow pulse signals with nanosecond period for data transmission. UWB-based ranging technology provides several advantages, such as low power consumption, high temporal resolution, robust anti-interference capabilities, and effective penetration characteristics. These characteristics ensure centimeter-level ranging accuracy in complex scenarios, thereby delivering superior positioning performance [7]. The integration of a UWB-assisted GNSS/INS combined navigation system provides redundant measurements that enhance the continuity and reliability of positioning accuracy. This approach facilitates seamless and continuous positioning in complex environments, such as indoor and outdoor overlapping areas, as evidenced by relevant research [8]. In scenarios with limited GNSS availability, deploying UWB anchor nodes can substantially enhance system positioning performance. Specifically, when UWB ranging is both available and reliable, it serves to calibrate IMU drift errors, thereby improving overall positioning accuracy. Zhang et al. [9] proposed a UWB/INS loosely coupled (LC) algorithm that fuses the joint state particle filter based on UWB distance measurement with the INS-based zero-speed update algorithm, resulting in higher accuracy and better adaptability. Li et al. [10] introduced a UWB/INS tightly coupled (TC) integrated method that combines a ranging offset calibration model with an enhanced robust cubature Kalman filter, effectively reducing the influence of UWB ranging offsets, NLOS errors, and linearization errors, thereby ensuring precise positioning and attitude estimation. In comparison to LC mode, the TC integrated system generally provides notable advantages in terms of continuous, reliable positioning and improved information utilization, making them more suitable for positioning applications in complex environments.

Kalman filter (KF)-based parameter estimation constitutes a fundamental component of the integrated system. The application of an optimal and robust estimator to the dynamic system’s state estimation can significantly enhance overall system accuracy. The extended Kalman filter (EKF) is extensively used in various applications, with its fundamental principle being the linearization of nonlinear systems through Taylor series expansion, while simultaneously integrating information within the KF framework. However, the optimality of state estimation can be compromised if there is insufficient quality characterization of the estimator, particularly if there is bias in the prior knowledge regarding the noise covariance properties. To resolve this, an adaptive Kalman filter (AKF) is introduced, which adaptively adjusts the estimation strategy by modifying the noise covariance matrices and correcting the Kalman gain. This approach effectively tackles inaccuracies and time-varying characteristics in noise statistics. Han et al. [11] proposed an adaptive estimation approach utilizing Sage–Husa AKF (SHAKF) to address measurement noise. However, SHAKF is susceptible to inaccuracies in estimation noise parameters, which can result in filter divergence. Chen et al. [12] integrated the fading factor into the Kalman filtering framework (FAKF) and applied it to a GNSS/INS combined navigation system to evaluate positioning performance. However, the computational process is relatively cumbersome, as it requires adjusting the predicted error covariance matrix to diminish the weight of measurements from the current epoch. Gao et al. [13] introduced the innovation-based AKF (IAKF) for UWB positioning in unmanned vehicles. Unfortunately, the prerequisite for achieving accurate state estimates with IAKF is the utilization of a relatively large sliding window to ensure reliable estimation of the measurement noise covariance matrix, which diminishes its applicability for rapidly changing measurement noise statistics. Furthermore, the integration of variational Bayesian (VB) estimation into the Kalman filtering framework facilitates accurate estimation of unknown time-varying noise statistics. Huang et al. [14] developed a novel variational Bayesian AKF (VBAKF) to tackle the challenges associated with inaccurate measurement noise and predicted error covariance matrices through variational recursion estimation. Moreover, VBAKF has garnered significant attention for its straightforward implementation and exceptional usefulness in engineering applications. However, the aforementioned AKF exhibits limited robustness against anomalous measurements. When an integrated system encounters anomalous measurements from a specific sensor, it can result in a mismatch between the noise statistical characteristics and prior knowledge, thereby degrading or causing divergence in the accuracy of the coupled system’s positioning. Consequently, researchers have developed various robust filtering techniques to address this issue, such as Huber-based KF and maximum correntropy criterion-based KF (MCCKF). Xu et al. [15] introduced a novel Huber M-estimation Kalman filter to address non-Gaussian noise resulting from outliers, with the objective of achieving optimal estimation. However, the efficacy of the Huber cost function diminishes in the presence of large outliers, which negatively affects estimation accuracy. Liu et al. [16] introduced a fault detection approach using autocorrentropy, employing the maximum correntropy criterion (MCC) as an objective function to mitigate the effect of outliers. Nevertheless, the selection of kernel bandwidth in the MCCKF presents a significant challenge to filtering performance. In conclusion, choosing an appropriate kernel bandwidth is essential for ensuring both filtering accuracy and computational efficiency.

Based upon the above, the principal contributions of this paper are highlighted:

(1)

A TC INS/UWB/GNSS-RTK integrated positioning system framework is proposed, which integrates raw IMU, UWB, and GNSS measurements at the raw measurement level, significantly improving the information utilization of the proposed system. Furthermore, the integration of the height constraint model further enhances the performance of the system, rendering it an effective solution for seamless positioning in indoor–outdoor transition scenarios.

(2)

A VBAKF based on the maximum mixture correntropy criterion (MMCC) is proposed, designed to fully exploit the potential of position estimation performance in the integrated system. The proposed approach utilizes VB estimation to adaptively adjust the unknown time-varying predicted error associated with measurement noise covariance matrices. Furthermore, it incorporates with the MMCC to enhance the system’s robustness and applicability under the condition of sensor anomalies.

(3)

The performance of the proposed method is validated through real-world and simulation experiments, accompanied by a comprehensive discussion and analysis of the experimental results. In addition, a detailed comparison of positioning accuracy against other algorithms is performed.

The organization of this paper is outlined as follows. Section 2 provides comprehensive reviews of the mathematical models related to the sensors, specifically focusing on the INS error model, the UWB measurement model, and the GNSS-RTK measurement model. The detailed description of the proposed TC INS/UWB/GNSS-RTK integrated positioning system is introduced in Section 3. Subsequently, Section 4 introduces a VBAKF that integrates the MMCC for state estimation in the integrated system. Simultaneously, an overview of the proposed system and its processing flow are shown. In Section 5, we conduct a series of real-world and simulation experiments designed to validate the approach proposed above. The experimental findings are discussed, with a particular emphasis on comparisons with other algorithms. Finally, Section 6 presents the conclusions.

2. Sensor Model

2.1. INS Error Model

The INS serves as a highly reliable independent navigation system, offering an autonomous navigation and positioning solution without reliance on external information or interaction with the surrounding environment [17]. Simultaneously, the primary source of errors in the INS stems from the core sensor unit, the IMU. A standard IMU typically contains three-axis accelerometers and three-axis gyroscopes, which are employed to measure the object’s specific force and angular velocity, to which the IMU is rigidly mounted at high sampling rates. Through the integration of the measured values, the object’s attitude, position, and velocity is derivable, thereby enabling the navigation state and corresponding covariance matrix propagation [18]. Specifically, the raw measurements obtained from the IMU can be characterized as

$${\tilde{\mathsf{\omega }}}_{ib}^b={\mathsf{\omega }}_{ib}^b+{\mathsf{b}}_g+{\epsilon }_g$$

(1)

$${\tilde{\mathsf{f}}}^b={\mathsf{f}}^b+{\mathsf{b}}_a+{\epsilon }_a$$

(2)

where ${\tilde{\mathsf{\omega }}}_{ib}^b$ and ${\mathsf{\omega }}_{ib}^b$ denote the measured associated with true angular velocity in the body coordinate system ($b$-frame), respectively. ${\tilde{\mathsf{f}}}^b$ and ${\mathsf{f}}^b$ denote the measured and true specific force. ${\mathsf{b}}_g$ and ${\mathsf{b}}_a$ denote the gyroscope’s and accelerometer’s bias instability, which can generally be modeled as a first-order Gaussian-Markov process [19]. ${\epsilon }_g$ and ${\epsilon }_a$ represent the measurement noise related to the gyroscope and accelerometer, which is modeled as zero-mean Gaussian white noise. The partial derivatives of the gyroscope and accelerometer biases can be formulated as

$${\dot{\mathsf{b}}}_g=-{\displaystyle \frac{1}{\tau }}{\mathsf{b}}_g+{\epsilon }_{{\mathsf{b}}_g}$$

(3)

$${\dot{\mathsf{b}}}_a=-{\displaystyle \frac{1}{\tau }}{\mathsf{b}}_a+{\epsilon }_{{\mathsf{b}}_a}$$

(4)

where ${\dot{\mathsf{b}}}_g$ and ${\dot{\mathsf{b}}}_a$ represent the time derivatives of the biases associated with the gyroscope and accelerometer, respectively. $\tau$ denotes the correlation time. ${\epsilon }_{{\mathsf{b}}_g}$ and ${\epsilon }_{{\mathsf{b}}_a}$ represent the corresponding random white noise.

The INS state propagation process is facilitated via mechanization, transforming the raw IMU measurements from the gyroscope and accelerometer mounted on the object from the $b$-frame to the desired navigation coordinate system. This conversion is essential for performing integration operation that update the object’s position, velocity, and attitude [20]. Thus, the error dynamics equations of the INS in the local navigation coordinate system ($n$-frame), based on the Psi-angle error model [21], can be expressed as

$$\left[\begin{array}{c}\mathit{\delta }{\dot{\mathsf{r}}}^n\\ \mathit{\delta }{\dot{\mathsf{v}}}^n\\ \dot{\mathsf{\theta }}\end{array}\right]=\left[\begin{array}{c}-{\mathsf{\omega }}_{en}^n\times \mathit{\delta }{\mathsf{r}}^n+\mathit{\delta }{\mathsf{v}}^n\\ -\left(2{\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n\right)\times \mathit{\delta }{\mathsf{v}}^n+C_b^n{\mathsf{f}}^b\times \mathsf{\theta }+C_b^n\mathit{\delta }{\mathsf{f}}^b\\ -\left({\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n\right)\times \mathsf{\theta }-C_b^n\mathit{\delta }{\mathsf{\omega }}_{ib}^b\end{array}\right]$$

(5)

where $\mathit{\delta }{r}^n$, $\mathit{\delta }{\mathsf{v}}^n$, and $\mathsf{\theta }$ denote the position, velocity, and attitude errors in the $n$-frame, respectively. $\mathit{\delta }{\dot{r}}^n$, $\mathit{\delta }{\dot{\mathsf{v}}}^n$, and $\dot{\mathsf{\theta }}$ denote the corresponding derivatives. ${\mathsf{\omega }}_{en}^n$ represents the angular velocity of the $n$-frame with respect to the earth-centered earth-fixed (ECEF) frame ($e$-frame), projected within the $n$-frame. ${\mathsf{\omega }}_{ie}^n$ represents the angular velocity of the $n$-frame with respect to the inertial coordinate system ($i$-frame), projected within the $n$-frame, which corresponds to the earth’s rotational angular velocity. $C_b^n$ denotes the direction cosine matrix (DCM) for transforming the $b$-frame into the $n$-frame. The symbol $\left(\times \right)$ indicates the cross-product operation. ${\mathsf{f}}^b$ indicates the accelerometer outputs within the $b$-frame. $\mathit{\delta }{\mathsf{f}}^b$ and $\mathit{\delta }{\mathsf{\omega }}_{ib}^b$ indicate the summation errors associated with the accelerometer’s and gyroscope’s outputs, respectively [22], which can be mathematically formulated:

$$\mathit{\delta }{\mathsf{f}}^b={\mathsf{b}}_a+{\epsilon }_a$$

(6)

$$\mathit{\delta }{\mathsf{\omega }}_{ib}^b={\mathsf{b}}_g+{\epsilon }_g$$

(7)

where ${\epsilon }_a$ and ${\epsilon }_g$ denote the measurement noise associated with the accelerometer and gyroscope, respectively.

2.2. UWB Measurement Model

UWB technology, recognized as a form of broadband wireless communication, employs nanosecond-level non-sinusoidal narrow pulse signals for data transmission [23]. The implementation of nanosecond-level time resolution for inter-device clock synchronization allows for accurate measurement of time of flight (TOF) between two devices, thereby enabling precise estimation of the distance between them. Additionally, the Cramer–Rao lower bound (CRLB) for TOF measurement estimation is inversely related to the effective signal bandwidth squared [24]. Therefore, UWB technology delivers high-precision ranging measurements.

UWB positioning techniques primarily encompass several methods, such as time of arrival (TOA) [25], time difference of arrival (TDOA) [26], angle of arrival (AOA) [27], and received signal strength indicator (RSSI) [28]. Notably, the TOA and TDOA methods impose stringent requirements on the clock synchronization between anchor nodes, which is critical for accurate ranging estimation. Furthermore, the AOA method requires that anchor nodes be equipped with multiple receiving antennas, significantly increasing hardware complexity and thereby constraining its practical applicability in various scenarios. To mitigate the challenges associated with clock synchronization between anchor nodes, the two-way ranging (TWR) method emphasizes the measurement of time intervals instead of absolute time stamps. This approach significantly reduces the dependence on precise clock synchronization, enhancing the method’s robustness and applicability. Furthermore, the IEEE 802.15.4/4z amendment introduces advanced TWR techniques [29], including single-sided two-way ranging (SS-TWR) [30] and double-sided two-way ranging (DS-TWR) [31], which are designed to improve the robustness and precision of UWB technology. The first method estimates TOF by exchanging two data packets between anchor nodes, resulting in low latency but limited accuracy. In contrast, the second method enhances ranging accuracy by incorporating additional data packet exchanges, thereby mitigating the impact of clock drift. This improvement comes at the cost of increased latency but yields significantly higher precision. In this study, the DS-TWR method is utilized for UWB ranging. Figure 1 depicts the message exchange process involved in the DS-TWR.

The derivation of the DS-TWR ranging calculation is presented:

(1)

Node A transmits a request message to node B while recording the transmission timestamp $T_0$ (referred to as the ranging marker).

(2)

Node B receives the request message and records the corresponding reception timestamp $T_1$. Concurrently, node B sends a response message to node A while recording its transmission timestamp $T_2$. The time interval between the recorded $T_1$ and $T_2$ at node B is denoted as $T_{replyB}$.

(3)

Node A receives the response message and records the corresponding reception timestamp $T_3$. The time interval between the recorded $T_0$ and $T_3$ at node A is represented as $T_{roundA}$. Subsequently, node A transmits a termination message to node B and records the associated transmission timestamp $T_4$. The time interval between $T_3$ and $T_4$ is denoted as $T_{replyA}$.

(4)

Node B receives the termination message and records the corresponding reception timestamp $T_5$. The time interval between $T_2$ and $T_5$ is represented as $T_{roundB}$.

Thus, the TOF $T_P$ between node A and node B can be derived, calculated as follows:

$$T_p={\displaystyle \frac{T_{roundA}\times T_{roundB}-T_{replyA}\times T_{replyB}}{T_{roundA}+T_{replyA}+T_{roundB}+T_{replyB}}}$$

(8)

Then, the ranging measurement $d$ between node A and node B can be expressed as

$$d=c·T_p$$

(9)

where $c$ is the speed of light.

After acquiring the DS-TWR measurements, a UWB positioning system typically relies on pre-defined anchor nodes with known locations and estimated ranging measurements to determine the unknown tag’s position. Figure 2 depicts the schematic diagram for the multilateration positioning method to determine the tag’s position.

The relationship between the anchor nodes and the tag can be formulated as

$$\left\{\begin{array}{c}{r}_1=\sqrt{{\left(x_1-x\right)}^2+{\left(y_1-y\right)}^2+{\left(z_1-z\right)}^2}\\ r_2=\sqrt{{\left(x_2-x\right)}^2+{\left(y_2-y\right)}^2+{\left(z_2-z\right)}^2}\\ ⋮\\ r_L=\sqrt{{\left(x_L-x\right)}^2+{\left(y_L-y\right)}^2+{\left(z_L-z\right)}^2}\end{array}\right.$$

(10)

where $\left(x,y,z\right)$ denotes the unknown position of the tag. $\left(x_l,y_l,z_l\right)\left(l=1,2,\cdots ,L\right)$ represents the known coordinate pertaining to the $l$-th anchor node. $r_l\left(l=1,2,\cdots ,L\right)$ denotes the ranging measurement of the tag relative to the $l$-th anchor node.

Taking into account the noise introduced during UWB signal propagation and measurement, the measurement model between the anchor nodes and the tag is established:

$$r_l=d_l+{\epsilon }_l$$

(11)

where $d_l$ represents the ground truth value of the distance between the $l$-th anchor node and the tag. ${\epsilon }_l$ denotes the corresponding measurement noise.

2.3. GNSS-RTK Measurement Model

Raw GNSS measurements consist of several key components, including code pseudorange, carrier phase, signal-to-noise ratio (SNR), and particularly, the undifferenced pseudorange and carrier phase incorporate parameters such as the receiver’s position, clock offset, and diverse error terms [32]. Therefore, the undifferenced measurement equations [33] are written as

$$\mathsf{P}=\mathrm{\rho }+c\left(d{\mathsf{t}}_r-d{\mathsf{t}}^s\right)+\mathsf{I}+\mathsf{T}+{\epsilon }_{\mathsf{P}}$$

(12)

$$\mathsf{L}=\mathrm{\rho }+c\left(d{\mathsf{t}}_r-d{\mathsf{t}}^s\right)-\mathsf{I}+\mathsf{T}+{\lambda }_f\mathsf{N}+{\epsilon }_{\mathsf{L}}$$

(13)

where $\mathsf{P}$ denotes the code pseudorange measurements. $\mathrm{\rho }$ indicates the geometric range separating the satellite $s$ form receiver. $d{\mathsf{t}}_r$ and $d{\mathsf{t}}^s$ represent the clock offset of the receiver and satellite, respectively. $\mathsf{I}$ indicates the ionospheric delay error. $\mathsf{T}$ is the tropospheric delay error. $\mathsf{L}$ represents the carrier phase measurements. ${\lambda }_f$ denotes the carrier wavelength of frequency $f$. $\mathsf{N}$ represents the carrier phase integer ambiguity. ${\epsilon }_{\mathsf{P}}$ and ${\epsilon }_{\mathsf{L}}$ represent the measurement noise associated with pseudorange and carrier phase.

For effective GNSS-RTK processing, it is essential to have reference stations with accurately known locations as a prerequisite. The single difference (SD) measurement equations for the rover and reference receivers [34] can be expressed as

$${\mathsf{P}}_{f,br}^s={\mathrm{\rho }}_{br}^s+c\left(d{\mathsf{t}}_{br}-d{\mathsf{t}}_{f,br}^s\right)+{\mathsf{I}}_{br}^s+{\mathsf{T}}_{br}^s+{\epsilon }_{\mathsf{P},br}^s$$

(14)

$${\mathsf{L}}_{f,br}^s={\mathrm{\rho }}_{br}^s+c\left(d{\mathsf{t}}_{br}-d{\mathsf{t}}_{f,br}^s\right)-{\mathsf{I}}_{br}^s+{\mathsf{T}}_{br}^s+{\lambda }_f{\mathsf{N}}_{f,br}^s+{\epsilon }_{\mathsf{L},br}^s$$

(15)

where ${\left(\cdot \right)}_{br}={\left(\cdot \right)}_r-{\left(\cdot \right)}_b$.

The fundamental concept of GNSS-RTK involves performing double difference (DD) operation on the measurements between the satellite and the receiver, thereby eliminating or significantly mitigating most errors [35]. It is noteworthy that for short- and medium-baseline (below 30 km) RTK, the clock offset and atmospheric delays in the DD measurement equation can be considered negligible [36]. In this case, the parameters requiring estimation in the DD measurement equation include the integer ambiguity and rover position. Therefore, the single-epoch DD measurement equations for GNSS-RTK [37] are modeled as

$$∆\nabla {\mathsf{P}}_{f,br}^{s_{i}s}=∆\nabla {\mathrm{\rho }}_{f,br}^{s_{i}s}+∆\nabla {\epsilon }_{\mathsf{P},br}^{s_{i}s}$$

(16)

$$∆\nabla {\mathsf{L}}_{f,br}^{s_{i}s}=∆\nabla {\mathrm{\rho }}_{f,br}^{s_{i}s}+{\lambda }_f∆\nabla {\mathsf{N}}_{f,br}^{s_{i}s}+∆\nabla {\epsilon }_{\mathsf{L},br}^{s_{i}s}$$

(17)

where $∆\nabla$ is the DD operator. $s_{\mathsf{I}}$ denotes the reference satellite. $∆\nabla {\mathsf{P}}_{f,br}^{s_{i}s}$ represents the code pseudorange DD measurements. $∆\nabla {\mathsf{L}}_{f,br}^{s_{i}s}$ denotes the carrier phase DD measurements. $∆\nabla {\mathrm{\rho }}_{f,br}^{s_{i}s}$ indicates the DD geometric range with respect to the receiver and satellite. $∆\nabla {\mathsf{N}}_{f,br}^{s_{i}s}$ denotes the DD integer ambiguity. $∆\nabla {\epsilon }_{\mathsf{P},br}^{s_{i}s}$ and $∆\nabla {\epsilon }_{\mathsf{L},br}^{s_{i}s}$ represent the DD pseudorange and carrier phase measurement noise, respectively.

3. Theoretical Analysis of TC INS/UWB/GNSS-RTK Integrated Positioning Model

3.1. State Model

In this study, a TC INS/UWB/GNSS-RTK integrated positioning system is proposed to address the challenging task of seamless indoor and outdoor positioning for low-cost UGVs. The INS drives the mechanization process and is utilized to propagate the integrated system state vector, while single-epoch GNSS and UWB measurements are employed to correct the INS drift errors [38]. The raw GNSS measurements, including code and carrier phase pseudoranges, are utilized in GNSS-RTK to mitigate the impact of most error sources, such as ionospheric and tropospheric delay errors. Thus, the integrated system state vector contains not only the parameters of the INS error model but also the relevant parameters of the GNSS-RTK model. Meanwhile, the UWB measurement model’s state information is incorporated into the INS error model. The coupled system’s state vector comprises two components: the INS and GNSS states, which can be represented as

$${\mathsf{x}}_{sys}={\left[\begin{array}{cc}{\mathsf{x}}_{ins}^T& {\mathsf{x}}_{gnss}^T\end{array}\right]}^T$$

(18)

$${\mathsf{x}}_{ins}={\left[\begin{array}{ccccc}{\left(\mathit{\delta }{\mathsf{r}}^n\right)}^T& {\left(\mathit{\delta }{\mathsf{v}}^n\right)}^T& {\left(\mathsf{\theta }\right)}^T& {\left({\mathsf{b}}_g\right)}^T& {\left({\mathsf{b}}_a\right)}^T\end{array}\right]}^T$$

(19)

$${\mathsf{x}}_{gnss}={\left[{\left(∆\nabla \mathsf{N}\right)}^T\right]}^T$$

(20)

Furthermore, to unify the reference points of different sensor units, the position errors of the GNSS and UWB sensors are uniformly transformed into IMU position errors based on extrinsic perturbation, thereby mitigating the impact of the lever-arm effect.

In practical applications, this is typically modeled as a linear continuous-time stochastic system [39], which can be expressed as

$$\dot{\mathsf{X}}\left(\mathsf{t}\right)=\mathsf{F}\left(\mathsf{t}\right)\mathsf{X}\left(\mathsf{t}\right)+\mathsf{W}\left(\mathsf{t}\right)$$

(21)

where $\mathsf{F}\left(\mathsf{t}\right)$ indicates the time-varying state transition matrix. $\mathsf{X}\left(\mathsf{t}\right)$ denotes the continuous-time system state vector. $\mathsf{W}\left(\mathsf{t}\right)$ represents the stochastic system noise.

By combining the Equations (3)–(7), $\mathsf{F}\left(\mathsf{t}\right)$ can be propagated, and expressed as

$$\mathsf{F}\left(\mathsf{t}\right)=\left[\begin{array}{cccccc}-{\mathsf{\omega }}_{en}^n\times & I_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times \mathsf{J}}\\ 0_{3\times 3}& -\left(2{\mathsf{\omega }}_{ie}^n+{\mathsf{\omega }}_{en}^n\right)\times & C_b^n{\mathsf{f}}^b\times & 0_{3\times 3}& C_b^n& 0_{3\times \mathsf{J}}\\ 0_{3\times 3}& 0_{3\times 3}& -\left({\mathsf{\omega }}_{Ie}^n+{\mathsf{\omega }}_{en}^n\right)\times & -C_b^n& 0_{3\times 3}& 0_{3\times \mathsf{J}}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -{\displaystyle \frac{1}{\tau }}{I}_{3\times 3}& 0_{3\times 3}& 0_{3\times \mathsf{J}}\\ 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& 0_{3\times 3}& -{\displaystyle \frac{1}{\tau }}{I}_{3\times 3}& 0_{3\times \mathsf{J}}\\ 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times \mathsf{J}}\end{array}\right]$$

(22)

where $\mathsf{J}$ is the number of DD integer ambiguity.

In the integrated system state estimation, Equation (21) is discretized utilizing linearization theory [40]:

$${\mathsf{x}}_{\mathsf{k}}={\mathsf{\Phi }}_{\mathsf{k}|\mathsf{k}-1}{\mathsf{x}}_{\mathsf{k}-1}+{\mathsf{w}}_{\mathsf{k}-1}$$

(23)

where ${\mathsf{x}}_{\mathsf{k}}\in {ℝ}^{\mathrm{n}}$ indicates the coupled system’s discretized state vector. ${\mathsf{w}}_{\mathsf{k}-1}$ denotes the linear transformation of $\mathsf{W}\left(\mathsf{t}\right)$, with the covariance matrix given by $Q_{\mathsf{k}-1}$ [7]. ${\mathsf{\Phi }}_{\mathsf{k}|\mathsf{k}-1}$ indicates the discretized state transition matrix [41]:

$${\mathsf{\Phi }}_{\mathsf{k}|\mathsf{k}-1}=\mathsf{e}\mathsf{x}\mathsf{p}\left(\mathsf{F}\left(\mathsf{t}\right)·∆\mathsf{t}\right)\approx I+\mathsf{F}\left(\mathsf{t}\right)·∆\mathsf{t}$$

(24)

where $∆\mathsf{t}$ indicates the time interval, determined by the IMU output rate.

3.2. Measurement Model

In the TC INS/UWB/GNSS-RTK integrated positioning system, the measurement model consists of the DD measurement between the raw GNSS measurements and the zero-order term in the linearized INS-predicted distances, as well as the difference measurement between the UWB measurements and INS-predicted distances. Thus, the measurement model for the integrated system can be represented as

$${\mathsf{z}}_{\mathsf{k}}={\mathsf{H}}_{\mathsf{k}}{\mathsf{x}}_{\mathsf{k}}+{\mathsf{v}}_{\mathsf{k}}$$

(25)

where ${\mathsf{z}}_{\mathsf{k}}\in {ℝ}^{\mathsf{m}}$ denotes the measurements of the integrated system, which can be described as

$${\mathsf{z}}_{\mathsf{k}}=\left[\begin{array}{c}∆\nabla \mathsf{P}-∆\nabla {\mathrm{\rho }}_{ins}\\ ∆\nabla \mathsf{L}-∆\nabla {\mathrm{\rho }}_{ins}\\ r-r_{ins}\end{array}\right]$$

(26)

where ${\mathrm{\rho }}_{ins}$ represents linearized zero-order term of the INS inertial propagation. $r_{ins}$ denotes the distance predicted by the INS.

${\mathsf{H}}_{\mathsf{k}}$ indicates the measurement design matrix, which can be described as

$${\mathsf{H}}_{\mathsf{k}}=\left[\begin{array}{cccccc}∆\nabla \mathsf{y}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times \mathsf{J}}\\ ∆\nabla \mathsf{y}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& 0_{\mathsf{J}\times 3}& {\lambda }_f{I}_{\mathsf{J}\times \mathsf{J}}\\ \mathsf{g}& 0_{L\times 3}& 0_{L\times 3}& 0_{L\times 3}& 0_{L\times 3}& 0_{L\times \mathsf{J}}\end{array}\right]$$

(27)

where $\mathsf{y}$ represents the direction cosine vector connecting all received GNSS satellites to the receiver. $\mathsf{g}$ indicates the direction cosine vector from the UWB tag to each anchor node. It is noteworthy that the lever-arm errors connecting the IMU to UWB and GNSS have been corrected to alleviate the effect of extrinsic perturbation.

Additionally, ${\mathsf{v}}_{\mathsf{k}}$ denotes the measurement noise, assumed to have zero-mean and a covariance matrix ${\mathsf{R}}_{\mathsf{k}}$ [42].

It is evident that the integrated system effectively achieves tight coupling of all the raw measurements from GNSS and UWB sensors in an equivalent manner. Simultaneously, in practical applications, the raw measurements from different sensors are point-to-point, fully utilizing all available effective measurements from various terminals to improve the overall interference resistance of the integrated system [43]. Furthermore, a large amount of redundant measurements are synchronized and fused at the same epoch, enabling the integrated system to more effectively handle anomalous measurements and sensor failures.

3.3. Height Constraint-Aided Model

To improve the coupled system’s performance, a non-holonomic constraint (NHC) model of the carrier is incorporated into the filtering process of the system state estimation. Specifically, NHC-assisted conditions are commonly referred to as pseudo-observations [44]. In the context of wheeled vehicles operating on land vehicle applications, the following primary assumptions can be employed to establish the NHC-assisted conditions for wheeled vehicles:

(i)

When wheeled vehicles drive on flat surfaces, they maintain close contact with the ground while moving forward, without any instances of experiencing jump or sideslip within a specified time period [45].

(ii)

When vehicles drive in relatively flat regions, the changes in road surface terrain over a given period remain nearly at the same elevation, with only minor variation in height [46].

Thus, for the physical motion characteristics of wheeled vehicles in suitable scenarios, prior constraints on height variation can be applied to restrict the height estimation drift. In this study, we utilize a UGV as the mobile platform for a multi-sensor suite. The height constraint model incorporates external height information (typically measured by a sensor such as barometer) as a reference to constrain the estimated height of the UGV, thereby enhancing the estimation accuracy of the integrated system. Simultaneously, integrating the height constraint model can facilitate the mitigation of inaccuracies in height estimation resulting from the layout of anchor nodes [47].

In conclusion, the height constraint model can be formulated as

$$\begin{array}{c}{\mathsf{z}}_{nhc}=\left[{\mathsf{h}}_{est}^n-{\mathsf{h}}_{ref}\right]\hfill \\ =\left[\begin{array}{cccccccc}0& 0& 1& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times 3}& 0_{1\times J}\end{array}\right]{\mathsf{x}}_{\mathsf{k}}+{\epsilon }_{nhc}\end{array}$$

(28)

where ${\mathsf{z}}_{nhc}$ denotes the misclosure of the height constraint model, defined as the deviation from the external reference ${\mathsf{h}}_{ref}$ to the estimated height ${\mathsf{h}}_{est}^n$. ${\epsilon }_{nhc}$ represents the measurement noise associated with the height constraint model.

4. Mathematical Model of VBAKF Under MMCC

4.1. Generalized MMCC

Given two random variables, $\mathsf{Y},\mathrm{\Psi }\in ℝ$, the correlation between them can be quantified using the concept of correntropy. The correntropy of variables $\mathsf{Y}$ and $\mathrm{\Psi }$ [48] can be defined as

$$\mathsf{V}\left(\mathsf{Y},\mathrm{\Psi }\right)=\mathsf{E}\left[\kappa \left(\mathsf{Y},\mathrm{\Psi }\right)\right]={\displaystyle \int \kappa \left(y,\mathrm{\psi }\right)}d{F}_{\mathsf{Y}\mathrm{\Psi }}\left(y,\mathrm{\psi }\right)$$

(29)

where $\mathsf{E}\left[\cdot \right]$ serves as the expectation operator. $\kappa \left(\cdot ,\cdot \right)$ indicates a kernel that follows to the shift-invariant under Mercer’s theory. $F_{\mathsf{Y}\mathrm{\Psi }}\left(y,\mathrm{\psi }\right)$ represents the joint distribution for variables $\mathsf{Y}$ and $\mathrm{\Psi }$. Specifically, Gaussian kernels are commonly selected as the kernel functions [49]:

$$\kappa \left(y,\mathrm{\psi }\right)=G_{\sigma }\left(y-\mathrm{\psi }\right)={\displaystyle \frac{1}{\sqrt{2\pi }\sigma }}\mathsf{e}\mathsf{x}\mathsf{p}\left(-{\displaystyle \frac{{\left(y-\mathrm{\psi }\right)}^2}{2{\sigma }^2}}\right)$$

(30)

where $\sigma >0$ represents the Gaussian kernel bandwidth.

In practical applications, the true $F_{\mathsf{Y}\mathrm{\Psi }}\left(y,\mathrm{\psi }\right)$ of the given random variables is typically unknown. To address the above issue, a limited amount of available data is generally utilized to estimate the correntropy defined by Equation (29) through the sample mean estimator [50], which can be estimated as

$$\widehat{\mathsf{V}}\left(\mathsf{Y},\mathrm{\Psi }\right)={\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle {\sum }_{\zeta =1}^{\mathrm{N}}{G}_{\sigma }\left(y\left(\zeta \right)-\mathrm{\psi }\left(\zeta \right)\right)}$$

(31)

where ${\left\{y\left(\zeta \right),\mathrm{\psi }\left(\zeta \right)\right\}}_{\zeta =1}^{\mathrm{N}}$ denotes the $\mathrm{N}$ samples drawn from $F_{\mathsf{Y}\mathrm{\Psi }}\left(y,\mathrm{\psi }\right)$.

The Gaussian kernel in Equation (31) can be expanded taking the Taylor series [51] as

$$\widehat{\mathsf{V}}\left(\mathsf{Y},\mathrm{\Psi }\right)={\displaystyle {\sum }_{\upsilon =0}^{\infty }\frac{{\left(-1\right)}^{\upsilon }}{{\left(2{\sigma }^2\right)}^{\upsilon }\upsilon \mathbf{!}}}\mathsf{E}\left[{\left(\mathsf{Y}-\mathrm{\Psi }\right)}^{2\upsilon }\right]$$

(32)

As indicated by Equation (32), the correntropy of the difference between the two given random variables is expressed as a weighted combination of their even-order moments. A higher correntropy indicates greater similarity between the two variables. Thus, the MCC can be employed as an optimization criterion to enhance the robustness of the estimated state.

The cost function, based on the MCC, can be defined as

$$J_{MCC}={\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle {\sum }_{\zeta =1}^{\mathrm{N}}{G}_{\sigma }\left(y\left(\zeta \right)-\mathrm{\psi }\left(\zeta \right)\right)}$$

(33)

The optimal solution of Equation (33) is given by $\widehat{\mathsf{W}}$:

$$\widehat{\mathsf{W}}=\underset{\mathsf{W}\in \mathrm{\Xi }}{\mathsf{a}\mathsf{r}\mathsf{g}\mathsf{m}\mathsf{a}\mathsf{x}}{\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle {\sum }_{\zeta =1}^{\mathrm{N}}{G}_{\sigma }\left(y\left(\zeta \right)-\mathrm{\psi }\left(\zeta \right)\right)}$$

(34)

where $\mathrm{\Xi }$ is the set of all feasible solutions.

The kernel bandwidth, as an essential tunable parameter in the kernel function, is pivotal for suppressing outliers and enhancing robustness against outliers. When anomalous measurements are present, a smaller kernel bandwidth can be set to reduce the weight contribution of the outliers, thereby mitigating the adverse effect as much as possible. Conversely, for normal measurements, a larger kernel bandwidth can be retained to achieve accurate state estimation [52]. Therefore, in this study, the MMCC is employed to improve estimation precision and strengthen robustness in state calculations.

Based on MCC above, the MMCC can be defined as

$$\mathsf{M}\left(\mathsf{Y},\mathrm{\Psi }\right)=\mathsf{E}\left[\vartheta {\kappa }_1\left(\mathsf{Y},\mathrm{\Psi }\right)+\left(1-\vartheta \right){\kappa }_2\left(\mathsf{Y},\mathrm{\Psi }\right)\right]$$

(35)

where $\vartheta$ represents the coefficient. ${\kappa }_1\left(\cdot ,\cdot \right)$ and ${\kappa }_2\left(\cdot ,\cdot \right)$ denote the kernel functions with different kernel bandwidths, respectively. Similarly, the corresponding cost function based on the MMCC is given as

$$J_{MMCC}={\displaystyle \frac{1}{\mathrm{N}}}{\displaystyle {\sum }_{\zeta =1}^{\mathrm{N}}\left[\vartheta G_{{\sigma }_1}\left(y\left(\zeta \right)-\mathrm{\psi }\left(\zeta \right)\right)+\left(1-\vartheta \right)G_{{\sigma }_2}\left(y\left(\zeta \right)-\mathrm{\psi }\left(\zeta \right)\right)\right]}$$

(36)

4.2. Derived MMCC-Based VBAKF

When applying the EKF for the TC system’s state estimation, the one-step prediction $\mathsf{p}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)$, as well as the measurement likelihood $\mathsf{p}\left({\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)$, follow the probability density function (PDF) as follows:

$$\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right.\right)=\mathrm{N}\left({\mathsf{x}}_{\mathsf{k}};{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)$$

(37)

$$\mathsf{p}\left({\mathsf{z}}_{\mathsf{k}}\left|{\mathsf{x}}_{\mathsf{k}},{\mathsf{R}}_{\mathsf{k}}\right.\right)=\mathrm{N}\left({\mathsf{z}}_{\mathsf{k}};\mathsf{h}\left({\mathsf{x}}_{\mathsf{k}}\right),{\mathsf{R}}_{\mathsf{k}}\right)$$

(38)

where ${\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}$ denotes the one-step state prediction. ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$ indicates the corresponding predicted error covariance matrix. $h\left(\cdot \right)$ is the nonlinear measurement function. $\mathsf{N}\left(\cdot ;\mu ,\mathsf{\Sigma }\right)$ denotes the PDF that follows a Gaussian distribution with mean $\mu$ and covariance matrix $\mathsf{\Sigma }$.

According to the VB approach, the true joint posterior PDF is factorized and approximated as the product of simpler conditional PDFs, enabling efficient recursive estimation [53]. In Bayesian statistics, the conjugate exponential family is frequently employed as a tool function to accommodate scenarios where the time-varying and unknown noise covariance matrix adheres to a multivariate normal distribution. Specifically, it is assumed that the covariance matrix $\mathsf{\Sigma }$ in the EKF follows a Gaussian distribution with a known mean $\mu$, and the inverse Wishart ($\mathsf{I}\mathsf{W}$) distribution serves as the conjugate prior distribution function [54]. Consider a $\beta \times \beta$-dimensional symmetric positive definite random matrix $\mathsf{B}$, for which the $\mathsf{I}\mathsf{W}$ PDF can be defined as

$$\mathsf{I}\mathsf{W}\left(\mathsf{B};\mathsf{\lambda },\mathcal{l}\right)={\left|\mathcal{l}\right|}^{{\displaystyle \frac{\mathrm{\lambda }}{2}}}{\left|\mathsf{B}\right|}^{-{\displaystyle \frac{\left(\mathrm{\lambda }+\beta +1\right)}{2}}}{\displaystyle \frac{\mathsf{e}\mathsf{x}\mathsf{p}\left\{-0.5\mathsf{t}\mathsf{r}\left(\mathcal{l}{\mathsf{B}}^{-1}\right)\right\}}{2^{\beta \mathrm{\lambda }/2}{\mathrm{\Gamma }}_{\beta }\left(\mathsf{\lambda }/2\right)}}$$

(39)

where $\mathsf{\lambda }$ and $\mathcal{l}\in {ℝ}^{\beta \times \beta }$ indicate the degree of freedom parameter associated with the corresponding inverse scale matrix. $\left|\cdot \right|$ is the matrix’s determinant operator. $\mathsf{r}\mathsf{r}\left(\cdot \right)$ indicates the matrix’s trace operator. ${\mathrm{\Gamma }}_{\beta }\left(\cdot \right)$ represents the $\beta$-variate gamma function. Furthermore, if $\mathsf{\lambda }>\beta +1$, the $\mathsf{I}\mathsf{W}$ PDF satisfies $\mathsf{E}\left[{\mathsf{B}}^{-1}\right]=\left(\mathsf{\lambda }-\beta -1\right){\mathcal{l}}^{-1}$ [55].

The $\mathsf{I}\mathsf{W}$ distribution guarantees that the functional forms of the prior and posterior distributions remain consistent [56]. Additionally, ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\mathsf{R}}_{\mathsf{k}}$ represent the covariance matrices corresponding to the Gaussian PDFs. Therefore, the corresponding prior distribution can be modeled as the $\mathsf{I}\mathsf{W}$ PDF:

$$\mathsf{p}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)=\mathsf{I}\mathsf{W}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1};{\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1},{\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}\right)$$

(40)

$$\mathsf{p}\left({\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)=\mathsf{I}\mathsf{W}\left({\mathsf{R}}_{\mathsf{k}};{\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1},{\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}\right)$$

(41)

where ${\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}$ indicate the degree of freedom parameter associated with the corresponding inverse scale matrix of the $\mathsf{I}\mathsf{W}$ PDF $\mathsf{p}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)$, respectively. ${\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}$ correspond to the same parameters for the $\mathsf{I}\mathsf{W}$ PDF $\mathsf{p}\left({\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)$. For ${\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}$, the corresponding prior parameters are defined as follows [57]:

$${\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1}=\mathsf{\tau }+\mathrm{n}+1$$

(42)

$${\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}=\mathsf{\tau }{\tilde{\mathsf{P}}}_{\mathsf{k}|\mathsf{k}-1}$$

(43)

where $\mathsf{\tau }\ge 0$ indicates the adjustable parameter. ${\tilde{\mathsf{P}}}_{\mathsf{k}|\mathsf{k}-1}$ indicates the nominal matrix corresponding to ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$. Additionally, for ${\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}$, updates are carried out by introducing a forgetting factor parameter $\rho \in \left[0,1\right]$:

$${\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1}=\rho \left({\widehat{\mathsf{u}}}_{\mathsf{k}-1}-\mathsf{m}-1\right)+\mathsf{m}+1$$

(44)

$${\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}=\rho {\widehat{\mathsf{U}}}_{\mathsf{k}-1}$$

(45)

Assume that the prior distribution of ${\mathsf{x}}_{\mathsf{k}}$ follows a Gaussian distribution, while ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\mathsf{R}}_{\mathsf{k}}$ follow an $\mathsf{I}\mathsf{W}$ distribution. Therefore, the true joint PDF $\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)$ is expressed as

$$\begin{array}{l}\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)=\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)\mathsf{p}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)\mathsf{p}\left({\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}-1}\right.\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}=\mathsf{N}\left({\mathsf{x}}_{\mathsf{k}};{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)\mathsf{I}\mathsf{W}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1};{\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1},{\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}\right)\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\times \mathsf{I}\mathsf{W}\left({\mathsf{R}}_{\mathsf{k}};{\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1},{\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}\right)\end{array}$$

(46)

The complexity of the true joint posterior PDF $\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{\mathbf{1}:\mathsf{k}}\right.\right)$ precludes obtaining a direct analytical solution. Therefore, the VB approximation is adopted, where the approximate distribution of $\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}}\right.\right)$ is represented as a product of independent factored form [58], defined as

$$\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}}\right.\right)\approx \mathsf{q}\left({\mathsf{x}}_{\mathsf{k}}\right)\mathsf{q}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)\mathsf{q}\left({\mathsf{R}}_{\mathsf{k}}\right)$$

(47)

where $\mathsf{q}\left(\cdot \right)$ indicates the approximate estimated posterior distribution of $\mathsf{p}\left(\cdot \right)$. The VB method performs approximate estimation by minimizing the Kullback–Leibler divergence ($\mathsf{K}\mathsf{L}\mathsf{D}$) [59], formulated as follows:

$$\begin{array}{l}\left\{\mathsf{q}\left({\mathsf{x}}_{\mathsf{k}}\right),\mathsf{q}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right),\mathsf{q}\left({\mathsf{R}}_{\mathsf{k}}\right)\right\}=\mathsf{a}\mathsf{r}\mathsf{g}\mathsf{m}\mathsf{i}\mathsf{n}\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\mathsf{K}\mathsf{L}\mathsf{D}\left(\mathsf{q}\left({\mathsf{x}}_{\mathsf{k}}\right)\mathsf{q}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)\mathsf{q}\left({\mathsf{R}}_{\mathsf{k}}\right)‖\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}\left|{\mathsf{z}}_{1:\mathsf{k}}\right.\right)\right)\end{array}$$

(48)

Then, the estimated posterior PDF can be expressed as

$$\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{q}\left({\mathsf{x}}_{\mathsf{k}}\right)={\mathsf{E}}_{{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}}\left[\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}|{\mathsf{z}}_{1:\mathsf{k}}\right)\right]+c_{{\mathsf{x}}_{\mathsf{k}}}$$

(49)

$$\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{q}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)={\mathsf{E}}_{{\mathsf{x}}_{\mathsf{k}},{\mathsf{R}}_{\mathsf{k}}}\left[\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}|{\mathsf{z}}_{1:\mathsf{k}}\right)\right]+c_{{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}}$$

(50)

$$\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{q}\left({\mathsf{R}}_{\mathsf{k}}\right)={\mathsf{E}}_{{\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}\mathbf{|}\mathsf{k}-1}}\left[\mathsf{l}\mathsf{o}\mathsf{g}\mathsf{p}\left({\mathsf{x}}_{\mathsf{k}},{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1},{\mathsf{R}}_{\mathsf{k}}|{\mathsf{z}}_{1:\mathsf{k}}\right)\right]+c_{{\mathsf{R}}_{\mathsf{k}}}$$

(51)

where $c_{{\mathsf{x}}_{\mathsf{k}}}$, $c_{{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}}$, and $c_{{\mathsf{R}}_{\mathsf{k}}}$ indicate the constants in the logarithmic equations of ${\mathsf{x}}_{\mathsf{k}}$, ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$, and ${\mathsf{R}}_{\mathsf{k}}$, respectively.

Then, for ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\mathsf{R}}_{\mathsf{k}}$, which follow the $\mathsf{I}\mathsf{W}$ prior distribution, the logarithmic formulations of the VB approximate posterior PDF can be expressed as

$$\begin{array}{l}\mathsf{l}\mathsf{o}\mathsf{g}{\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)\propto -{\displaystyle \frac{1}{2}}\left(\mathrm{n}+{\widehat{\mathsf{t}}}_{\mathsf{k}|\mathsf{k}-1}+2\right)\mathsf{l}\mathsf{o}\mathsf{g}\left|{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right|\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} -{\displaystyle \frac{1}{2}}\mathsf{t}\mathsf{r}\left[\left({\mathsf{A}}_{\mathsf{k}}^{\left(i\right)}+{\widehat{\mathsf{T}}}_{\mathsf{k}|\mathsf{k}-1}\right){\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{-1}\right]+c_{\mathsf{p}}\end{array}$$

(52)

$$\begin{array}{l}\mathsf{l}\mathsf{o}\mathsf{g}{\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{R}}_{\mathsf{k}}\right)\propto -{\displaystyle \frac{1}{2}}\left(\mathsf{m}+{\widehat{\mathsf{u}}}_{\mathsf{k}|\mathsf{k}-1}+2\right)\mathsf{l}\mathsf{o}\mathsf{g}\left|{\mathsf{R}}_{\mathsf{k}}\right|\\ \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em} -{\displaystyle \frac{1}{2}}\mathsf{t}\mathsf{r}\left[\left({\mathsf{B}}_{\mathsf{k}}^{\left(i\right)}+{\widehat{\mathsf{U}}}_{\mathsf{k}|\mathsf{k}-1}\right){\mathsf{R}}_{\mathsf{k}}^{-1}\right]+c_{\mathsf{R}}\end{array}$$

(53)

where ${\mathsf{q}}^{\left(i+1\right)}\left(\cdot \right)$ is the $i+1$-th iteration of $\mathsf{q}\left(\cdot \right)$. ${\mathsf{A}}_{\mathsf{k}}^{\left(i\right)}$ and ${\mathsf{B}}_{\mathsf{k}}^{\left(i\right)}$ can be written as

$${\mathsf{A}}_{\mathsf{k}}^{\left(i\right)}={\mathsf{P}}_{\mathsf{k}}^{\left(i\right)}+\left({\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}-{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}\right){\left({\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}-{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}\right)}^T$$

(54)

$${\mathsf{B}}_{\mathsf{k}}^{\left(i\right)}=\left({\mathsf{z}}_{\mathsf{k}}-{\mathsf{H}}_{\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}\right){\left({\mathsf{z}}_{\mathsf{k}}-{\mathsf{H}}_{\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}\right)}^T+{\mathsf{H}}_{\mathsf{k}}{\mathsf{P}}_{\mathsf{k}}^{\left(i\right)}{\mathsf{H}}_{\mathsf{k}}^T$$

(55)

Meanwhile, ${\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)$ and ${\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{R}}_{\mathsf{k}}\right)$ follow the $\mathsf{I}\mathsf{W}$ distribution, i.e.,

$${\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}\right)=\mathsf{I}\mathsf{W}\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1};{\widehat{\mathsf{t}}}_{\mathsf{k}}^{\left(i+1\right)},{\widehat{\mathsf{T}}}_{\mathsf{k}}^{\left(i+1\right)}\right)$$

(56)

$${\mathsf{q}}^{\left(i+1\right)}\left({\mathsf{R}}_{\mathsf{k}}\right)=\mathsf{I}\mathsf{W}\left({\mathbf{R}}_{\mathbf{k}};{\widehat{\mathsf{u}}}_{\mathbf{k}}^{\left(i+1\right)},{\widehat{\mathsf{U}}}_{\mathbf{k}}^{\left(i+1\right)}\right)$$

(57)

where the prior parameters ${\widehat{\mathsf{t}}}_{\mathbf{k}}^{\left(i+1\right)}$, ${\widehat{\mathsf{T}}}_{\mathbf{k}}^{\left(i+1\right)}$, ${\widehat{\mathsf{u}}}_{\mathbf{k}}^{\left(i+1\right)}$, and ${\widehat{\mathsf{U}}}_{\mathbf{k}}^{\left(i+1\right)}$ are described as

$${\widehat{\mathsf{t}}}_{\mathbf{k}}^{\left(i+1\right)}={\widehat{\mathsf{t}}}_{\mathbf{k}|\mathbf{k}-1}+1$$

(58)

$${\widehat{\mathsf{T}}}_{\mathbf{k}}^{\left(i+1\right)}={\mathbf{A}}_{\mathbf{k}}^{\left(i\right)}+{\widehat{\mathsf{T}}}_{\mathbf{k}|\mathbf{k}-1}$$

(59)

$${\widehat{\mathsf{u}}}_{\mathbf{k}}^{\left(i+1\right)}={\widehat{\mathsf{u}}}_{\mathsf{k}|\mathbf{k}-1}+1$$

(60)

$${\widehat{\mathsf{U}}}_{\mathbf{k}}^{\left(i+1\right)}={\mathsf{B}}_{\mathbf{k}}^{\left(i\right)}+{\widehat{\mathsf{U}}}_{\mathbf{k}|\mathbf{k}-1}$$

(61)

In addition, the update processes for ${\mathsf{P}}_{\mathbf{k}|\mathbf{k}-1}$ and ${\mathbf{R}}_{\mathbf{k}}$ are written as

$${\mathbf{E}}^{\left(i+1\right)}\left[{\mathsf{P}}_{\mathbf{k}|\mathbf{k}-1}^{-1}\right]=\left({\widehat{\mathsf{t}}}_{\mathbf{k}}^{\left(i+1\right)}-\mathrm{n}-1\right){\left({\widehat{\mathsf{T}}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}$$

(62)

$${\mathsf{E}}^{\left(i+1\right)}\left[{\mathsf{R}}_{\mathsf{k}}^{-1}\right]=\left({\widehat{\mathsf{u}}}_{\mathsf{k}}^{\left(i+1\right)}-\mathsf{m}-1\right){\left({\widehat{\mathsf{U}}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}$$

(63)

Through the VB method, it becomes possible to accurately estimate the time-varying and unknown ${\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}$ and ${\mathsf{R}}_{\mathsf{k}}$ during the filtering process, leading to improved instantaneous precision and robustness in the integrated system’s state estimation. It should be noted that in practical applications, measurements of the integrated system may contain outliers that degrade the performance of state estimation. Thus, the MMCC is integrated with VB estimation within the KF framework to achieve robustness against anomalous measurements [60]. In the integrated system, the correntropy metric is constructed using MMCC, and the corresponding cost function is given by

$$\begin{array}{l}{J}_{MMCC}=\left[\vartheta G_{{\sigma }_1}\left({‖{\widehat{\mathsf{x}}}_{\mathsf{k}}-{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}‖}_{{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{-1}}\right)+\left(1-\vartheta \right)G_{{\sigma }_2}\left({‖{\widehat{\mathsf{x}}}_{\mathsf{k}}-{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}‖}_{{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{-1}}\right)\right]\\ +{\displaystyle \frac{1}{\mathsf{m}}}{\displaystyle {\sum }_{\varsigma =1}^{\mathsf{m}}\left[\vartheta G_{{\sigma }_1}\left({‖{\mathsf{z}}_{\varsigma ,\mathsf{k}}-{\mathsf{H}}_{\varsigma ,\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}‖}_{{\mathsf{R}}_{\varsigma ,\mathsf{k}}^{-1}}\right)+\left(1-\vartheta \right)G_{{\sigma }_2}\left({‖{\mathsf{z}}_{\varsigma ,\mathsf{k}}-{\mathsf{H}}_{\varsigma ,\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}‖}_{{\mathsf{R}}_{\varsigma ,\mathsf{k}}^{-1}}\right)\right]}\end{array}$$

(64)

where ${\mathsf{z}}_{\varsigma ,\mathsf{k}}$ indicates the $\varsigma$-th element of ${\mathsf{z}}_{\mathsf{k}}$. ${\mathsf{H}}_{\varsigma ,\mathsf{k}}$ indicates the corresponding $\varsigma$-th row of ${\mathsf{H}}_{\mathsf{k}}$. ${\mathsf{R}}_{\varsigma ,\mathsf{k}}$ denotes the $\varsigma$-th diagonal element of ${\mathsf{R}}_{\mathsf{k}}$. Therefore, the KF gain can be calculated as

$${\mathsf{\Theta }}_{\mathsf{k}}^{\left(i+1\right)}=\vartheta G_{{\sigma }_1}\left({‖{\mathsf{z}}_{\mathsf{k}}-{\mathsf{H}}_{\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}‖}_{{\left({\mathsf{R}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}}\right)+\left(1-\vartheta \right)G_{{\sigma }_2}\left({‖{\mathsf{z}}_{\mathsf{k}}-{\mathsf{H}}_{\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i\right)}‖}_{{\left({\mathsf{R}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}}\right)$$

(65)

$${\mathsf{K}}_{\mathsf{k}}^{\left(i+1\right)}={\left({\left({\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{\left(i+1\right)}\right)}^{-1}+{\mathsf{H}}_{\mathsf{k}}^T{\mathsf{\Theta }}_{\mathsf{k}}^{\left(i+1\right)}{\left({\mathsf{R}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}{\mathsf{H}}_{\mathsf{k}}\right)}^{-1}{\mathsf{H}}_{\mathsf{k}}^T{\mathsf{\Theta }}_{\mathsf{k}}^{\left(i+1\right)}{\left({\mathsf{R}}_{\mathsf{k}}^{\left(i+1\right)}\right)}^{-1}$$

(66)

The $i+1$-th iteration of ${\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i+1\right)}$ and ${\mathsf{P}}_{\mathsf{k}}^{\left(i+1\right)}$ is given by

$${\widehat{\mathsf{x}}}_{\mathsf{k}}^{\left(i+1\right)}={\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}+{\mathsf{K}}_{\mathsf{k}}^{\left(i+1\right)}\left({\mathsf{z}}_{\mathsf{k}}-{\mathsf{H}}_{\mathsf{k}}{\widehat{\mathsf{x}}}_{\mathsf{k}|\mathsf{k}-1}\right)$$

(67)

$${\mathsf{P}}_{\mathsf{k}}^{\left(i+1\right)}={\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{\left(i+1\right)}-{\mathsf{K}}_{\mathsf{k}}^{\left(i+1\right)}{\mathsf{H}}_{\mathsf{k}}{\mathsf{P}}_{\mathsf{k}|\mathsf{k}-1}^{\left(i+1\right)}$$

(68)

In conclusion, the proposed MMCC-based VBAKF sequentially performs the time update, variational measurement update, and MMCC algorithm. An overview of its application to the integrated system is illustrated in Figure 3, with the filtering algorithm flowchart provided in Figure 4.

5. Experimental Evaluation and Analysis

5.1. Equipment Setup and Experiment Description

We conducted a series of real-world and simulation experiments to comprehensively quantify the effectiveness of the MMCC-based VBAKF in the TC INS/UWB/GNSS-RTK integrated positioning system. Furthermore, the proposed approach’s effectiveness was validated through comparisons with existing filtering algorithms. The evaluation utilized performance metrics, including mean absolute error (MAE), standard deviation (STD), and root mean square error (RMSE), calculated relative to the reference trajectory, to ensure a thorough performance comparison of the integrated system. A UGV-based platform was constructed, and relevant data collection was conducted on the campus of Shandong University, Weihai. The platform was equipped with a multi-sensor suite that included multiple GNSS receiver (NovAtel PwrPak7, Calgary, AB, Canada), a MEMS IMU sensor (Epson EG320N IMU, Epson, Suwa, Japan), and a low-cost UWB device based on the Decawave DW1000 chip (Apopka, FL, USA). The performance parameters of the MEMS IMU are given in

Table 1. MEMS IMU performance parameters.

Item	Gyroscope	Accelerometer
Bias stability	${0.8}^{\circ }/\mathrm{h}$	$0.01 \mathrm{mg}$
Measurement range	$\pm {450}^{\circ }/\mathrm{s}$	$\pm 10 \mathrm{g}$
Sampling rate	200 Hz	200 Hz
Angular random walk	${0.06}^{\circ }/\sqrt{\mathrm{h}}$	-
Velocity random walk	-	$0.025 \mathrm{m}/\mathrm{s}/\sqrt{\mathrm{h}}$

. The raw measurements obtained from the GNSS receiver, IMU, and UWB sensors were sampled at rates of 1, 200, and 1 Hz, respectively. Each sensor was meticulously calibrated and time-synchronized to ensure data reliability and precision during collection. In addition, in this study, the reference trajectory was derived using a smoothed TC multi-constellation post-processed precise point positioning (PPP)/INS reference solution, computed through Inertial Explorer 8.9 commercial software. The data collection platform and the reference trajectory are illustrated in Figure 5.

In this study, real-world UWB line-of-sight (LOS) scenes and simulated experimental scenes involving non-Gaussian noise introduced by anomalous measurements were designed to comprehensively evaluate the positioning accuracy, stability, and mean error level of the integrated system. Furthermore, several filtering strategies were employed for comparative analysis, including the EKF [61,62], MCC-EKF [63], MMCC-EKF [64], and MMCC-VBAKF methods. These different strategies shared identical prior parameter configurations, such as the initial state vector associated with error covariance matrix.

5.2. Positioning Performance Analysis in Real-World Experiment

In this section, we carried out the real-world experiment in UWB LOS scenario (Case 1) to assess the performance of the proposed MMCC-VBAKF for the INS/UWB/GNSS-RTK TC integrated positioning system. Figure 6 illustrates the time sequences of positioning error in the east (E), north (N), and up (U) directions for various solution strategies in Case 1. A comparison of the positioning errors for different solution strategies clearly indicates significant variations in positioning performance among the strategies in Case 1. In particular, in the EN directions, the EKF, MCC-EKF, and MMCC-EKF methods exhibit significant fluctuations in positioning errors. In contrast, the MMCC-VBAKF demonstrates reduced error variability, reflecting enhanced stability. The MCC-EKF demonstrates a considerable range of overall error fluctuations influenced by the prior selection of a single kernel bandwidth, particularly evident in the initial data. In contrast, the MMCC-EKF significantly enhances error control by incorporating the MMCC. However, its stability remains inferior to that of the MMCC-VBAKF. In conclusion, the MMCC-VBAKF effectively integrates the MMCC and VB estimation, resulting in optimal positioning performance for the integrated system and minimizing overall error fluctuations. Notably, the incorporation of the height constraint model within the integrated system achieves the same level of positioning error in the U direction across different solution strategies.

Figure 7 illustrates the overview of estimated position trajectory of various solution strategies in relation to the reference trajectory. To emphasize the differences in position estimation solutions more clearly, Figure 7 compares only the one loop motion trajectory. In comparison, the EKF and MCC-EKF indicate larger estimation error and greater trajectory deviation in areas of high dynamic variation. Although the MMCC-EKF more closely follows the reference trajectory, it still exhibits some position estimation deviation at certain turns. Moreover, the MMCC-VBAKF’s estimated position trajectory aligns more closely with the reference trajectory, especially highlighting superior stability and positioning accuracy at turns. This further validates the filtering stability and accuracy of the proposed MMCC-VBAKF.

Table 2. Statistical analysis of positioning errors for various solution strategies in Case 1.

Method		EKF	MCC-EKF	MMCC-EKF	MMCC-VBAKF
RMSE (m)	E	0.116	0.106	0.093	0.046
	N	0.059	0.040	0.039	0.029
	U	0.030	0.030	0.030	0.030
	2D	0.130	0.114	0.101	0.054
MAE (m)	E	0.106	0.097	0.085	0.042
	N	0.053	0.033	0.033	0.025
	U	0.024	0.023	0.023	0.023
	2D	0.123	0.104	0.093	0.052
STD (m)	E	0.048	0.085	0.039	0.022
	N	0.026	0.026	0.025	0.025
	U	0.030	0.030	0.030	0.030
	2D	0.040	0.045	0.039	0.015

presents the statistical analysis of positioning errors for various solution strategies, providing a comparative assessment of positioning performance in Case 1. Specifically, the RMSEs of the MMCC-VBAKF are 0.046 and 0.029 m in the EN directions, respectively, and reach 0.054 m in the horizontal (2D) direction, demonstrating superior positioning accuracy compared to other methods. Furthermore, in terms of STD, the MMCC-VBAKF achieves 0.022 and 0.025 m in the EN directions, respectively, and 0.015 m in the 2D direction. This significantly mitigates fluctuations in positioning errors and enhances the stability of position estimation. Furthermore, examining the error estimation directly, and in line with expectation, the MAEs of the MMCC-VBAKF are 0.042 and 0.025 m in the EN directions, respectively, and 0.052 m in the 2D direction, ensuring a consistently low average error level. This further demonstrates the method’s superior horizontal positioning accuracy in Case 1. It should be noted that, due to the integration of the height constraint model within the integrated system, the positioning performance in the U direction remains nearly identical across different solution strategies. Consequently, the MMCC-VBAKF exhibits excellent overall positioning accuracy, effectively enhancing the integrated system’s performance in Case 1. Compared to other solution strategies, the MMCC-VBAKF leverages multi-model fusion and optimization to deliver more precise position estimation.

Figure 8 illustrates the cumulative distribution function (CDF) curves for various solution strategies in Case 1, focusing on horizontal positioning errors. The CDF curves reveal that the proposed MMCC-VBAKF rises more rapidly than other solution strategies, indicating that MMCC-VBAKF provides more accurate positioning results. Furthermore, with horizontal positioning errors of 0.1 m as the boundary, the MMCC-VBAKF achieves an availability of 99.79%, surpassing the availabilities of other solution strategies, which are 56.46%, 43.87%, and 30.69%, respectively.

In addition, we provide the distribution of horizontal positioning errors for various solution strategies in Case 1, as depicted in Figure 9. Intuitively, the proportions of horizontal positioning errors exceeding 0.15 m for EKF, MCC-EKF, and MMCC-EKF are 26.88%, 18.83%, and 8.19%, respectively. In contrast, the horizontal positioning errors for MMCC-VBAKF are entirely within 0.15 m. The quantitative results of statistical analysis strongly indicate that, compared to other solution strategies, the MMCC-VBAKF achieves continuous, accurate, and reliable position estimation, representing a significant improvement.

Table 3. Statistics on the improvement of the proposed MMCC-VBAKF in Case 1.

Method	Improvements
	RMSE (%)			MAE (%)			STD (%)
	E	N	2D	E	N	2D	E	N	2D
EKF	60.34	50.85	58.46	60.38	52.83	57.72	54.17	3.85	62.50
MCC-EKF	56.60	27.50	52.63	56.70	24.24	50.00	74.12	3.85	66.67
MMCC-EKF	50.54	25.64	46.53	50.59	24.24	44.09	43.59	0.00	61.54

and Figure 10 depict additional statistics on the improvement achieved by the proposed MMCC-VBAKF in Case 1. Regarding RMSEs, the MMCC-VBAKF demonstrates a substantial advantage in improved percentage in the EN directions compared to EKF, MCC-EKF, and MMCC-EKF, achieving overall enhancements of 58.46%, 52.63%, and 46.53% in the 2D direction, respectively. In MAE terms, the MMCC-VBAKF demonstrates the most notable improvement in the E direction, significantly reducing the average error level with enhancements of 60.38%, 56.7%, and 50.59% over the other three solution strategies. Additionally, it shows a certain degree of improvement in the N direction. For STDs, the MMCC-VBAKF exhibits superior performance in the 2D direction, with improvement percentages exceeding 60%, i.e., 62.5%, 66.67%, and 61.54%, which significantly reduce fluctuations in positioning errors. In conclusion, the proposed MMCC-VBAKF exhibits superior positioning performance compared to other solution strategies in Case 1, indicating the enhanced integrated system’s positioning accuracy and stability.

5.3. Positioning Performance Analysis in Simulation Experiment

To investigate the filtering performance and reliability of the proposed MMCC-VBAKF when anomalous measurements are present in the integrated system, the simulation experiment in a scenario involving non-Gaussian noise introduced by outliers (Case 2) was specifically designed. To better simulate non-Gaussian noise, we adopt a mixed Gaussian distribution. The probability distribution, disturbed by anomalous measurements, is expressed as

$${𝑣}_{\mathrm{N}G}\sim 0.9\mathcal{N}\left(0,0.016\right)+0.1\mathcal{N}\left(0,0.01\right)$$

where ${𝑣}_{\mathrm{N}G}$ denotes the mixed Gaussian noise. $\mathcal{N}\left(0,\cdot \right)$ represents the Gaussian distribution with 0 mean and the corresponding covariance.

Figure 11 depicts the time sequences of positioning error in the ENU directions for various solution strategies in Case 2, relative to the reference trajectory. Intuitively, by incorporating the MMCC and VB estimation, the MMCC-VBAKF mitigates the effect of anomalous measurements and unknown, time-varying noise statistical characteristics. Specifically, the EKF illustrates a greater deviation in positioning errors in the E direction compared to solution strategies incorporating the MCC. Furthermore, solution strategies that integrate VB estimation improve error control when handling unknown, time-varying noise statistics introduced by outliers, demonstrating enhanced accuracy and robustness in filtering estimation. Thus, compared to other solution strategies, the MMCC-VBAKF exhibits superior position estimation performance in Case 2, significantly mitigating position drift error. This presents considerable application potential in certain extremely challenging scenarios, effectively suppressing error accumulation in the integrated system. Notably, the integration of the height constraint model further improves the coupled system’s overall positioning accuracy.

The overview of the estimated trajectory of various solution strategies relative to the reference trajectory are shown in Figure 12. This clearly illustrates that, in Case 2, the MCC-EKF reduces the degree of positioning error deviation induced by outliers compared to the EKF. In addition, the MMCC-EKF further improves the robustness and accuracy of position estimation, partially mitigating position drift error. However, the positioning performance of the above three solution strategies still presents certain limitations when addressing unknown and time-varying noise statistics. By incorporating VB estimation, the MMCC-VBAKF adaptively adjusts the noise statistical characteristics within the filtering process, enabling close alignment with the reference trajectory. This provides substantial advantages in positioning accuracy and robustness under uncertainty disturbances in the integrated system. Experimental results further indicate that the MMCC-VBAKF achieves high accuracy and robustness in filtering estimation of the integrated system’s state vector.

To evaluate the effectiveness of various solution strategies in Case 2,

Table 4. Statistical analysis of positioning errors for various solution strategies in Case 2.

Method		EKF	MCC-EKF	MMCC-EKF	MMCC-VBAKF
RMSE (m)	E	0.917	0.428	0.280	0.268
	N	1.626	1.540	1.450	0.727
	U	0.030	0.030	0.030	0.030
	2D	1.867	1.598	1.477	0.774
MAE (m)	E	0.898	0.404	0.264	0.245
	N	1.541	1.455	1.362	0.671
	U	0.023	0.025	0.024	0.024
	2D	1.821	1.532	1.395	0.726
STD (m)	E	0.184	0.189	0.161	0.116
	N	0.520	0.505	0.500	0.279
	U	0.030	0.030	0.030	0.030
	2D	0.412	0.454	0.395	0.269

reports the corresponding statistical analysis of the positioning errors. The RMSEs of the MMCC-VBAKF are 0.268 and 0.727 m in the EN directions, respectively, and reach 0.774 m in the horizontal direction, demonstrating superior positioning accuracy compared to other solution strategies. Additionally, the STDs of the MMCC-VBAKF are 0.116 and 0.279 m in the EN directions, respectively, and 0.269 m in the horizontal direction, indicating superior performance in the stability of position estimation and improved error control. In terms of MAE, the MMCC-VBAKF achieves 0.245 and 0.671 m in the EN directions, respectively, and 0.726 m in the 2D direction. Compared to other solution strategies, the quantitative results strongly indicate that the MMCC-VBAKF exhibits superior performance in positioning error suppression in Case 2, effectively mitigating error divergence. The positioning accuracy in the U direction remains nearly consistent across different solution strategies, a result of the integrated system incorporating the height constraint model. In conclusion, the statistical analysis indicates that the MMCC-VBAKF achieves superior positioning performance when applied to the integrated system in Case 2.

The CDF curves of horizontal positioning errors for different solution strategies in Case 2 are depicted in Figure 13. Intuitively, compared to other solution strategies, the CDF curve of the MMCC-VBAKF rises more rapidly, achieving higher positioning accuracy. Furthermore, with horizontal positioning availability of 80% as the boundary, the horizontal positioning error of the MMCC-VBAKF is 0.986 m, surpassing those of other solution strategies, which are 1.530, 2.008, and 2.126 m, respectively.

Figure 14 depicts the distribution of horizontal positioning errors across various solution strategies in Case 2. The proportions of horizontal positioning errors exceeding 1.5 m are 64.72%, 40.95%, and 27.73% for EKF, MCC-EKF, and MMCC-EKF, respectively. In contrast, the horizontal positioning errors of the MMCC-VBAKF remain within 1.5 m. This further highlights that the MMCC-VBAKF can achieve more accurate and reliable positioning performance in the integrated system.

The degrees of improvement achieved by the proposed MMCC-VBAKF in Case 2 are shown in

Table 5. Statistics on the improvement of the proposed MMCC-VBAKF in Case 2.

Method	Improvements
	RMSE (%)			MAE (%)			STD (%)
	E	N	2D	E	N	2D	E	N	2D
EKF	70.77	55.29	58.54	72.72	56.46	60.13	36.96	46.35	34.71
MCC-EKF	37.38	52.79	51.56	39.36	53.88	52.61	38.62	44.75	40.75
MMCC-EKF	4.29	49.86	47.60	7.20	50.73	47.96	27.95	44.20	31.90

and Figure 15. Statistical results indicate that, in terms of RMSE in the 2D direction, the MMCC-VBAKF achieves improvement percentages of 58.54%, 51.56%, and 47.6%, outperforming the EKF, MCC-EKF, and MMCC-EKF, respectively. For MAEs, statistics show that the MMCC-VBAKF provides a remarkable degree of improvement in the EN directions, significantly improving error control. Regarding STDs, the improvement percentages of the MMCC-VBAKF in the 2D direction are 34.71%, 40.75%, and 31.9%, respectively. The quantitative results indicate that the MMCC-VBAKF effectively suppresses error divergence. In conclusion, the simulation experiment in Case 2 indicates that the MMCC-VBAKF achieves superior positioning performance and robustness compared to the other three solution strategies when addressing non-Gaussian noise interference introduced by outliers.

6. Conclusions

Precise, stable, and robust position estimation is essential to guarantee the continuity of a UGV’s position without position drift error. This paper firstly proposes a TC INS/UWB/GNSS-RTK integrated positioning system framework. This proposed system employs the IMU as the core driving component, enabling fusion at the raw measurement level to maximize the utilization of all available information. Specifically, the proposed system tightly couples raw UWB and GNSS measurements directly into an IMU-centered estimator. Through rigid transformation, these measurements are aligned with the core IMU to achieve state estimation for the integrated system. Furthermore, the proposed system further improves positioning accuracy by incorporating the height constraint model, aiming to serve as a seamless indoor and outdoor positioning solution. In addition, we propose a novel MMCC-based VBAKF, designed to ensure accurate initialization and state estimation within the integrated system. The proposed approach integrates the robustness of the MMCC, providing instantaneous resilience against non-Gaussian noise introduced by outliers, thereby achieving stable positioning accuracy under challenging conditions. Meanwhile, the proposed method employs VB estimation to adaptively adjust the coupled system’s noise statistics, effectively mitigating the impact of uncertainty disturbances on positioning accuracy.

To rigorously assess the proposed method, a comprehensive series of real-world and simulated on-vehicle experiments were carried out, focusing on its robustness and accuracy in estimating the integrated system’s state vector. In particular, in real-world UWB LOS scenario, the proposed method achieves 99.79% availability within a horizontal position error boundary of 0.1 m, markedly surpassing the positioning performance of the three comparative approaches. Additionally, in simulation experiments with non-Gaussian noise induced by outliers, the proposed method achieves a horizontal position error of 0.986 m within an 80% availability boundary, demonstrating effective control over positioning errors under challenging conditions. It has been demonstrated through experiments that the proposed approach ensures accurate and reliable estimation of the integrated system’s state vector, effectively mitigating position drift error induced by uncertainty disturbances.

In the future, to further investigate the effectiveness of more efficient and high-performance data processing strategies in complex seamless indoor–outdoor environments, it would be worthwhile to integrate IMU sensor with additional heterogeneous low-cost sensors and computational modules, such as camera-based relative positioning sensor.