Variational Bayesian Innovation Saturation Kalman Filter for Micro-Electro-Mechanical System–Inertial Navigation System/Polarization Compass Integrated Navigation

Abstract

Aiming at the issue of time-varying measurement noise with heavy-tailed characteristics and outliers generated by the polarization compass (PC) in the micro-electro-mechanical system–inertial navigation system (MEMS-INS) and PC-integrated navigation system when it is subject to internal and external disturbances, an improved Variational Bayesian Innovation Saturation Robust Adaptive Kalman filter (VISKF) algorithm is proposed. This algorithm utilizes the variational Bayesian (VB) method based on Student’s t-distribution (STD) to approximately calculate the statistical characteristics of the time-varying measurement noise of the PC, thereby obtaining more accurate measurement noise statistical parameters. Additionally, the algorithm introduces an innovation saturation function and proposes an adaptive update strategy for the saturation boundary. It mitigates the problem of innovation value divergence in PC caused by outliers through a two-layer structure that can track the changes in the innovation value to adaptively adjust the saturation boundary. To verify the effectiveness of the algorithm, static and dynamic experiments were conducted on an unmanned vehicle. The experimental results show that compared with adaptive Kalman filter (AKF), variational Bayesian robust adaptive Kalman filter (VBRAKF), and innovation saturate robust adaptive Kalman filter (ISRAKF), the proposed algorithm improves the dynamic orientation accuracy by 76.89%, 67.23%, and 84.45%, respectively. Moreover, compared with other similar target algorithms, the proposed algorithm also has obvious advantages. Therefore, this method can significantly improve the navigation accuracy and robustness of the INS/PC integrated navigation system in complex environments.

1. Introduction

Autonomous navigation is the crucial technology for automating and intelligent vehicles, with extensive applications in space exploration, aviation, maritime transportation, ground vehicles [1]. Currently, the micro-electro-mechanical system–inertial navigation system (MEMS-INS) and Global Navigation Satellite System (GNSS) are the most widely employed integrated navigation methods for acquiring navigation information [2,3,4,5]. However, INS errors accumulate over time [6,7,8]. Meanwhile, GNSS, which relies on satellite signals from space, is susceptible to both natural and man-made interference, resulting in reduced accuracy [9,10]. Therefore, bionic polarization navigation technology has emerged as a promising alternative due to its advantages in independence, concealment, continuity, autonomy, resistance to electromagnetic interference, and high precision [11]. This technology leverages the polarization characteristics of light to detect the polarization distribution patterns of sky or sunlight, thereby obtaining the heading angle information of the carrier [9]. Extensive research has demonstrated that the novel autonomous navigation system combining MEMS-INS and polarization compass (PC) exhibits superior performance, effectively meeting the stringent requirements for high-precision and high-robustness navigation information acquisition.

Kalman filtering is a widely adopted method for fusing PC and MEMS-INS information. However, its performance critically depends on the accurate modeling of the random characteristics of both system processes and measurement noise [12]. In complex scenarios—such as lens occlusion or high-speed carrier maneuvers—the measurement noise of the PC exhibits time-varying and heavy-tailed characteristics [13,14], introducing stochastic and systematic errors into system modeling and data acquisition that substantially degrade navigation accuracy [1]. These factors contribute to measurement uncertainty in the PC, significantly undermining the performance of the INS/PC integrated navigation system.

To address measurement uncertainty and enhance the robustness of integrated navigation systems, extensive research has been conducted on robust Kalman filtering and adaptive filtering algorithms. In the realm of robust Kalman filtering, methods such as the Huber-based Kalman Filter (HKF) [15,16], Maximum Entropy Criterion-based Kalman Filter (MCCKF) [17,18], and Student’s t-distribution (STD)-based Kalman Filter [19,20,21,22] have garnered significant attention. Meanwhile, many adaptive stochastic models directly estimate the covariance matrices of system process and measurement noise, monitor changes in the covariance of innovations and residuals, and make timely adjustments. For instance, Hide et al. [23] developed a covariance scaling method that enhances filter robustness by introducing scaling factors into the covariance matrix. Yang et al. [24] designed a robust adaptive filter that combines robust maximum likelihood estimation with adaptive filtering, adjusting the weights of the predicted parameters based on the differences between system measurements and predicted innovation values. Gao et al. [25] proposed a robust adaptive filter that adaptively adjusts the state noise covariance matrix using an adaptive factor derived from prediction residuals.

Significant research has been conducted on robust adaptive filtering for INS/PC integrated navigation systems. For instance, Zhao et al. [13] proposed an enhanced Variational Bayesian Cubature Kalman Filter (VBCKF) to estimate the time-varying measurement noise covariance of the sub-filters. While this approach can partially capture the variations in measurement noise in complex environments, it simplifies the modeling of measurement noise and does not account for its heavy-tailed characteristics. To address the issue of outliers, Zhao et al. introduced a residual Chi-squared detection algorithm to identify information mismatches or sensor faults, along with a residual-based compensation algorithm to mitigate abnormal PC measurements. This method detects faults using thresholds determined by the Chi-squared distribution; however, it is not sensitive to small outliers. Dou et al. [26] proposed an adaptive adjustment method based on the degree of polarization to tackle the problem of outliers in PC measurements. In their work, the degree of polarization was defined as a deterministic factor to adaptively adjust the measurement noise matrix by modifying the update rule. Although this approach improved the stability of the sky polarization light-based autonomous navigation system under outlier conditions, it only adjusts the measurement noise matrix by constraining the update term with a deterministic factor, failing to accurately capture the variations in measurement noise.

Building on previous research, high-precision and robust INS/PC integrated navigation systems face several key challenges. One significant issue is accurately modeling the time-varying measurement noise of the PC, particularly its heavy-tailed characteristics, which conventional approaches struggle to capture effectively. Additionally, existing INS/PC fusion methods lack the flexibility to adapt the noise covariance matrix in real time, reducing their robustness in dynamic environments. Furthermore, outliers caused by internal sensor errors and external disturbances exacerbate these challenges, as they are not effectively mitigated by current filtering methods. Addressing these limitations is critical to enhancing system accuracy and reliability.

In the field of robust filtering algorithms, Kalman filters based on the STD have recently emerged as a prominent area of research, with the STD-based measurement uncertainty filter already applied in integrated navigation systems for complex underwater environments [27]. Motivated by this approach and in response to the challenges outlined above, this paper proposes a STD-based Variational Bayesian Innovation Saturated Kalman Filter (VISKF).

The primary contributions of this paper can be summarized as follows:

(1) Modeling and estimating the measurement noise of PC in complex environments. This study developed a VB estimation method based on the STD to model and estimate the measurement noise of PC in complex environments. The remodeling of the measurement noise using the STD aims to better capture the heavy-tailed characteristics of the noise in such environments. Subsequently, this study employed VB theory to estimate statistical characteristic parameters of the measurement noise, resulting in a more accurate model.

(2) Innovation saturated the robust adaptive fusion method. This method introduces a predictive innovation saturation (IS) function to address the propagation of innovation errors caused by anomalous PC measurements. To further enhance robustness, an adaptive innovation saturation boundary strategy is employed, ensuring the retention of accurate values while suppressing erroneous ones. The boundary is dynamically adjusted through a dual-layer structure: the lower layer monitors and responds to changes in innovation values, while the upper layer refines the boundary to maintain precision. This design ensures accurate corrections to enhance system performance.

The remainder of this paper is structured as follows: Section 2 provides a comprehensive overview of the modeling of the INS/PC. Section 3 presents detailed insights into the proposed algorithm. Section 4 outlines the experimental process and data analysis results. Finally, in Section 5, a conclusion is drawn and a brief summary of the paper is provided.

2. INS/PC Integrated Navigation System Model

The navigation frames (n-frames) referenced in this paper are orthogonal reference frames aligned with the East–North–Up (E-N-U) geodetic axes. The INS/PC integrated navigation system can be described as follows:

$$\left\{\begin{array}{l}\dot{\mathit{X}}=\mathit{F}\mathit{X}+\mathit{N}\hfill \\ \mathit{Z}=\mathit{H}\mathit{X}+\mathit{V}\hfill \end{array}\right.,$$

(1)

where $\mathit{F}$ is the state transition matrix, given by

$$F=\left[\begin{array}{ccccc}{M}_{aa}& M_{av}& M_{ap}& -C_b^n& 0_{3\times 3}\\ M_{va}& M_{vv}& M_{vp}& 0_{3\times 3}& C_b^n\\ 0_{3\times 3}& M_{pv}& M_{pp}& 0_{3\times 3}& 0_{3\times 3}\\ & & 0_{6\times 15}& & \end{array}\right],$$

(2)

where $M_{aa}$, $M_{av}$, $M_{ap}$, $M_{va}$, $M_{vv}$, $M_{vp}$, $M_{pv}$, and $M_{pp}$ can be found in [28]. $\mathit{N}$ is the process noise vector, $\mathit{Z}$ signifies the measurement information of the PC, and $\mathit{H}$ denotes the system observation matrix. $\mathit{V}$ is the measurement noise, and $\mathit{X}$ is the state vector, and its representation is as follows:

$$\mathit{X}={[\begin{array}{lllll}{\varphi }_{1\times 3}^T\hfill & \delta {\mathit{V}}_{1\times 3}^T\hfill & \delta P_{1\times 3}^T\hfill & \delta {\omega }_{1\times 3}^T\hfill & \delta f_{1\times 3}^T\hfill \end{array}]}^T,$$

(3)

where ${\varphi }_{1\times 3}={\left[ \begin{array}{ccc}\delta \theta & \delta \gamma & \delta \psi \end{array}\right]}^T$ denotes the attitude error, $\delta {\mathit{V}}_{1\times 3}={[\begin{array}{ccc}\delta v_E^n& \delta v_N^n& \delta v_U^n\end{array}]}^T$ represents the velocity error vector, and $\delta P_{1\times 3}={\left[\begin{array}{ccc}\delta L& \delta \lambda & \delta h\end{array}\right]}^T$ indicates the position error vector. $\delta {\omega }_{1\times 3}={\left[\begin{array}{ccc}{\omega }_x^b& {\omega }_y^b& {\omega }_z^b\end{array}\right]}^T$ is the gyroscope drift error vector, and $\delta f_{1\times 3}={\left[\begin{array}{ccc}{f}_x^b& f_y^b& f_z^b\end{array}\right]}^T$ is the accelerometer bias error vector.

Upon discretization of the state equation, the following expression can be derived:

$$x_k=\left(I+F\mathsf{\Delta }t\right)x_{k-1}+n_{k-1}\mathsf{\Delta }t={\Phi }_{k-1}{x}_{k-1}+w_{k-1},$$

(4)

where $k$ represents the system instant, $\mathsf{\Delta }t$ denotes the sampling interval, ${\Phi }_{k-1}$ represents the discrete state transition matrix, and $w_{k-1}$ signifies the discrete process noise matrix.

The following equation can be derived using the heading output obtained from the PC and INS:

$$\mathit{Z}={[\begin{array}{cc}{\mathit{Z}}_{\varphi }& 0_{1\times 12}\end{array}]}^T,\hspace{1em}{\mathit{Z}}_{\varphi }=[\begin{array}{ccc}0& 0& {\psi }_{INS}-{\psi }_{PC}\end{array}],$$

(5)

$${\mathit{Z}}_{\varphi }=\left(\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 1\end{array}\right)\varphi ={\mathit{M}}_{\psi }\varphi ,$$

(6)

where ${\mathit{M}}_{\psi }$ represent the observation matrix. ${\psi }_{INS}$ and ${\psi }_{PC}$ denote the heading angles produced by INS and PC, respectively. At this point, the discrete representation of the measurement equation can be formulated as follows:

$$z_k=H{x}_k+v_k,$$

(7)

$$H=\left[\begin{array}{llll}{\mathit{M}}_{\psi }\hfill & {\mathbf{0}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 6}\hfill \\ {\mathbf{0}}_{3\times 3}\hfill & {\mathit{I}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 6}\hfill \\ {\mathbf{0}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 3}\hfill & {\mathit{I}}_{3\times 3}\hfill & {\mathbf{0}}_{3\times 6}\hfill \end{array}\right],$$

(8)

where $z_k$ represents the difference between the heading angle of PC and INS. $H$ denotes the measurement matrix, while $v_k$ signifies the measurement noise of PC. The covariance matrices for process and measurement noise are denoted as $Q_k$ and $R_k$, respectively.

3. VISKF Algorithm

3.1. Reformulate a Probabilistic Distribution Model

In the INS/PC integrated system, the INS demonstrates relative stability with few significant outliers, making it reasonable to assume that the process noise follows a Gaussian distribution. As a result, the one-step prediction probability density function (PDF) can be given by

$$p\left(x_k|z_{1:k-1}\right)=N\left(x_k;{\widehat{x}}_{k|k-1},P_{k|k-1}\right).$$

(9)

The measurement noise $v_k$, exhibiting heavy-tailed behavior and following the STD of the PC, can be effectively represented by the following equation:

$$p\left(v_k\right)=\mathrm{S}t\left(v_k;0,R_k,\gamma \right),$$

(10)

where $\mathrm{S}t\left(\dots ;u,a,b\right)$ is the PDF of the STD, where the parameters represent the distribution that satisfies a mean of $u$, a scale matrix of $a$, and a degree of freedom of $b$. Using (10), the heavy-tailed measurement noise of the PC has been modeled as an STD that satisfies a mean of 0, a scale matrix of $R_k$, and a degree of freedom of $\gamma$.

The PDF of PC measurement likelihood distribution can be delineated as follows:

$$p(z_k|x_k)=\mathrm{S}t\left(z_k;H_k{x}_k,R_k,\gamma \right).$$

(11)

The STD can be interpreted as an infinite mixture of Gaussian distributions [29], $p\left(v_k\right)$ and $p(z_k|x_k)$ can be expressed as the convolution of Gaussian and Gamma distributions.

$$p\left(v_k\right)=\int N\left(v_k;0,{\displaystyle \frac{R_k}{{\lambda }_k}}\right)G\left({\lambda }_k;{\displaystyle \frac{\gamma }{2}},{\displaystyle \frac{\gamma }{2}}\right)d{\lambda }_k,$$

(12)

$$p\left(z_k|x_k\right)=\int N\left(z_k;H_k{x}_k,{\displaystyle \frac{R_k}{{\lambda }_k}}\right)G\left({\lambda }_k;{\displaystyle \frac{\gamma }{2}},{\displaystyle \frac{\gamma }{2}}\right)d{\lambda }_k,$$

(13)

where ${\lambda }_k$ is introduced as an auxiliary variable to dynamically adjust the covariance matrix $R_k$ of the measurement noise. By expressing the covariance of the measurement noise as $R_k/{\lambda }_k$, the model can effectively address heavy-tailed noise, thereby enhancing the accuracy and robustness of the tracking process. By modeling this auxiliary random variable, the distribution characteristics of $R_k$ can be indirectly inferred.

The likelihood probability density function of system can be represented in a hierarchical Gaussian structure [30]

$$p\left(z_k|x_k,R_k,{\lambda }_k\right)=N\left(z_k;H_k{x}_k,{\displaystyle \frac{R_k}{{\lambda }_k}}\right),$$

(14)

$$\begin{array}{c}p\left({\lambda }_k\right)=G\left({\lambda }_k;{\displaystyle \frac{\gamma }{2}},{\displaystyle \frac{\gamma }{2}}\right)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}=\left({\displaystyle \frac{m+\gamma }{2}}-1\right)\log {\lambda }_k-{\displaystyle \frac{{\lambda }_k\gamma }{2}}-{\displaystyle \frac{{\lambda }_k}{2}}{\left(z_k-H_k{x}_k\right)}^T{R}_k^{-1}\left(z_k-H_k{x}_k\right)\hfill \\ \hspace{1em}\hspace{1em}\hspace{1em}-{\displaystyle \frac{1}{2}}{\left(x_k-{\widehat{x}}_{k\left|k-1\right.}\right)}^T{P}_{k\left|k-1\right.}^{-1}\left(x_k-{\widehat{x}}_{k\left|k-1\right.}\right)+C,\hfill \end{array}$$

(15)

where $C$ is a constant.

3.2. Variational Approximation Estimation

By applying the VB method, each parameter of the PC is treated as an independent variable, enabling us to approximate the joint posterior PDF as follows:

$$p(x_k,R_k,{\lambda }_k|z_{1:k})\approx q\left(x_k\right)q\left(R_k\right)q\left({\lambda }_k\right).$$

(16)

To obtain the solution for the approximate distribution, it is necessary to minimize the Kullback–Leibler Divergence (KLD) between the posterior distribution of the PC measurement information and its approximate distribution. Thus, solving the following equation becomes essential:

$$\begin{array}{c}\left\{q\left(x_k\right),q\left(R_k\right),q\left({\lambda }_k\right)\right\}=\hfill \\ \arg \min KLD\left(\Theta ‖p\left(x_k,R_k,{\lambda }_k\left|z_{1:k}\right.\right)\right)\left(\Theta =q\left(x_k\right),q\left(R_k\right),q\left({\lambda }_k\right)\right),\hfill \end{array}$$

(17)

where $KLD\left(·\right)$ is used to calculate KLD. Its precise mathematical formulation is given by

$$KLD\left(q\left(z\right)‖p\left(\mathrm{z}\right)\right)=\int q\left(z\right)\log {\displaystyle \frac{q\left(z\right)}{p\left(\mathrm{z}\right)}}dz.$$

(18)

In accordance with the computational approach of VB, expressions for $q\left(x_k\right)$, $q\left(R_k\right)$, $q\left({\lambda }_k\right)$ can be derived as

$$\left\{\begin{array}{c}\log q\left(x_k\right)=E_{R_k,{\lambda }_k}\left[\log p\left(x_k,R_k,{\lambda }_k,z_{1:k}\right)\right]+C_x\\ \log q\left(R_k\right)=E_{x_k,{\lambda }_k}\left[\log p\left(x_k,R_k,{\lambda }_k,z_{1:k}\right)\right]+C_R\\ \log q\left({\lambda }_k\right)=E_{x_k,R_k}\left[\log p\left(x_k,R_k,{\lambda }_k,z_{1:k}\right)\right]+C_{\lambda },\end{array}\right.$$

(19)

where $C_x$, $C_R$, and $C_{\lambda }$ are constants. Approximate solutions for the coupled variables are computed by the fixed-point iteration method.

Using the properties of the hierarchical Gaussian state-space model, the joint PDF can be given by

$$\begin{array}{c}p(x_k,R_k,{\lambda }_k,z_{1:k})\hfill \\ =p(z_k|x_k,R_k,{\lambda }_k)p\left(x_k|z_{1:k-1}\right)p\left(R_k\right)p\left({\lambda }_k\right)\hfill \\ =N\left(z_k;H_k{x}_k,{\displaystyle \frac{R_k}{{\lambda }_k}}\right)N\left(x_k;{\widehat{x}}_{k-1},P_{k-1}\right)\times IW\left(R_k;{\widehat{u}}_{k-1},{\widehat{U}}_{k-1}\right)G\left({\lambda }_k;{\displaystyle \frac{\gamma }{2}},{\displaystyle \frac{\gamma }{2}}\right).\hfill \end{array}$$

(20)

Using Equations (19) and (20), the following can be obtained:

$$\log q^{\left(n+1\right)}\left(R_k\right)=-{\displaystyle \frac{1}{2}}\left(m+{\widehat{u}}_{k-1}+2\right)\log \left|R_k\right|-{\displaystyle \frac{1}{2}}tr\left[{\widehat{U}}_{k-1}+E^{\left(n+1\right)}\left[{\lambda }_k\right]A_k^{\left(n+1\right)}\right]+C_{R_k}.$$

(21)

Using Equation (21), $q^{\left(n+1\right)}\left(R_k\right)$ is expressed as an inverse Wishart distribution (WD), which can be given by

$$q^{\left(n+1\right)}\left(R_k\right)=IW\left(R_k;{\widehat{u}}_k^{\left(n+1\right)},{\widehat{U}}_k^{\left(n+1\right)}\right),$$

(22)

where ${\widehat{u}}_k^{\left(n+1\right)}$ and ${\widehat{U}}_k^{\left(n+1\right)}$ represent the degrees of freedom and the inverse scale matrix, respectively, which can be given by

$${\widehat{u}}_{k|k}^{\left(n+1\right)}={\widehat{u}}_{k|k-1}+1,\hspace{1em}{\widehat{U}}_{k|k}^{\left(n+1\right)}={\widehat{U}}_{k|k-1}+A_k^{\left(n+1\right)}{E}^{\left(n+1\right)}\left[{\lambda }_k\right],$$

(23)

where $A_k^{\left(n+1\right)}$ is the auxiliary matrix, and it can be given by

$$A_k^{\left(n+1\right)}=(z_k-H_k{\widehat{x}}_k^{\left(n+1\right)}){(z_k-H_k{\widehat{x}}_k^{\left(n+1\right)})}^T+H_k{P}_k^{\left(n+1\right)}{H}_k^T.$$

(24)

By substituting Equations (15) and (20) into Equation (19), a variational form of $q^{\left(n+1\right)}\left({\lambda }_k\right)$ can be given by

$$\log q^{\left(n+1\right)}\left({\lambda }_k\right)=\left({\displaystyle \frac{m+\gamma }{2}}-1\right)\log {\lambda }_k-{\displaystyle \frac{1}{2}}\left(\gamma +tr\left(A_k^{\left(n\right)}{E}^{\left(n\right)}\left[R_k^{-1}\right]\right)\right){\lambda }_k+C_{{\lambda }_k},$$

(25)

where tr(·) is the trace operation, and $m$ is the dimension of the measurement vector.

In accordance with Equation (15), the approximate posterior PDF of ${\lambda }_k$ can be given by

$$q^{\left(n+1\right)}\left({\lambda }_k\right)=G\left({\lambda }_k;a_k^{\left(n+1\right)},b_k^{\left(n+1\right)}\right),$$

(26)

where $a_k^{\left(n+1\right)}$ and $b_k^{\left(n+1\right)}$ denote the shape parameter and rate parameter, respectively. Their values can be given by

$$a_k^{\left(n+1\right)}={\displaystyle \frac{1}{2}}\left(m+\gamma \right)b_k^{\left(n+1\right)}={\displaystyle \frac{1}{2}}\left(\gamma +tr\left(A_k^{\left(n\right)}{E}^{\left(n\right)}\left[R_k^{-1}\right]\right)\right).$$

(27)

Since ${\lambda }_k$ is distributed according to a Gamma distribution, the mathematical expectation of ${\lambda }_k$ can be given by

$$E^{\left(n+1\right)}\left({\lambda }_k\right)={\displaystyle \frac{a_k^{\left(n+1\right)}}{b_k^{\left(n+1\right)}}}.$$

(28)

From the properties of the WD, the mathematical expectation of $R_k^{-1}$ can be deduced as follows:

$$E^{\left(n+1\right)}\left(R_k^{-1}\right)=\left({\widehat{u}}_k^{\left(n+1\right)}-m-1\right){\left({\widehat{U}}_k^{\left(n+1\right)}\right)}^{-1}.$$

(29)

The updated measurement noise covariance matrix can be derived from Equations (27) and (28) as follows:

$${\tilde{R}}_k^{\left(n+1\right)}={\left(E^{\left(n+1\right)}\left[R_k^{-1}\right]\right)}^{-1}/E^{\left(n+1\right)}\left[{\lambda }_k\right].$$

(30)

3.3. ISRAKF

To further enhance the filtering accuracy of the algorithm, this study introduces the IS robust adaptive Kalman filter (ISRAKF), which effectively eliminates outliers by means of predicting and updating the saturation function. Outliers have the potential to compromise innovation, thereby impacting the efficacy of KF in rectifying state estimation. Therefore, the introduction of saturation functions is used to suppress the divergence of innovation values caused by outlier deviations.

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k{\mathit{sat}}_{\alpha }\left({\mathit{e}}_k\right)({\mathit{z}}_k-H_k{\widehat{x}}_{k|k-1}),$$

(31)

where $e_k=z_k-H_k{\widehat{x}}_{k|k-1},$ represents the innovation value, and the saturation function is defined as follows:

$$sa{t}_{\alpha }\left(d\right)=sa{t}_{\sqrt{{\alpha }_k}}\left(d\right)=\left\{\begin{array}{c}-{\displaystyle \frac{1}{d}}\sqrt{{\alpha }_k},d<-\sqrt{{\alpha }_k}\\ 1\hspace{1em}\hspace{1em}\hspace{1em},\left|d\right|\le \sqrt{{\alpha }_k}\hfill \\ {\displaystyle \frac{1}{d}}\sqrt{{\alpha }_k},\sqrt{{\alpha }_k}<d,\end{array}\right.$$

(32)

where ${\alpha }_k>0$, and ${\alpha }_k$ is the saturation threshold. When the innovation falls within the interval $\left[-{({\alpha }_k)}^{1/2},{({\alpha }_k)}^{1/2}\right]$, the saturation function becomes a constant (=1), and the PC measurement is fully incorporated into the state estimate. Conversely, if $e_k$ is impacted by an outlier and exceeds the anticipated innovation range, the saturation function will reach its limit, thereby mitigating the influence of the outlier.

However, defining a consistent and effective saturation boundary presents significant challenges. If an outlier falls within the saturation range, the effectiveness of the saturation function may be compromised. Additionally, when the update value exceeds the saturation range at a given moment, it can adversely affect the precision of the filtering process. To address these challenges, this paper proposes adaptive strategies for dynamically adjusting the saturation boundary, which can be represented by the following formula:

$${\alpha }_k={\alpha }_{k-1}(1-e^{-{\eta }_1{\tau }_k}),$$

(33)

$${\tau }_k={\xi }_k{e}_k+{\eta }_2{\left(e_k\right)}^2,$$

(34)

where ${\tau }_k>0$, and ${\tau }_k$ denotes the number of connections within the two-layer mechanism. ${\xi }_k$ represents the positive definite coefficient, ensuring that ${\tau }_k$ consistently satisfies the positive condition. ${\eta }_1>0$ and ${\eta }_2>0$. Changing ${\eta }_1$ can adjust the response speed of the saturation boundary to changes in innovation, while changing ${\eta }_2$ can adjust the sensitivity of the saturation boundary to innovation changes. Under normal circumstances, the value of $e_k$ will remain within an appropriate range. However, in the presence of outliers during the update process, $e_k$ will increase. Simultaneously, as $e_k$ increases due to outlier influence, ${\alpha }_k$ will decrease rapidly. This facilitates self-adaptive regulation of the saturation boundary. Consequently, normal measurement values and outlier values can be effectively distinguished from each other, while innovation values affected by outliers can be filtered out. To strengthen the scientific rigor of the proposed method and to provide systematic guidance for selecting the saturation boundary, a formal mathematical analysis is presented in Appendix A.

Combining the estimated results from the preceding section, the iterative ISRAKF prediction process of PC at the $(n+1)$th iteration is as follows:

$${\widehat{x}}_{k|k-1}={\Phi }_k{\widehat{x}}_{k-1|k-1},$$

(35)

$$P_{k|k-1}={\Phi }_k{P}_{k-1|k-1}{\Phi }_k^T+Q_{k-1},$$

(36)

where ${\widehat{x}}_{k|k-1}$ denotes the “prior,” while $P_{k|k-1}$ represents the error covariance matrix associated with the “prior.” Additionally, ${\widehat{x}}_{k-1|k-1}$ and $P_{k-1|k-1}$ denote the “posterior” and “posterior” error covariance matrices of the PC at time $k-1$. The correction process of ISRAKF can be given by

$$K_k^{(n+1)}=P_{k|k-1}^{(n+1)}{H}_k^T{\left(H_k{P}_{k|k-1}^{(n+1)}{H}_k^T+{\tilde{R}}_i^{(n+1)}\right)}^{-1},$$

(37)

$${\widehat{x}}_{k|k}^{(n+1)}={\widehat{x}}_{k|k-1}^{(n+1)}+K_k^{(n+1)}sa{t}_{\sqrt{{\alpha }_k}}(e_k)\left(z_k-H_k{\widehat{x}}_{k|k-1}\right),$$

(38)

$$P_{k|k}^{(n+1)}=\left(I-K_k^{(n+1)}{H}_k\right)P_{k|k-1}^{(n+1)},$$

(39)

where $K_k^{(n+1)}$ is the Kalman gain, ${\widehat{x}}_{k|k}^{(n+1)}$ is the posterior estimate, and $P_{k|k}^{(n+1)}$ is the posterior error covariance matrix.

To facilitate understanding, the workflow is summarized in Algorithm 1, and the algorithm’s structure is illustrated in the flowchart in Figure 1.

By the proposed algorithm, the measurement noise in the INS/PC integrated navigation system is modeled as heavy-tailed noise following the STD, and outliers in the PC are suppressed. Thereby enhancing the performance of the navigation system.

Algorithm 1: The Proposed VISKF

Inputs: ${\widehat{x}}_{k-1|k-1}$, $z_k$, $P_{k-1|k-1}$, $H_k$, ${\Phi }_{k-1}$, ${\widehat{u}}_{k-1|k-1}$, ${\widehat{U}}_{k-1|k-1}$, $m$, $\gamma$, $Q_{k-1}$, ${\widehat{R}}_k$, $N$, ${\eta }_1$$,{\eta }_2$$,{\alpha }_{k-1}$

Time update:

1: Calculate ${\widehat{x}}_{k|k-1}={\Phi }_k{\widehat{x}}_{k-1|k-1}$

2: Calculate $P_{k|k-1}={\Phi }_k{P}_{k-1|k-1}{\Phi }_k^T+Q_{k-1}$

Measurement update:

Initialization:

3: ${\widehat{x}}_{k|k}^{\left(0\right)}={\widehat{x}}_{k-1|k-1}$$,P_{k|k}^{\left(0\right)}=P_{k|k-1}$$,{\widehat{u}}_{k|k}^{(0)}=m+1$$,{\widehat{U}}_{k|k}^{(0)}={\widehat{R}}_k$

For $n=0:N-1$

4: $A_k^{\left(n\right)}=(z_k-H_k{\widehat{x}}_{k|k}^{\left(n\right)}){(z_k-H_k{\widehat{x}}_{k|k}^{\left(n\right)})}^T+H_k{P}_{k|k}^{\left(n\right)}{H}_k^T$

5: Calculate $a_k^{\left(n+1\right)},b_k^{\left(n+1\right)}$ via (27)

6: Update $q^{\left(n+1\right)}\left({\lambda }_k\right)=G\left({\lambda }_k;a_k^{\left(n+1\right)},b_k^{\left(n+1\right)}\right)$

7: Calculate $E^{\left(n+1\right)}\left({\lambda }_k\right)=a_k^{\left(n+1\right)}/b_k^{\left(n+1\right)}$

8: Calculate ${\widehat{u}}_{k|k}^{\left(n+1\right)}$$,{\widehat{U}}_{k|k}^{\left(n+1\right)}$ via (23)

9: Update $q^{\left(n+1\right)}\left(R_k\right)=IW\left(R_k;{\widehat{u}}_{k|k}^{\left(n+1\right)},{\widehat{U}}_{k|k}^{\left(n+1\right)}\right)$

10: Calculate $E^{\left(n+1\right)}\left(R_k^{-1}\right)=\left({\widehat{u}}_k^{\left(n+1\right)}-m-1\right){\left({\widehat{U}}_{k|k}^{\left(n+1\right)}\right)}^{-1}$

11: Calculate ${\tilde{R}}_k^{\left(n+1\right)}={\left(E^{\left(n+1\right)}\left(R_k^{-1}\right)\right)}^{-1}/E^{\left(n+1\right)}\left({\lambda }_k\right)$

12: Update ${\alpha }_k$$,{\tau }_k$ via (33) and (34)

13: $K_k^{(n+1)}=P_{k|k-1}^{(n+1)}{H}_k^T{\left(H_k{P}_{k|k-1}^{(n+1)}{H}_k^T+{\tilde{R}}_i^{(n+1)}\right)}^{-1}$

14: ${\widehat{x}}_{k|k}^{(n+1)}={\widehat{x}}_{k|k-1}^{(n+1)}+K_k^{(n+1)}sa{t}_{\sqrt{{\alpha }_k}}(e_k)\left(z_k-H_k{\widehat{x}}_{k|k-1}\right)$

15: $P_{k|k}^{(n+1)}=\left(I-K_k^{(n+1)}{H}_k\right)P_{k|k-1}^{(n+1)}$

end for

16: ${\widehat{x}}_{k|k}\leftarrow {\widehat{x}}_{k|k}^{\left(N\right)}$$,P_{k|k}\leftarrow P_{k|k}^{\left(N\right)}$$,{\widehat{R}}_k\leftarrow {\tilde{R}}_k^{(N)}$

Outputs: ${\widehat{x}}_{k|k}$$,P_{k|k}$$,{\widehat{R}}_k$

4. Empirical Validation and Statistical Analysis

4.1. Experimental Equipment

To validate the efficacy of the proposed algorithm, static and dynamic experiments were conducted on an unmanned navigation test platform situated on a campus safety road, yielding a substantial volume of reliable data. The SPAN-KVH1750 system (Ronghui Positioning Technology Co., Ltd., Beijing, China) is employed as the heading reference. The configuration of the unmanned navigation test platform and the INS/PC integrated navigation system is illustrated in Figure 2, while sensor parameters are detailed in

Table 1. Table of sensor parameters.

Sensors	Parameters	Value
SPAN-KVH1750	Heading accuracy (RMS)	0.035°
	Output frequency	100 Hz
INS	Heading accuracy (RMS)	0.15°/m
	Output frequency	100 Hz
PC	Static heading accuracy (RMS)	0.3°
	Dynamic heading accuracy (RMS)	0.5°
	Output frequency	15 Hz

. The experimental setup utilizes a self-designed polarization compass, which employs the ZYNQ7020 chip (Shenzhen Wanchuang Xinda Electronics Co., Ltd., Shenzhen, China) as the computational core and the Sony IMAX250MZR CMOS (Shenzhen Wanchuang Xinda Electronics Co., Ltd., Shenzhen, China) as the polarization image sensor. To enhance computational efficiency, the polarization compass skips graphical processing and relies solely on the light intensity information from a central 40 × 40 region to calculate the polarization angle. The solar azimuth is computed using pre-input values of latitude, longitude, and time, with time updates provided by the internal chip clock.

4.2. Experimental Investigation of the Dynamic Condition

To comprehensively evaluate the proposed algorithm, ablation experiments and comparative experiments with comparable algorithms were conducted under both static and dynamic conditions. Among the control groups in the ablation experiments are AKF; ISRAKF; and VBAKF. The details of the method parameters for the ablation experiment are shown in

Table 2. Detailed ablation method configuration.

Sensors	Adaptive Mechanism	IS Mechanism	Improved VB
AKF	√	×	×
ISRAKF	√	√	×
VBAKF	√	×	√
Proposed	√	√	√

.

The similar target algorithms included in the comparative experiment are described below.

Huber’s M-estimation-Based Robust Adaptive Kalman Filter (Huber-based RAKF): The algorithm adopts Huber’s M-estimation paradigm. During the measurement-update step, an iterative scheme is constructed in which Huber’s M-estimator is employed to rescale—effectively inflate—the prior state-estimation covariance via a weighting matrix. This inflation mitigates the influence of process uncertainties induced by abrupt maneuvers. Iteration proceeds until all elements of the weight matrix converge to unity, a criterion that also minimizes computational effort [15].

Maximum Correntropy Criterion Kalman Filter (MCCKF): To enhance the robustness of the KF in non-Gaussian environments, the maximum correntropy Kalman filter (MCKF) replaces the conventional minimum-mean-square-error (MMSE) criterion with the maximum correntropy criterion (MCC) and introduces a fixed-point iteration scheme for its solution. By substituting correntropy for the mean-square-error cost and employing fixed-point iterations in the update step, the algorithm transmutes the traditional KF into a variant exhibiting markedly improved robustness against impulsive noise [17].

Among these algorithms, AKF serves as a baseline control group for evaluating the performance improvement achieved by various algorithms. Meanwhile, AKF, ISRAKF and VBAKF, along with the proposed algorithm, are employed as control groups in ablation experiments to investigate the impact of each module of the proposed algorithm. Huber-based RAKF and MCCKF serve as comparative benchmarks to evaluate the optimization impact of the proposed algorithm in comparison with other similar target algorithms.

The main challenges encountered by PC involve image degradation, including loss, distortion, and blurring, which are primarily caused by occlusion, vehicle turning, and high maneuverability. These complexities may result in outliers from PC. Obstruction to the PC may result in the loss of atmospheric polarization information captured by the PC. This information loss introduces an anomaly in solving the polarization angle, which in turn prevents the PC from outputting the correct heading angle. When shooting on a rapidly maneuvered carrier, the PC can theoretically remain relatively still in the current scene as long as the exposure time is sufficiently short. This is because shorter exposure times reduce motion blur and enhance image sharpness. However, in practice, the exposure time of the camera cannot be set too short due to specific brightness requirements for polarization heading resolution. If the exposure time is excessively short and insufficient light reaches the sensor, it will significantly increase calculated heading angle errors and may even cause algorithm failure. Therefore, solely reducing exposure time and adjusting sensor gain are not feasible approaches to mitigate high mobility’s adverse effects on PC performance. In fact, during actual motion, the image is directly captured on the CMOS sensor, resulting in a blurred photograph with trailing effects. Consequently, PC will inevitably produce outliers. Therefore, to optimize the overall performance of the integrated navigation system, it is imperative to mitigate these adverse effects while simultaneously enhancing navigation accuracy and stability. To assess the efficacy of the proposed algorithm in addressing the aforementioned issues, additional tests were conducted using a vehicle to execute high-speed maneuvers and turns for the INS/PC system. Specifically, high-speed movements and multiple turns were performed on the PC for a specific duration, as outlined in

Table 3. Experimental procedures for dynamic conditions.

Case	Detailed Operation
Case1	Traverse the shaded road section.
Case2	Continuously making multiple turns and randomly select one.
Case3	The carrier exhibits high mobility and any segment of it can be selectively utilized.

.

To evaluate the efficacy of the proposed algorithm in dynamic scenarios, a 1200 s dynamic experiment was conducted on 24 June 2025, at 4:30 p.m. under clear weather conditions.

According to the results presented in Figure 3 and Figure 4 as well as

Table 4. Max, Min, Mean, and RMS of ablation experiment heading angle error for various methods (unit: °).

Method	Max	Min	Mean	RMS	Improve
INS	10.66	–1.07	3.98	4.78	/
PC	153.90	–39.85	0.25	5.65	/
AKF	10.64	–1.07	3.97	4.76	/
ISRAKF	9.16	–3.60	0.24	1.10	76.89%
VBAKF	16.76	–13.71	0.21	1.56	67.23%
Proposed	3.59	–2.09	0.18	0.74	84.45%

, the proposed VISKF algorithm demonstrates effective suppression of outliers in PC during vehicle turning and high-speed movement, while also exhibiting high maneuverability.

The proposed algorithm demonstrates an 84.45% improvement in accuracy compared to the AKF algorithm. Furthermore, it is evident that the various modules of the VISKF algorithm maintain their effectiveness under dynamic conditions.

As depicted in Figure 5 and Figure 6, as well as

Table 5. Max, Min, Mean, and RMS of experiment heading angle error for various methods (unit: °).

Method	Max	Min	Mean	RMS	Improve
AKF	10.64	−1.07	3.97	4.76	/
Huber-based RAKF	152.40	−39.46	0.29	3.60	24.37%
MMCKF	151.40	−39.20	0.31	2.56	46.21%
Proposed	3.59	−2.09	0.18	0.74	84.45%

, the proposed VISKF algorithm exhibits superior performance in enhancing navigation accuracy and system robustness compared to alternative algorithms, thereby significantly outperforming them.

The complexity of time-varying noise encountered by PC in dynamic conditions is primarily attributed to environmental factors, sensor characteristics, and variations in system state. These elements collectively contribute to the temporal variability of noise characteristics and distributions. However, the Huber-based RAKF algorithm presumes the reliability of external measurements and therefore lacks the capability to simultaneously address the coexistence of measurement and process uncertainties. Furthermore, the Huber-based RAKF algorithm fails to timely adjust its parameters to accommodate new noise characteristics, potentially leading to inaccurate estimation outcomes. MCCKF is intrinsically limited by the Gaussian kernel’s local approximation capacity when confronted with extremely heavy-tailed or unknown noise distributions; it can only mediate between over-smoothing and over-sensitivity through a bandwidth parameter that must be set a priori. In contrast, the proposed algorithm adopts a heavy-tailed Student’s t noise model and leverages online variational Bayes to directly estimate the time-varying scale matrix. This formulation preserves higher-order statistical information while eliminating the need for an exact, pre-specified covariance. Consequently, VISKF demonstrates markedly superior adaptability and robustness in environments characterized by severe outliers and complex, non-Gaussian noise structures.

In the above field experiments, the degree of freedom parameter and the number of iteration in the proposed method are chosen as $\gamma =5$ and $N=10$. and the initial value of the saturation boundary is chosen as ${\alpha }_0=9$. The other parameters in the innovation saturation mechanism are set as ${\eta }_1=0.01$, ${\eta }_2=0.01$ and ${\xi }_k=0.5\times abs(e_k)/e_k$.

The setting parameters are set as follows:

$${\widehat{x}}_{0\left|0\right.}=zeros\left(15,0\right),$$

(40)

$$P_{0\left|0\right.}=diag{\left\{\left[\nabla \varphi ;\nabla V;\nabla P;{\omega }^b;f^b\right]\right\}}^2,$$

(41)

$$Q_0=diag{\left\{\left[{\sigma }_g^b;{\sigma }_a^b;zeros\left(9,1\right)\right]\right\}}^2,$$

(42)

$$R_0=diag{\left(\delta \psi \right)}^2,$$

(43)

where

$$\left\{\begin{array}{l}\nabla \varphi =\left[0.18°,0.04°,-0.01°\right]\\ \nabla V=\left[0.1\mathrm{m}/\mathrm{s},0.1\mathrm{m}/\mathrm{s},0.1\mathrm{m}/\mathrm{s}\right]\\ \nabla P=\left[0.5\mathrm{m},0.5\mathrm{m},0.5\mathrm{m}\right]\\ {\omega }^b=\left[0.02°/\mathrm{h},0.02°/\mathrm{h},0.05°/\mathrm{h},\right]\\ f^b=\left[2000\mathsf{\mu }\mathrm{g},1400\mathsf{\mu }\mathrm{g},500\mathsf{\mu }\mathrm{g}\right]\\ {\sigma }_g^b=\left[0.028°/\mathrm{h},0.028°/\mathrm{h},0.028°/\mathrm{h}\right]\\ {\sigma }_a^b=\left[5\times {10}^{-3}\mathrm{g},5\times {10}^{-3}\mathrm{g},5\times {10}^{-3}\mathrm{g}\right].\end{array}\right.$$

(44)

The variational Bayesian method and innovation saturation mechanism introduce additional computational complexity. Consequently, this paper conducts a real-time performance analysis of AKF, ISRAKF, and VBAKF. The single-step implementation CPU time of various algorithms is given by

Table 6. Single-step runtime of various algorithms (unit: s).

	AKF	ISRAKF	VBAKF	Proposed
CPU Time	2.47 × 10−6	2.59 × 10−6	2.83 × 10−5	2.84 × 10−5

. The experiments were performed on an AMD Ryzen 9 7945HX CPU (Lenovo Limited, Beijing, China). CPU time is crucial in real-time systems. If CPU time is excessively prolonged, real-time requirements may not be satisfied. Through the reasonable parameter selection, the proposed algorithm can meet the real-time requirements of the INS/PC integrated navigation system.

5. Conclusions

In this paper, an improved adaptive Kalman filter is proposed to solve the problem of measurement outliers caused by PC in occlusion environments and during turns and high-speed movements in the INS/PC integrated navigation system. The proposed VISKF algorithm uses the STD to better approximate the posterior distribution of PC measurement noise, thereby overcoming the measurement anomalies faced by PC in complex environments when performing navigation information fusion with INS. In addition, a saturation function is introduced to suppress the divergence of the update vector, further eliminating outliers. The experimental results show that the algorithm has a very precise and sensitive response to the noise and outliers of PC. In future work, additional experiments will be conducted in more diverse and challenging scenarios, such as underwater and aerial environments. In addition, the complex meteorological conditions are also an important factor affecting the accuracy of polarization orientation. Next, we will also conduct experiments under various weather conditions (such as cloudy, rainy, etc.) to further verify the adaptability of the proposed algorithm in a more comprehensive manner. Furthermore, research into more flexible and general noise models capable of handling a wider range of noise types will be pursued. Additionally, exploring the application of noise models to different fields, including autonomous driving, UAV navigation, and robot navigation, will be important directions for future research.