Novel Gain-Optimized Two-Step Fusion Filtering Method for Ranging-Based Localization Using Predicted Residuals

Abstract

A two-stage fusion filtering positioning algorithm based on prediction residuals and gain adaptation is proposed to address the problems of disturbance and modeling errors in the application of distance-based positioning algorithms in wireless sensor networks, as well as inaccurate initial filtering values leading to large estimation errors or even divergence. Firstly, based on parameterization methods, a pseudo linear equation is constructed from the time of arrival (TOA) and multipath delay. The weighted least squares (WLS) method is applied to obtain the initial value of target position resolution, and its performance approaches the Cramér–Rao lower bound (CRLB). Secondly, the exact position of the target is obtained using the reconstructed Gaussian white noise statistics and the Kalman filtering algorithm. The simulation results show that compared with other initial positioning algorithms, the average positioning accuracy of the proposed algorithm is improved by 28.57%, and it has a better filtering performance.

1. Introduction

Wireless sensor networks (WSNs) have functions such as wireless communication and information collection and processing. They can combine wireless communication and network technology to perceive, transmit data, and provide terminal services for the target object in a monitoring environment [1,2]. The prominent feature of WSNs applied to the Internet of Things (IoT) architecture is the random broadcasting and distributed computing of a large number of nodes. It is an unattainable advantage of other detection means using sensor nodes carrying signal processing devices to implement information collection and conversion, especially to complete specific tasks in inaccessible military, civilian, and commercial areas, such as positioning and tracking [3,4]. In various engineering fields, the integration of data-driven approaches represented by artificial intelligence (AI) and optimization techniques has shown significant potential for improving system performance. For example, these techniques have been used in thermal engineering [5,6,7] and renewable energy consumption [8,9]. Similarly, in wireless sensor networks (WSNs), advanced optimization and AI techniques can enhance the accuracy and robustness of localization algorithms, improving overall performance. Nodes are at the very end of WSN applications, and the location information of nodes or targets is related to the application quality requirements of the scene and the implementation of monitoring networks. The techniques for implementing position estimation mainly include distance, time, and angle measurement, as well as the collection, organization, and processing of other crucial information that can participate in positioning. In terms of positioning quality, qualitative judgment and quantitative calculation of performance indicators such as computational complexity and real-time requirements should also be considered [10]. The two-dimensional positioning application scenario has led to extensive applications in various industrial fields, such as highways, medicine, public places, and emergency rescue. The current highly studied three-dimensional location estimation can fully exploit the potential of location perception and realize position-based advanced service functions, for which the classification and fusion technology of positioning has emerged [11,12].

In the study of positioning problems in WSN, according to whether distance is measured, the positioning algorithm can be divided into two kinds: ranging-based and non-ranging. Generally, the ranging-based positioning method has a higher accuracy. Common ranging methods such as TOA (time of arrival), AOA (angle of arrival), TDOA (time difference of arrival), FOA (frequency of arrival), and RSS (received signal strength) belong to the ranging-based techniques.

TOA technology is widely used in target positioning scenarios with high-performance requirements in indoor and outdoor areas due to its accurate distance measurement. A positioning network can estimate the location of massive nodes or targets by calculating the arrival time received by at least three sensor nodes, or implement precise positioning using collaborative techniques, ranging and inertial combination, and combined spatial–temporal arrays [13,14,15]. For example, Garraffa et al. [16] proposed a control-theoretic framework for range-inertial localization, rigorously analyzing error dynamics and convergence properties—a theoretical cornerstone for our filtering approach. Complementary to this, Feng et al. [17] demonstrated a Kalman filter-based fusion of UWB and IMU data, effectively suppressing observation noise and enhancing robustness in indoor navigation. In positioning methods based on distance measurement or angle measurement, such as AOA, RSS, and FOA, high-precision experimental results can be achieved if two or three of them are used for mixed positioning. In [18], the author studied the optimal sensor placement strategy for single static target positioning using mixed RSS, AOA, and TOA in a 2D plane, derived the theoretical minimum trace of the Cramér–Rao lower bound (CRLB) using the optimal criterion, proposed the corresponding optimal sensor placement strategy, and finally verified the results through simulation examples. In [19], the authors used TOA and RSS measurements to accurately locate the target in a non-line of sight (NLOS) environment, enhancing positioning accuracy. Starting from the general approximation of the maximum likelihood (ML) estimation problem, the authors established the nonlinear weighted least squares (NLWLS) using the equilibrium parameter, then used semi-definite relaxation (SDR) technology to solve the NLWLS problem, and the numerical simulation proved that the proposed method significantly outperforms the existing methods.

To improve the positioning accuracy of the WSN network, Kalman filtering and particle filtering methods can also be used. The Kalman filtering is an efficient recursive filter capable of estimating the states of dynamic systems from a series of incomplete and noisy inclusive measurements and is particularly suitable for handling linear systems and systems with Gaussian white noise. Particle filtering can deal with nonlinear, non-Gaussian localization problems caused by environmental complexity (such as multipath effect, NLOS propagation). In practical applications, the two methods can also be combined to further improve positioning accuracy and robustness. The development of Kalman filtering series algorithms has gone through several stages, from the initial Kalman filter (KF) to the extended Kalman filter (EKF) for nonlinear systems, to the untracked Kalman filter (UKF), these algorithms have played an important role in their respective application fields.

Although the particle filter series algorithm is effective when dealing with nonlinear systems, the particle filter has the disadvantages of degradation and a large amount of computation, making it difficult to the strict requirements of limited positioning resources in WSN. Therefore, the Kalman filter series algorithm is the better method to deal with the positioning problem of nonlinear systems based on ranging [20,21]. The series algorithms of Kalman filtering have developed rapidly in recent years and have been applied in various data and information processing scenarios [22,23]. In the field of positioning, tracking, and navigation based on ranging technology, it is often seen that Kalman filtering has a superior performance in accurate estimation, which is used to reduce the process or measurement error, improve the positioning accuracy, and obtain satisfactory positioning tracking and navigation effects [24,25,26,27]. UKF is a filtering method for the nonlinear system and its core is the differential-free suboptimal algorithm of unscented transformation (UT), that is, from approaching a nonlinear function to approaching a probability distribution, so that the calculation process is optimized and the filtering effect is significantly improved. Therefore, many application scenarios have been developed [28,29,30,31].

Many factors in the system positioning process adversely affect the location estimation, and this paper mainly considers the decrease in localization accuracy caused by ranging error and colored measurement noise. Based on the above requirements of ranging problems and real-time positioning, this study mainly makes the following contributions:

• Setting the distance measurement based on TOA as a Gaussian stochastic process, adopting the nonlinear system parametric method to derive the pseudo-linear equation with unknown multipath delay, then applying the weighted least squares (WLS) criterion and the effective analytical calculation of process parameters to obtain the approximate solution for the target primary position estimation that can approximate the CRLB based on TOA measurement estimation.
• Realizing the optimal estimation, using the new Gaussian distributed white noise statistic to whiten the observed colored noise to eliminate the adverse effect of the colored noise on the filter localization, and update the Kalman filter parameters after obtaining the new measurement model.
• Setting the correlation coupling matrix to realize the decoupling according to the independent conditions of process and observation error, to derive the filter decoupling measurement model, and then update the filter parameters.
• Because of the problem of error in noise modeling, realizing self-optimization filtering estimation using predicting residual statistics, noise covariance, and gain update, and by compensating the estimation error again to accelerate localization convergence and enhance the robustness of the system. Finally, the feasibility and effectiveness of the positioning algorithm are verified by numerical simulation experiments.

2. Related Work

The scenario based on location information mainly appears in various application fields, and location estimation can make full use of the potential of position perception and realize location-based advanced service functions, so the technology and algorithm development on positioning have shown an amazing development speed. In [32], the authors proposed a robust localization algorithm based on a Gaussian mixture model and fitting polynomials for the positioning error problem in complex indoor environments. Firstly, the predicted and measured residues are clustered using the Gaussian mixture model (GMM) and the line of sight (LOS) probability and non-line of sight (NLOS) probability are calculated; then, the measured values are filtered through the Kalman filter (KF), variable parameter unscented Kalman filter (VPUKF), and variable parameter particle filter (VPPF); finally, position coordinate estimates are calculated using the maximum likelihood method. The simulation results showed that the proposed algorithm has a better localization accuracy and robustness than several other comparison algorithms. In [33], the authors proposed a new robust matrix approximation scheme using multidimensional similarity (MDS) analysis under non-line of sight (NLOS) propagation and applied the multiplier’s alternating direction method to solve the resulting nonlinear constraint optimization problem. The simulation results showed that the proposed method is superior to several existing methods. In the ranging positioning technology based on TOA, the time synchronization problem is relatively prominent, and when the asynchrony is serious, the positioning error will be too large or even result in positioning failure. Therefore, it is necessary to put forward the positioning algorithm to consider the synchronization problem [34]. In [35], the authors analyzed the strict time synchronization between the source and the base station required for TOA localization, and introduced the synchronization error into the TOA-based NLOS localization model, considering that NLOS propagation significantly reduces positioning precision.

Similarly, to solve the asynchronous positioning problem, in [36], considering the positioning and tracking in the application of autonomous underwater vehicles (AUVs), the authors first developed an asynchronous positioning algorithm to estimate the location of AUVs and designed a model-free tracking controller for AUVs to track the reference trajectory. In addition, they derived the convergence conditions and CRLB of the localization algorithm, constructed the Lyapunov function to analyze its stability, and verified its effectiveness through simulation experiments, using an optimization scheme to achieve high-precision positioning is an effective way to further reduce position errors. In [37], the authors studied the localization of multiple signal sources, proposed a self-clustering measurement combination method, using the source location estimation obtained by hyperbolic positioning algorithm as the clustering mode, defined the scatter of the patterns of different subsets of TOA measurements as the criterion function, adopted the three-step heuristic clustering algorithm to solve the TOA ambiguity, and analyzed the mean square error performance and computational complexity. In [38], the authors applied collaborative technology to achieve positioning and proposed a non-convex robust weighted least squares (RWLS) problem to locate multiple stationary target nodes. With the help of semi-definite relaxation technology, the RWLS problem is transformed into a convex semi-definite second-order cone hybrid programming problem, and both the simulation and actual experimental data verified the superior performance of the proposed method. Back to the positioning model itself, in [39], based on the single mobile station positioning system, according to the available measurement, including AOA, TOA, and FOA, the authors derived the corresponding pseudo linear equation, established the total least squares optimization model, and used the inverse iterative algorithm to solve the static target position with a theoretical derivation and rigorous proof to achieve CRLB performance.

The positioning algorithms based on single ranging or joint ranging listed above solve some problems from the perspective of different application scenarios, have certain applicability in specific environments, and provide ideas for future positioning exploration. With the change in scene time and space, it is still necessary to develop new technologies for different positioning environments and constantly innovate and improve them.

Kalman filtering methods have been widely applied in data and information processing scenarios of various dynamic systems. Kalman filtering in the field of positioning, tracking, and navigation has a superior performance and is used to reduce process or measurement error and improve positioning accuracy. The study of Gaussian white noise is relatively mature, but colored noise is widespread in actual localization systems, so it is necessary to study the latter [40]. One of the biggest challenges encountered in applying Kalman filtering in location-tracking scenarios based on ranging technology is the noise processing problem at the measurement end. This problem often shows us how to deal with the colored noise to make it white, to be suitable for Kalman filtering, and to obtain improved positioning accuracy and good robustness [41,42]. In [41], since the existence of colored noise in global navigation satellite system (GNSS) measurement will reduce the performance of fault detection and exclusion (FDE) algorithm based on KF, the authors proposed an FDE scheme based on improved KF, using the colored noise as a first-order self-regression model to improve the performance of FDE, and evaluated the performance of the proposed algorithm through real-time kinematic positioning application, obtaining good positioning results. In [42], considering state coupling and colored noise among system nodes, the authors designed a robust extended state Kalman filter estimation algorithm for nonlinear complex networks. The innovative measurement information subtraction method at adjacent moments deals with colored noise, which greatly reduces the influence of network bandwidth constraints, obtains upper bounds of error covariance, and achieves real-time gain optimization. The rationality and accuracy of the proposed estimator are verified by simulation.

It can be seen from the above studies that some effective methods have been developed for the problem of reducing the algorithm performance in the measurement process contaminated by colored noise. However, most algorithms still have the problem of the processed white noise sequence involving the coupling between the system process and the observation process. In addition, some problems such as the cumbersome algorithm design process and the increasing estimation cost with the sampling frequency are prominent. Therefore, the robustness and efficiency of the algorithm should be the focus of the next consideration.

UKF propagates statistical properties via no-trace nonlinear transformations and employs weighted statistical linear regression models to calculate the mean and covariance of random variables to obtain updates of the state estimates, especially suitable for target localization based on nonlinear ranging scenarios. In [43], the authors consider that the choice of process noise covariance matrix and measurement noise covariance matrix will directly affect the filtering accuracy of the algorithm, and propose using a hybrid algorithm including UKF and the genetic particle swarm algorithm to estimate several key states of the vehicle. The simulation and real vehicle test results show that the algorithm has a higher accuracy and less computational complexity than the traditional estimators based on UKF and traceless particle filters. In [44], the authors extended the special requirements for positioning application scenarios, designed and developed a positioning framework composed of three algorithms, proposed an improved Kalman filter positioning method based on UKF and PF positioning, and verified the robustness of the proposed algorithm through numerical simulation. The simulation results show that the proposed positioning algorithm can be used for target tracking, robot positioning, and other purposes.

The above UKF-based filtering improvement algorithm or the combined filtering positioning algorithm have achieved a good application effect in different positioning environments. However, some algorithms only provide beneficial exploration, but it is difficult to achieve in practice. With the change in scene time and space, it is still necessary to develop new technologies to continuously innovate and improve from the aspects of improving performance and stronger adaptability.

This work proposes a two-stage fusion filter positioning algorithm, which can be quickly converged based on the original algorithm performance and achieve the purpose of optimizing the performance. The techniques used in this paper include Gaussian noise modeling, the analytical method of weighted least squares criterion, derivation of the Cramér–Rao lower bound, colored noise signal processing, multi-source information fusion, and gain adaptive filtering, etc. Comprehensive application of these technologies in target positioning can obtain the precise position of the target, enhance the robustness of the system, and obtain a better positioning effect. The experimental results also demonstrate that the fusion algorithm is consistent with the theoretical derivation, the average localization accuracy improves, and the system has a better accuracy and filtering stability.

3. TOA Ranging Modeling and Initial Positioning

3.1. Location Information Modeling

If TOA ranging is used for positioning, assuming that the electromagnetic pulse is emitted at time t₀, the arrival time of the receiving pulse at time t_i, $t_i$ can be expressed as follows.

$$t_i=t_0+{\displaystyle \frac{D_i}{c}}$$

(1)

where $D_i$ is the distance between the transmitter and the receiver, and c is the light speed.

Considering the influence of path delay in the wireless signal transmission process, the transmission mode between any two points on the signal path is nonlinear propagation, which is usually processed mathematically as propagation along a straight line, thus forming a ranging error. Therefore, considering the integrated path delay influence, the wireless signal path observation equation based on TOA is as follows:

$$t_i=t_0+t_y+{\displaystyle \frac{D_i}{c}}$$

(2)

where $t_y$ is the channel integrated path delay.

In this work, the 3D case is considered, where the position of the target node or the anchor node is represented by a column vector consisting of three coordinates. Let the set consisting of N target nodes be ${\Psi }_m=\left\{m_1,m_2,\cdots ,m_N\right\}$, randomly or consciously deployed in a three-dimensional region. The true position of the target is represented as $z={\left[z_x, z_y{,z}_z\right]}^T$, where z is unknown and its estimated position is represented as $\widehat{z}={\left[{\widehat{z}}_x,{\widehat{z}}_y,{\widehat{z}}_z\right]}^T$. Moreover, assuming that M anchor nodes are already equipped with GPS or have obtained precise position by the positioning algorithm, the position of the anchor nodes can be expressed as $m={\left[m_x,m_y,m_z\right]}^T$, and M anchor nodes can be expressed as ${\Psi }_a=\left\{a_1,a_2,\cdots ,a_M\right\}$. Generally, there is $M\ll N$, and there are at least 4 anchor nodes within the communication range of each target node.

The TOA-based measured distances between the target and the anchor nodes are modeled as

$$R_{ik}=c\left(t_{ik}-t_0-t_y\right)=‖z_i-m_k‖+v_{ik}$$

(3)

where $\Vert \ast \Vert$ is the norm operator, and $v_{ik}$ $\left(k=1, 2, \cdots , M\right)$, modeled as the corresponding measurement noise, is a random variable following an independent Gaussian homogeneous distribution with a mean of zero.

The location of target nodes can be determined using target positioning technology and obtained as accurately as possible using $R_{ik}$ with noise. In this paper, we consider using the measurement information to implement the simultaneous estimation of $z_i$ and $t_y$.

3.2. Positioning Algorithm Based on the Observed Pseudo-Linearized Analytical Solution

Based on the TOA measurement equation shown in Equation (3), assuming that the estimates of $z_i$ and $t_y$ are ${\widehat{z}}_i$ and ${\widehat{t}}_y$, respectively, the square of Equation (3) is treated and adjusted, and set ${\left(c{\widehat{t}}_y\right)}^2-{‖{\widehat{z}}_i‖}^2=\varrho$, then

$${\left(c{t}_{ik}-{ct}_0\right)}^2-{‖m_k‖}^2+\varrho =2\left(c{t}_{ik}-{ct}_0\right)c{\widehat{t}}_y-2{m_k}^T{\widehat{z}}_i+2\left(c{t}_{ik}-{ct}_0-c{\widehat{t}}_y\right)v_{ik}-{v_{ik}}^2$$

(4)

The matrix form of Equation (4) is given by

$$A_c+1\varrho =A_t\theta +V_v$$

(5)

where

$$\begin{gathered} A_c=\left[\begin{array}{c}{\left(c{t}_{i1}-{ct}_0\right)}^2-{‖m_1‖}^2\\ ⋮\\ {\left(c{t}_M-{ct}_0\right)}^2-{‖m_M‖}^2\end{array}\right], 1=\left[\begin{array}{c}1\\ ⋮\\ 1\end{array}\right], \\ {A}_t=\left[\begin{array}{c}2c\left(c{t}_{i1}-{ct}_0\right)-2{m_1}^T\\ ⋮\\ 2c\left(c{t}_M-{ct}_0\right)-2{m_M}^T\end{array}\right], \\ {V}_v=\left[\begin{array}{c}2\left(c{t}_{i1}-{ct}_0-c{\widehat{t}}_y\right)v_{i1}-{v_{i1}}^2\\ ⋮\\ 2\left(c{t}_{iM}-{ct}_0-c{\widehat{t}}_y\right)v_{iM}-{v_{iM}}^2\end{array}\right] \end{gathered}$$

$\theta ={\left[\begin{array}{cc}{\widehat{t}}_y& {{\widehat{z}}_i}^T\end{array}\right]}^T$ is the unknown vector to be estimated, and $V_v$ is the noise vector.

For non-Gaussian noise $V_v$, assuming its variance is ${\sigma }_n^2$, by setting a vector $R_v$ and using ${R_v}^{-1/2}$ to achieve non Gaussian noise processing, that is, by calculating ${R_v}^{-1/2}{V}_v$ and obtaining standard Gaussian noise, it should be multiplied ${R_v}^{-1/2}$ at both ends of the Equation (5). This is a general process for handling non Gaussian noise.

Assuming $E\left[V_v{V_v}^T\right]/{\sigma }_n^2=R_v$, non-Gaussian noise is processed into Gaussian noise, yielding

$${R_v}^{-1/2}{A}_c+{R_v}^{-1/2}1\varrho ={R_v}^{-1/2}{A}_t\theta +{R_v}^{-1/2}{V}_v$$

(6)

Thus, the estimate of θ is

$$\widehat{\theta }=\alpha +\beta \varrho$$

(7)

where

$$\begin{gathered} {\alpha =\left({A_t}^T{R_v}^{-1}{A}_t\right)}^{-1}{A_t}^T{R_v}^{-1}{A}_c \\ \beta ={\left({A_t}^T{R_v}^{-1}{A}_t\right)}^{-1}{A_t}^T{R_v}^{-1} \end{gathered}$$

then

$$R_v=E\left[V_v{V_v}^T\right]/{\sigma }_n^2=\left[\begin{array}{ccc}4{‖{\widehat{z}}_i-m_1‖}^2+{3\sigma }_n^2& \cdots & {\sigma }_n^2\\ ⋮& \ddots & ⋮\\ {\sigma }_n^2& \cdots & 4{‖{\widehat{z}}_i-m_M‖}^2+{3\sigma }_n^2\end{array}\right]$$

The initial value of $R_v$ is usually set to be the unit matrix I, and weighted least squares are applied for the $R_v$. There is usually no prior value for ${\sigma }_n^2$ in the actual localization, and in subsequent calculations, $R_v$ is set to

$$R_v\approx diag\left\{{4\left(c{\mathrm{t}}_{i1}-{\mathrm{c}\mathrm{t}}_0-c{\widehat{t}}_y\right)}^2,\cdots ,{4\left(c{\mathrm{t}}_{iM}-{\mathrm{c}\mathrm{t}}_0-c{\widehat{t}}_y\right)}^2\right\}$$

(8)

Set the estimate of ϱ as $\widehat{\varrho }$, then

$${\beta }^{T}Bb{\widehat{\varrho }}^2+\left(2{\beta }^{T}B\alpha -1\right)\widehat{\varrho }+{\alpha }^{T}Q\alpha =0$$

(9)

where $B=\left[\begin{array}{ccc}1& 0& 0\\ 0& -1& 0\\ 0& 0& -1\end{array}\right]$. Therefore, we obtain the $\widehat{\varrho }$ by analytical solution and can then compute $\widehat{\theta }$. Register ${\left(2{\beta }^{T}B\alpha -1\right)}^2-4{\beta }^{T}B\beta {\alpha }^{T}Q\alpha =\mathsf{\gamma }$; then, for the case of ${\beta }^{T}B\beta =0$, the solution of $\widehat{\varrho }$ is

$$\widehat{\varrho }={\displaystyle \frac{{\alpha }^{T}B\alpha }{1-2{\beta }^{T}Q\alpha }}$$

Thus, the localization solution is

$$\widehat{\theta }=\left[\begin{array}{c}{\widehat{t}}_y\\ {\widehat{z}}_i\end{array}\right]=\alpha +\beta {\displaystyle \frac{{\alpha }^{T}B\alpha }{1-2{\beta }^{T}B\alpha }}$$

(10)

If ${\beta }^{T}B\beta \ne 0$ and $\mathsf{\gamma }=0$, there is a unique solution of $\widehat{\varrho }$, and the localization solution is

$$\widehat{\theta }=\left[\begin{array}{c}{\widehat{t}}_y\\ {\widehat{z}}_i\end{array}\right]=\alpha +\beta {\displaystyle \frac{1-2{\beta }^{T}B\alpha }{2{\beta }^{T}B\beta }}$$

(11)

If ${\beta }^{T}B\beta \ne 0$ and $\mathsf{\gamma }>0$, there are two different solutions of $\widehat{\varrho }$, and the localization solution is

$$\widehat{\theta }=\left[\begin{array}{c}{\widehat{t}}_y\\ {\widehat{z}}_i\end{array}\right]=\alpha +\beta {\displaystyle \frac{1-2{\beta }^{T}B\alpha \pm \sqrt{{\left(2{\beta }^{T}B\alpha -1\right)}^2-4{\beta }^{T}B\beta {\alpha }^{T}B\alpha }}{2{\beta }^{T}B\beta }}$$

(12)

Solving the uniqueness problem of the localization solution can be obtained using the least squares criterion based on the TOA measurement.

$$\underset{{\widehat{t}}_y,{\widehat{\mathit{z}}}_i}{\min }\sum _{k=1}^M{\left(‖{\widehat{z}}_i-m_k‖-c{\mathrm{t}}_{ik}+{\mathrm{c}\mathrm{t}}_0+c{\widehat{t}}_y\right)}^2$$

(13)

In other cases, there is no effective solution, and at this time, the effective measurement value cannot be obtained, so other alternative algorithms can be sought to solve it. The details of the initial localization algorithm steps are illustrated in Algorithm 1.

Algorithm 1. The initial localization algorithm

1: Initialization, set $R$, calculate $\alpha$, $\beta$, and $\mathsf{\varrho }$.

2: If $\mathsf{\gamma }<0$, then select the other algorithm as an alternative.

3: If ${\beta }^{T}B\beta =0$, then have $\widehat{\theta }$ from Equation (10).

4: If $\mathsf{\gamma }=0$, then have $\widehat{\theta }$ from Equation (11).

5: If $\mathsf{\gamma }>0$, then obtain two $\widehat{\theta }$ from Equation (12).

6: the uniqueness of the solution can be solved by Equation (13).

7: return.

4. Self-Optimized Target Filter Localization Method with Colour Noise Observation

In terms of processing strong nonlinear systems such as target positioning systems, the significant advantages of the unscented Kalman filter (UKF) algorithm are filtering accuracy, algorithm stability, and computational load. As with the basic linear filtering method, for the UKF algorithm, we also assume that the process noise and observation noise are Gaussian white noise. However, in the location tracking application, influenced by the environment and facilities, the algorithm model will include colored noise characteristics, which may usually affect the filter accuracy or directly lead to filter divergence.

4.1. Filter Localization Problem Modeling and the Basic UKF Algorithm

4.1.1. Filter Localization Problem Modeling

If a target moves in a constant straight line in a short period, the acceleration is theoretically 0. But in practice, the acceleration of the target affected by itself and the environment is abstracted as a random variable, which can be modeled as a zero-mean Gaussian white noise process at time t, namely $\ddot{x}=\complement \left(t\right)$, whose mean is $E\left[\complement \left(t\right)\right]=0$ and covariance is $E\left[\complement \left(t\right)\complement \left(\tau \right)\right]=q\left(t\right)\delta \left(t-\tau \right)$, where $\delta \left(t-\tau \right)=0$ at $t\ne \tau$ and $\delta \left(t-\tau \right)=1$ at $t=\tau$; that is, the white noise is only correlated with its current moment.

Suppose that the position coordinates and speed of the target in the 3-D system are $\left(x,y,z\right)$ and $\left(v_x,v_y,v_z\right)$, respectively, and its state vector can be expressed as $X=[X_p,X_v]$, where X represents the state vector of the positioning system, $X_p={\left[\begin{array}{ccc}x& y& z\end{array}\right]}^T$, and $X_v={\left[\begin{array}{ccc}{v}_x& v_y& v_z\end{array}\right]}^T$. Then, the continuous-time equation of the state of the system is expressed as follows:

$$\dot{X}\left(t\right)=AX\left(t\right)+BW\left(t\right)$$

(14)

where A represents the state transition matrix, B is the process noise allocation matrix, and W(t) represents the process noise, which is a mutually independent white noise vector component with a zero-mean Gaussian distribution. To facilitate digital filtering estimation, the discretized equation of state is set as

$$X_{k+1}=f\left(X_k\right)+W_k=F{X}_k+B{W}_k$$

(15)

where $F=\left[\begin{array}{cc}{1}_{3\times 3}& T_{3\times 3}\\ O_{3\times 3}& 1_{3\times 3}\end{array}\right]$ represents the state transition matrix from discrete time k to k + 1; k represents the discrete time; T represents the sampling time; $X_{k+1}$ represents the state vector of the system at the discrete time k + 1; B represents the process noise allocation matrix from discrete time k to k + 1; and $W_k$ represents the process noise at discrete time k, which satisfies $E\left[W_k\right]=0$ and $cov\left[{W_{k}W}_j\right]=Q_k{\delta }_{kj}$.

The measuring device can obtain the distance information from the anchor node to the target. For the TOA of the received signal at the anchor node, the distance was calculated using the equation $d=cT=‖z_i-m_k‖$. Thus, the distance measurements can be modeled as

$$d=h\left(x\right)=cT=‖z_i-m_k‖$$

(16)

where $h\left(x\right)$ is a nonlinear function of the calculated distance information. Considering the observation error of the measurement equipment, the following observation equation can be established

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

(17)

where $f\left(X_k\right)$ is the vector of nonlinear observation functions, and $V_k$ is the observation noise vector of the measurement device.

4.1.2. The Basic UKF Algorithm

The unscented transformation (UT) can directly find the mean and covariance of the target distribution, avoiding the approximation of nonlinear function, so the unscented filtering effect is next only to particle filtering, but the computational load is less, which is more suitable for low-dimensional strong nonlinear systems such as ranging positioning.

For KF class filtering, we consider only the additive noise, namely, both $W_k$ and $V_k$ are zero-mean Gaussian white noise. The process noise variance of the positioning system is

$$E\left[\left(B_k{W}_k\right){\left(B_k{W}_k\right)}^T\right]=E\left[\left(B_k\left(W_k{W}_k^T\right)B_k^T\right)\right]=E\left[\left(B_k{\mathrm{\mho }}_k{B}_j^T\right)\right]=Q_k{\delta }_{kj}$$

(18)

where ${\delta }_j$ is the $\mathrm{K}\mathrm{r}\mathrm{o}\mathrm{n}\mathrm{e}\mathrm{c}\mathrm{k}\mathrm{e}\mathrm{r}-\delta$ function, taking 1 if $k=j$ and 0 if $k\ne j$. The variance of the observed noise $V_k$ is $R_k$, which satisfies $E\left[\left(V_k{V}_j^T\right)\right]=R_k{\delta }_{kj}$, and assumes that $W_k$ is not correlated with $V_k$.

In this study, a scaled UT transformation method called SUT transformation is adopted to give full play to the filtering performance. The sigma points ${\chi }_{k-1}^0,{\chi }_{k-1}^1,\cdots ,{\chi }_{k-1}^{2n}$ are selected according to the selection rule using Equation (19).

$$\left\{\begin{array}{c}{\chi }^0=\overline{x},\hfill \\ {\chi }^i=\overline{x}+\sqrt{n+\lambda }{\left[\sqrt{P_x}\right]}_i\hfill \\ {\chi }^{n+i}=\overline{x}-\sqrt{n+\lambda }{\left[\sqrt{P_x}\right]}_i,\hfill \end{array}\right.,i=1,\cdots ,n$$

(19)

Using Equation (20) to determine the weight value $w_0,w_1,\cdots ,w_{2n}$,

$$\left\{\begin{array}{c}{w}_0^{\left(m\right)}=\lambda /(n+\lambda ),\hfill \\ w_0^{\left(c\right)}=\lambda /(n+\lambda )+(1-{\alpha }^2+\beta )\hfill \\ w_i^{\left(m\right)}=w_i^{\left(c\right)}=1/(2n+2\lambda ),\hfill \end{array}\right.,i=1,\cdots ,2n$$

(20)

where $\lambda ={\alpha }^2(n+\kappa )-n$. We set the value range of parameter α as ${10}^{-4}\le \alpha <1$, $\kappa$ we usually select as 0 (state dimension $N\ge 3$) or $3-n$ (state dimension $N<3$), and we select β = 2 when the noise is a Gaussian distribution. ${\left[·\right]}_i$ represents column i of the matrix, and each sigma point constitutes each column of the sigma matrix.

The computational flow of the basic UKF algorithm is described as follows:

(1) Initialization

Step 1: Initialize the state ${\widehat{X}}_0$ and its covariance $P_0$, $Q_0$, and $R_0$; ${\widehat{X}}_0$ is the state estimation vector at the initial time (time 0); $P_0$ is the state covariance matrix at the initial time, $Q_0$ is the initial process noise variance matrix; and $R_0$ is the initial observation noise variance matrix. For a series of moments k, the loop proceeds from the algorithm Step (2) to Step (6).

(2) Prediction

Step 2: The sigma point nonlinear transformation is performed according to the estimated mean ${\widehat{X}}_{k-1}$, the covariance matrix $P_{k-1}$, and the system dynamic model, that is

$${\widehat{\chi }}_{\left.k\right|k-1}^i=f\left({\chi }_{k-1}^i\right), i=0,\cdots ,2n$$

(21)

Step 3: For the state mean and covariance, the predicted values are given by Equations (22) and (23), respectively.

$${\widehat{X}}_{\left.k\right|k-1}=\sum _{i=0}^{2n}{w}_i^{\left(m\right)}{\widehat{\chi }}_{\left.k\right|k-1}^i$$

(22)

$$P_{\left.k\right|k-1}=\sum _{i=0}^{2n}{w}_i^{\left(c\right)}\left({\widehat{\chi }}_{\left.k\right|k-1}^i-{\widehat{X}}_{\left.k\right|k-1}\right){({\widehat{\chi }}_{\left.k\right|k-1}^i-{\widehat{X}}_{\left.k\right|k-1})}^T+Q_{k-1}$$

(23)

(3) Updating

Step 4: The sigma point nonlinear transformation is performed according to the measurement model, i.e.,

$${\widehat{\mathbb{Z}}}_{\left.k\right|k-1}^i=h\left({\widehat{\chi }}_{\left.k\right|k-1}^i\right), i=0,\cdots ,2n$$

(24)

Step 5: Measurement predicts the mean, innovation covariance, mutual covariance between state and measurement, and filter gain as follows:

$${\widehat{Z}}_{\left.k\right|k-1}=\sum _{i=0}^{2n}{w}_i^{\left(m\right)}{\widehat{\mathbb{Z}}}_{\left.k\right|k-1}^i$$

(25)

$$S_k=\sum _{i=0}^{2n}{w}_i^{\left(c\right)}\left({\widehat{\mathbb{Z}}}_{\left.k\right|k-1}^i-{\widehat{Z}}_{\left.k\right|k-1}\right){({\widehat{\mathbb{Z}}}_{\left.k\right|k-1}^i-{\widehat{Z}}_{\left.k\right|k-1})}^T+R_k$$

(26)

$$C_k=\sum _{i=0}^{2n}{w}_i^{\left(c\right)}\left({\widehat{\chi }}_{\left.k\right|k-1}^i-{\widehat{X}}_{\left.k\right|k-1}\right){\left({\widehat{\mathbb{Z}}}_{\left.k\right|k-1}^i-{\widehat{Z}}_{\left.k\right|k-1}\right)}^T$$

(27)

$$K_k=C_k{S}_k^{-1}$$

(28)

Step 6: The mean and covariance matrix of the posterior state estimation at time k are given by Equations (29) and (30), respectively.

$${\widehat{X}}_k={\widehat{X}}_{\left.k\right|k-1}+K_k\left(Z_k-{\widehat{Z}}_{\left.k\right|k-1}\right)$$

(29)

$$P_k=P_{\left.k\right|k-1}-K_k{S}_k{K}_k^T$$

(30)

4.2. Nonlinear Filtering of the Localization System Based on the Whitening of the Colored Noise

4.2.1. Colored Noise Modeling and Whitening-Based Filtering Methods

The observed noise based on TOA measurement mainly refers to the colored noise, which can achieve the optimal estimation performance under the framework of the KF algorithm. Using a time-series analysis model, a smooth colored noise sequence can be composed of related sequences and white noise at each time, which is called an autoregressive sliding average model ARMA (p, q). In this paper, q = 0, p = 1, and colored noise are modeled as in the AR (1) model.

The output of Gaussian distributed white noise after passing the first-order linear filter is the colored noise $V_k$, which can be expressed as

$$V_k=\mathrm{ð}{V}_{k-1}+{\nu }_k$$

(31)

where $\mathrm{ð}=1-T/\tau$, ${\nu }_k=\left(T/\tau \right){\theta }_k$, ${\nu }_k$ is the Gaussian white noise with a mean of 0 and a variance of ${R_k=R}_r{\left(T/\tau \right)}^2$, where $R_r$ is the variance of the Gaussian distribution ${\theta }_k$.

$V_k$ in the system observation model is a colored noise vector, which is whitened so that the noise condition meets the UKF application requirements to obtain a good performance.

Consider the discretized system states and the observed model as follows:

$$X_k=F_{k-1}{X}_{k-1}+B_{k-1}{W}_{k-1}$$

(32)

$$Z_k=H_k{X}_k+V_k$$

(33)

Then, the colored noise $V_k=Z_k-H_k{X}_k$; considering the colored noise $V_{k-1}$, the corresponding observation model is as follows:

$$\begin{gathered} Z_{k-1}=H_{k-1}{X}_{k-1}+V_{k-1} \\ \to V_{k-1}=Z_{k-1}-H_{k-1}{X}_{k-1} \end{gathered}$$

(34)

Then,

$${\mathrm{ð}V}_{k-1}={\mathrm{ð}Z}_{k-1}-{\mathrm{ð}H}_{k-1}{X}_{k-1}$$

(35)

According to Equations (32)–(35), $(V_k-ðV_{k-1})$-type Gaussian distribution white noise is reconstructed, and we can obtain the following:

$$\begin{array}{cc}\hfill v_k& {=V}_k-\mathrm{ð}{V}_{k-1}\hfill \\ & =Z_k-H_k{X}_k-{\mathrm{ð}Z}_{k-1}-{\mathrm{ð}H}_{k-1}{X}_{k-1}\hfill \\ & =Z_k-\mathrm{ð}{Z}_{k-1}-X_{k-1}\left(H_k{F}_{k-1}-\mathrm{ð}{H}_{k-1}\right)\hfill \\ & +H_k{B}_{k-1}{W}_{k-1}\hfill \end{array}$$

(36)

Equation (36) can be abbreviated as

$$Z_k^z=H_k^h{X}_k+V_k^v$$

(37)

where

$$\begin{gathered} Z_k^z=Z_k-\mathrm{ð}{Z}_{k-1} \\ {H}_k^h=H_k{F}_{k-1}-\mathrm{ð}{H}_{k-1} \\ {V}_k^v=H_k{B}_{k-1}{W}_{k-1}+v_k \end{gathered}$$

Here, $V_k^v$ is the new noise, and its mean value is

$$E\left[V_k^v\right]=E\left[H_k{B}_{k-1}{W}_{k-1}+v_k\right]=0$$

(38)

That is, $V_k^v$ is the white noise with a mean of 0, which realizes the colored noise whitening processing.

The variance of $V_k^v$ is

$$\begin{array}{cc}& E\left[V_k^v{V_k^v}^T\right]\hfill \\ \hfill =& E\left[\left(H_k{B}_{k-1}{W}_{k-1}+v_k\right){\left(H_j{B}_{j-1}{W}_{j-1}+v_j\right)}^T\right]\hfill \\ \hfill =& {\delta }_{kj}\left(H_k{Q}_{k-1}{H}_j^T+R_k\right)\hfill \end{array}$$

(39)

Therefore, after the whitening of the colored noise, the filtering parameters can be updated as shown in Equations (40)–(43).

$${\widehat{\chi }}_{\left.k\right|k-1}^i=F_{k-1}{\chi }_{k-1}^i, i=0,\cdots ,2n$$

(40)

$${\widehat{Z^z}}_{\left.k\right|k-1}^i=h\left({\widehat{\chi }}_{\left.k\right|k-1}^i\right)=H_k^h{\chi }_{k-1}^i, i=0,\cdots ,2n$$

(41)

$${\widehat{Z}}_{\left.k\right|k-1}=\sum _{i=0}^{2n}{w}_i^{\left(m\right)}{\widehat{Z^z}}_{\left.k\right|k-1}^i$$

(42)

$$S_k=\sum _{i=0}^{2n}{w}_i^{\left(c\right)}\left({\widehat{Z^z}}_{\left.k\right|k-1}^i-{\widehat{Z}}_{\left.k\right|k-1}\right){\left({\widehat{Z^z}}_{\left.k\right|k-1}^i-{\widehat{Z}}_{\left.k\right|k-1}\right)}^T+R_k^r$$

(43)

Here,

$$R_k^r=H_k{Q}_{k-1}{H}_j^T+R_k$$

(44)

4.2.2. Noise Decoupling Filtering Algorithm

Since UKF is based on the framework of the KF filter, after the whitening of the colored observation noise, the covariance of the process noise $W_k$ and the observation noise $V_k^v$ is $E\left[W_k{V_k^v}^T\right]=E\left[W_k{(H_k{B}_k{W}_k+v_k)}^T\right]\ne 0$, and there is a coupling between the observation noise and system noise, which needs to be decoupled to continue Kalman filtering. After the whitening of the colored observation noise, the discretized system state and the observation model are shown in Equations (45) and (46).

$$X_k=F_{k-1}{X}_{k-1}+B_{k-1}{W}_{k-1}$$

(45)

$$Z_k^z=H_k^h{X}_{k-1}+V_k^v$$

(46)

For the noise coupling of the state and the observation, the new observation model is set as

$${\breve{Z}}_k^z=Z_k^z+{\breve{V}}_k^v$$

(47)

where $Z_k^z=H_k^h{X}_{k-1}+V_k^v$.

Setting the noise coupling matrix of the state and the observation be $O_{k-1}$, then the system state is expressed as

$$X_k=F_{k-1}{X}_{k-1}+B_{k-1}{W}_{k-1}+O_{k-1}{\breve{V}}_k^v$$

(48)

and there is

$$Z_k^z=H_k^h{X}_{k-1}+V_k^v$$

(49)

$$X_k=F_{k-1}{X}_{k-1}+B_{k-1}{W}_{k-1}+O_{k-1}{\breve{V}}_k^v$$

(50)

For the system process and observation model, Equations (48)–(50) are combined, and we can obtain Equation (51).

$$X_k=F_{k-1}^f{X}_{k-1}+O_{k-1}{Z}_k^z+{{\rm E}}_k^e$$

(51)

where

$$F_{k-1}^f=\left(F_{k-1}-O_{k-1}{H}_k^h\right)$$

$${{\rm E}}_k^e=B_{k-1}{W}_{k-1}-O_{k-1}{V}_k^v$$

The covariance matrix of the process error ${{\rm E}}_k^e$ and the observation error $V_k^v$ is given as follows:

$$\begin{gathered} E\left[\left(B_{k-1}{W}_{k-1}-O_{k-1}{V}_k^v\right){V_k^v}^T\right] \\ =E\left[B_{k-1}{W}_{k-1}{V_k^v}^T\right]-E\left[G_{k-1}{V}_k^v{V_k^v}^T\right] \\ =E\left[B_{k-1}{W}_{k-1}{V_k^v}^T\right]-G_{k-1}E\left[V_k^v{V_k^v}^T\right] \end{gathered}$$

(52)

Let $E\left[(B_{k-1}{W}_{k-1}-O_{k-1}{V}_k^v){V_k^v}^T\right]=$0, then the process noise and the observation noise are independent of each other, that is

$$O_{k-1}E\left[V_k^v{V_k^v}^T\right]=E\left[B_{k-1}{W}_{k-1}{V_k^v}^T\right]$$

(53)

and

$$O_{k-1}=E\left[B_{k-1}{W}_{k-1}{V_k^v}^T\right]{\left\{E\left[V_k^v{V_k^v}^T\right]\right\}}^{-1}$$

(54)

The new state equation and observation equation of the decoupled system are

$$X_k=X_{k-1}\left(F_{k-1}-O_{k-1}{H}_k^h\right)+O_{k-1}{Z}_k^z+W_{k-1}^w$$

(55)

$$Z_k^z=H_k^h{X}_{k-1}+V_k^{v }$$

(56)

where $W_{k-1}^w=B_{k-1}{W}_{k-1}-O_{k-1}{V}_k^v$.

The one-step prediction state vector of the new model is

$${\widehat{X}}_{\left.k\right|k-1}^x=F_{\left.k\right|k-1}^x{\widehat{X}}_{k-1}+O_{k-1}{V}_k^v$$

(57)

The one-step prediction covariance matrix of the state is calculated as follows:

$$\begin{array}{cc}\hfill P_{\left.k\right|k-1}^p& =E\left[\left(X_k-{\widehat{X}}_{\left.k\right|k-1}^x\right){\left(X_k-{\widehat{X}}_{\left.k\right|k-1}^x\right)}^T\right]\hfill \\ & =E\left[\left(X_k-{\widehat{X}}_{\left.k\right|k-1}^x\right)\left(X_k^T-{\left({\widehat{X}}_{\left.k\right|k-1}^x\right)}^T\right)\right]\hfill \\ & =E\left[X_k{X}_k^T+{\widehat{X}}_{\left.k\right|k-1}^x{\left({\widehat{X}}_{\left.k\right|k-1}^x\right)}^T-X_k{\left({\widehat{X}}_{\left.k\right|k-1}^x\right)}^T-{\widehat{X}}_{\left.k\right|k-1}^x{X}_k^T\right]\hfill \\ & =\left(F_{\left.k\right|k-1}-O_{k-1}{H}_k^h\right)P_{k-1}{\left(F_{\left.k\right|k-1}-O_{k-1}{H}_k^h\right)}^T+Q_{k-1}+O_{k-1}{R}_k^r{O}_{k-1}^T-N_k{O}_{k-1}^T-O_{k-1}{N}_k^T\hfill \end{array}$$

(58)

where

$$N_k=E\left[B_{k-1}{W}_{k-1}{V_k^v}^T\right]$$

$$R_k^r=E\left[V_k^v{V_k^v}^T\right]$$

$P_{\left.k\right|k-1}^p$ can be abbreviated as

$$P_{\left.k\right|k-1}^p=\mathcal{F}{P}_{k-1}{\mathcal{F}}^T+Q_{k-1}^q$$

(59)

where

$$\mathcal{F}=F_{\left.k\right|k-1}-O_{k-1}{H}_k^h$$

$${Q_{k-1}^q=Q_{k-1}+O}_{k-1}{R}_k^r{O}_{k-1}^T-N_k{O}_{k-1}^T-O_{k-1}{N}_k^T$$

Therefore, the filtering process can be updated as follows:

$$K_k=P_{\left.k\right|k-1}^p{\left[H_k^h\right]}^T{(H_k^h{P}_{\left.k\right|k-1}^p{\left[H_k^h\right]}^T+R_k^r)}^{-1}$$

(60)

$${\widehat{X}}_{\left.k\right|k-1}^x=F_{\left.k\right|k-1}^x{\widehat{X}}_{k-1}+O_{k-1}{Z}_k^z$$

(61)

$$P_{\left.k\right|k-1}^p=\mathcal{F}{P}_{k-1}{\mathcal{F}}^T+Q_{k-1}^q$$

(62)

The mean and covariance matrix of the posterior state estimation at time k of the system are

$${\widehat{X}}_k={\widehat{X}}_{\left.k\right|k-1}^x+K_k\left(Z_k^z-{\widehat{Z}}_{\left.k\right|k-1}\right)$$

(63)

$$P_k=P_{\left.k\right|k-1}^p-K_k{S}_k{K}_k^T$$

(64)

4.2.3. A Nonlinear Filtering Process Based on a New Measurement Model of the System

In summary, the UKF filtering algorithm process after denoising processing and removing noise correlation are as follows:

(1) The initialization state ${\widehat{X}}_0$ and its covariance $P_0$, $Q_0$, and $R_0$.

(2) According to the estimated mean ${\widehat{X}}_{k-1}$ and covariance matrix $P_{k-1}$ at time k−1, the sigma points and weights are selected. After the nonlinear transformation of the sigma points, the state prediction means ${\widehat{X}}_{\left.k\right|k-1}$ and the prediction covariance $P_{\left.k\right|k-1}$ are obtained.

(3) According to the new measurement model of the system, the nonlinear transformation of the sigma point is carried out to obtain the measurement prediction mean ${\widehat{Z}}_{\left.k\right|k-1}$, the innovation covariance $S_k$, the new step prediction state vector ${\widehat{X}}_{\left.k\right|k-1}^x$, the prediction covariance matrix $P_{\left.k\right|k-1}^p$, and the filtering gain $K_k$.

(4) Finally, the mean of state estimation ${\widehat{X}}_k$ and the covariance matrix $P_k$ at time k are updated.

Considering that the initial stage of filtering is sensitive to noise, it is easy to cause large errors in the filtering results and this can even lead to filter divergence. Therefore, the above initial positioning value is used as the initial value of UKF for filtering positioning.

4.3. Self-Optimizing Parameter Filtering Algorithm Based on Prediction Residuals

The predicted residual statistic is introduced to describe the random influence of the system state. By setting the threshold parameters, the calculated value and estimated value of the covariance matrix are used to judge the prediction error, and the self-optimization parameters are set to optimize the filter gain matrix in real time to enhance the robustness of the system. Set the prediction residual statistics to be

$${∆Z=Z}_k^z-H_k^h{X}_{k-1}$$

(65)

Then, the covariance matrix is as follows:

$$P_k^{\left(p\right)}=H_k^h{P}_{\left.k\right|k-1}^p{\left[H_k^h\right]}^T+R_k^r$$

(66)

The self-optimization parameter is set to ${\mu }_k$ and $Tr\left(·\right)$ represents the trace operation. ${\mu }_k$ can be determined according to the following method: when ${\left[∆Z\right]}^T∆Z<Tr\left[P_k^{\left(p\right)}\right]$, it means that the state prediction error is small, take ${\mu }_k=1$; otherwise, the state prediction error is large, take ${\mu }_k=Tr\left[P_k^{\left(p\right)}\right]/\left[{\left[∆Z\right]}^T∆Z\right]$. The observation accuracy can ensure that $Tr\left[P_k^{\left(p\right)}\right]$ is not too small, so the weight of the observation value with higher accuracy will not be reduced and the filtering error will be reduced.

We use the filter gain matrix to update the parameters and the self-optimization method to filter the noise modeling error. The filter gain is shown as follows:

$$K_k={\displaystyle \frac{1}{{\mu }_k}}{P}_{\left.k\right|k-1}^p{\left[H_k^h\right]}^T{\left[H_k^h{\displaystyle \frac{1}{{\mu }_k}}{P}_{\left.k\right|k-1}^p{\left[H_k^h\right]}^T+{\left(R_k^r\right)}^{-1}\right]}^{-1}$$

(67)

where ${\mu }_k$ is the self-optimization parameter and $0<{\mu }_k\le 1$.

The diagram of the Kalman filter process based on the self-optimization parameters is shown in Figure 1.

5. Positioning Performance Evaluation

To evaluate and compare the performance of the positioning algorithm, the relevant evaluation indicators and comparison algorithm are briefly described as follows.

(1) RMSE

The commonly used evaluation indicator representing the estimated performance is the root mean square error (RMSE). If the estimated multipath delay and location are ${\widehat{t}}_y$ and ${\widehat{z}}_i$, respectively, the RMSE representing the multipath delay and location estimation performance of the target node are shown in Equations (68) and (69), respectively.

$${\mathrm{R}\mathrm{M}\mathrm{S}\mathrm{E}}_{{\widehat{t}}_y}=\sqrt{{\displaystyle \frac{1}{M}}\sum _{i=1}^{M}E\left({‖{\widehat{t}}_y-t_y‖}^2\right)}$$

(68)

$${\mathrm{R}\mathrm{M}\mathrm{S}\mathrm{E}}_{{\widehat{z}}_i}=\sqrt{{\displaystyle \frac{1}{M}}\sum _{i=1}^{M}E\left({‖{\widehat{z}}_i-z_i‖}^2\right)}$$

(69)

(2) CRLB

The CRLB represents the bound of the estimated error covariance matrix of any unbiased estimate of an unknown parameter vector, which is equal to the inverse of the Fisher information matrix (FIM), and the FIM is created by the probability density function (PDF). The CRLB is briefly described below to compare algorithms.

The actual position coordinate of the target to be estimated is set to $z_i={\left[z_{xi},z_{yi},z_{zi}\right]}^T$, and the estimated position is marked as $p_i={\left[p_{xi},p_{yi},p_{zi}\right]}^T$. For non-random vector parameters, the CRLB is expressed as follows:

$$E\left[\left(‖p_i-z_i‖\right){\left(‖p_i-z_i‖\right)}^T\right]\ge {FIM}^{-1}$$

(70)

where the PDF of the $t_{ik}$ is

$$p\left(t_{ik}|t_y,u_i\right)={\displaystyle \frac{1}{\sqrt{{\left(2\pi {\sigma }_n^2\right)}^M}}}\exp \left(-{\sum }_{k=1}^M{\displaystyle \frac{1}{2{\sigma }_n^2}}{\left(c{t}_{ik}-c{t}_0-c{t}_y-‖p_i-z_k‖\right)}^2\right)$$

(71)

The corresponding Fisher information matrix is expressed as follows:

$$FIM=-E\left\{\left[\begin{array}{ccc}{\displaystyle \frac{{\partial }^2\ln p}{\partial {\tau }_p^2}}& {\displaystyle \frac{{\partial }^2\ln p}{\partial {\tau }_p\partial p_{xi}}}& \begin{array}{cc}{\displaystyle \frac{{\partial }^2\ln p}{\partial {\tau }_p\partial p_{yi}}}& {\displaystyle \frac{{\partial }^2\ln p}{\partial {\tau }_p\partial p_{zi}}}\end{array}\\ {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{xi}\partial {\tau }_p}}& {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{xi}^2}}& \begin{array}{cc}{\displaystyle \frac{{\partial }^2\ln p}{\partial p_{xi}\partial p_{yi}}}& {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{xi}\partial p_{zi}}}\end{array}\\ \begin{array}{c}{\displaystyle \frac{{\partial }^2\ln p}{\partial p_{yi}\partial {\tau }_p}}\\ {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{zi}\partial {\tau }_p}}\end{array}& \begin{array}{c}{\displaystyle \frac{{\partial }^2\ln p}{\partial p_{yi}\partial p_{xi}}}\\ {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{zi}\partial p_{xi}}}\end{array}& \begin{array}{cc}\begin{array}{c}{\displaystyle \frac{{\partial }^2\ln p}{\partial p_{yi}^2}}\\ {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{zi}\partial p_{yi}}}\end{array}& \begin{array}{c}{\displaystyle \frac{{\partial }^2\ln p}{\partial p_{yi}\partial p_{zi}}}\\ {\displaystyle \frac{{\partial }^2\ln p}{\partial p_{zi}^2}}\end{array}\end{array}\end{array}\right]\right\}$$

(72)

Therefore, the CRLB of time delay and position error variance is shown in Equations (73) and (74), respectively.

$${CRLB}_{{\tau }_p}={\left[{FIM}^{-1}\right]}_{\mathrm{1,1}}$$

(73)

$${CRLB}_{p_i}={\left[{FIM}^{-1}\right]}_{\mathrm{2,2}}+{\left[{FIM}^{-1}\right]}_{\mathrm{3,3}}+{\left[{FIM}^{-1}\right]}_{\mathrm{4,4}}$$

(74)

where ${FIM}^{-1}$ is the inverse matrix of $FIM$, ${\left[{FIM}^{-1}\right]}_{i,j}$ is the element of the matrix ${FIM}^{-1}$ in row i and column j, and for simplicity, we do not list the closed expression for the full CRLB here. The CRLB of multipath delay and localization performance discussed above will be used in the simulation later in this paper to compare the proposed algorithm with other existing algorithms.

6. Numerical Simulation and Analysis

6.1. Initial Positioning Simulation

This section evaluates the positioning performance of the proposed positioning algorithm through simulation. Assuming that the measurement error is an independent and identically distributed zero-mean Gaussian vector, the proposed algorithm is run on the target node on the wireless network. At the same time, it is assumed that the time of running the algorithm independently is less than the network data update time; that is, the data from the neighbor nodes are received, and the time is kept unchanged to obtain more accurate and effective positioning results.

The purpose of the simulation setup is to test the relationship between the convergence estimation performance of the proposed algorithm and the measurement error value. The simulation scenario is set as follows: the wireless network consists of nine nodes, of which one target node needs to be located, and the remaining eight anchor nodes are in the area that can normally participate in the positioning.

Table 1. Units for magnetic properties.

Coordinates	1	2	3	4	5	6	7	8	9
x	−300	0	300	−300	0	300	−300	0	300
y	300	300	300	0	0	0	−300	−300	−300
z	−300	−300	300	300	0	0	300	0	−300

shows the true location of nine nodes in the wireless network, where node 6 is the target node to be located, and the remaining nodes are anchor nodes, which can achieve TOA distance measurement with the target node.

The following WLS algorithm and RLS algorithm are described for performance comparison with the algorithm in this paper.

(1) WLS algorithm

It is assumed that there are N reference nodes in the network marked as $R_i(q_{xk},q_{yk},q_{zk})$, the measurement distance from the reference node to the unknown node $U(z_{xi},z_{yi},z_{zi})$ is $d_i$, and the vector to be estimated is from the reference node is denoted as $\theta ={\left[p_{xi},p_{yi},p_{zi}\right]}^T$. The measurement equation is

$${\widehat{d}}_i=d_i+v_i, i=1,2,\cdots ,N$$

where $d_i=‖p_i-z_k‖$, and $v_i$ is the measurement error. Assuming that h is the observation and b is the observation matrix, the positioning model can be abbreviated as

$$\mathit{h}=\mathit{b}\mathit{\theta }+\mathit{v}$$

(75)

Therefore, the solution of WLS is

$$\widehat{\theta }={\left(b^{T}Wb\right)}^{-1}{b}^{T}Wh$$

(76)

where $W={cov}^{-1}\left(\mathit{v}\right)$ is a weighted matrix and is required to be symmetric and positive definite. In practical applications, if the statistical characteristics of errors are unknown, W can be set as a unit matrix, and its value can be taken as $W=diag\left\{\begin{array}{ccc}1/d_1,1/d_2& \cdots ,& 1/d_N\end{array}\right\}$.

(2) RLS algorithm

Let ${\widehat{\theta }}_k$ be the estimated value obtained by the least squares algorithm using the first k observation data. The measurement process is modeled as follows:

$$h_k=b_k{\theta }_k+v_k$$

(77)

Using the RLS estimation algorithm with a forgetting factor, the measurement error cost function is as follows:

$$J\left({\theta }_k\right)={\displaystyle \frac{1}{2}}\sum _{k=1}^n{{\lambda }^{n-k}\left(h_k-b_k^T{\widehat{\theta }}_k\right)}^2$$

(78)

The value of ${\theta }_k$ that minimizes the cost function is the estimated value of ${\widehat{\theta }}_k$, so the recursive equation of the RLS algorithm with the forgetting factor is as follows:

$${\widehat{\theta }}_k={\widehat{\theta }}_{k-1}+a_k\left[h_k-b_k^T{\widehat{\theta }}_{k-1}\right]$$

(79)

where $a_k$ is the recursive gain matrix, $a_k={\phi }_{k-1}{b}_k{\left[\lambda I+b_k^T{\phi }_{k-1}{b}_k\right]}^{-1}$, ${\phi }_k$ is the covariance matrix ${\phi }_k=\left[I-a_k{b}_k^T\right]{\phi }_{k-1}/\lambda$, and $\lambda$ is the forgetting factor, $\lambda \in \left(0,1\right)$.

When the network is deployed according to Table 1, the location estimation performance of the three algorithms running on the target node 6 is as shown in Figure 2. The curves in the figure are the result obtained by taking the average value of the simulation after 500 times of independent operation.

It can be seen from Figure 2 that all algorithms can locate normally within a certain range of RMSE measurement errors based on TOA. The localization performance of the proposed algorithm is very close to that of CRLB, and better than that of the WLS algorithm and the RLS algorithm.

6.2. Filtering Simulation

6.2.1. Experimental Environment

The target is set to perform uniform linear motion in the three-dimensional plane. The starting position is set to (30, 0, 2), and the unit is m. That is, the real trajectory of the target motion is set to perform uniform linear motion. The motion speed $(v_x,v_y,v_z)$ of target is set to (15, 0, 10) and the unit is $\mathrm{m}/\mathrm{s}$. The disturbance acceleration variance $(a_x,a_y,a_z)$ of the target is $(\sqrt{2}/3,\sqrt{2}/3,\sqrt{2}/3)$, the unit is ${\left(\mathrm{m}/{\mathrm{s}}^2\right)}^2$, and the sampling period is 0.1 s. The target trajectory is shown in Figure 3.

The TOA ranging error is set as the output of the Gaussian distribution white noise through the first-order linear filter, which can be regarded as the colored noise driven by the white noise. The mean value of the white noise is set to 0, and the standard deviation is 2 m/s. The sampling period is set to 0.1 s, and the total simulation time is 500 s. The standard deviation of TOA colored noise is 1 m, as shown in Figure 4.

6.2.2. Target Observation and Filtering Results

The target is set to perform uniform linear motion in the three-dimensional plane, and the starting position is set to (30, 0, 2), (unit: m). The UKF algorithm based on colored noise processing is used to estimate the trajectory of the target motion. The target and the estimated trajectory are shown in Figure 5, which clearly shows that the estimation of the target motion trajectory by the filtering algorithm in this paper is close to the actual trajectory.

6.2.3. Comparison of Filtering Results Based on White Noise and Colored Noise

The sampling period is set to 0.1 s, and the filtering estimation results of white noise and colored noise are compared. The estimation results of the two algorithms are shown in Figure 6 by the position errors in the x-axis direction, the y-axis direction, and the z-axis direction, respectively. The phase performance of filtering errors is more complex, but the overall trend shows that the filtering estimation accuracy after colored noise processing is higher. The mean and standard deviation of the errors of the two algorithms in the three directions of the rectangular coordinate axis are shown in

Table 2. The error mean and standard deviation of the two algorithms in three directions.

Performance of Error (m)	Gaussian White Noise			Processed Colored Noise
	X-Axis	Y-Axis	Z-Axis	X-Axis	Y-Axis	Z-Axis
Mean	−0.3857	0.2352	−0.6384	−0.2367	−0.0153	−0.4231
Standard deviation	0.6652	0.6256	0.6132	0.4428	0.4432	0.4284

, which shows that the filtering performance of the processed colored noise is high, and the standard deviation has been maintained at a low level.

6.2.4. Prediction Error and Filtering Error

Set the target to perform uniform linear motion. The starting position of the target is now set to (30, 0, 2), (unit: m). The simulation results show that the standard deviations of the prediction error and the filtered position error are 0.817 and 0.362, respectively, under the condition of colored noise. The position error curves in both cases are shown in Figure 7.

6.2.5. RMSE Error and Analysis Based on Colored Noise Processing

The RMSE is used to calculate the error between the target estimation and the actual position. Under the premise that the observation noise is a colored noise vector, the environmental parameters are set as the same as the previous uniform linear motion, and the basic UKF, the non-self-optimizing UKF, and the self-optimizing UKF algorithm are run, respectively. The results are shown in Figure 8.

In the total simulation time of 100 s, the mean values of RMSE of the target are 0.32 m, 0.35 m, and 0.67 m, respectively. The results of Figure 9 show that in the positioning based on TOA ranging, the observation with colored noise will seriously affect the positioning accuracy, and the basic KF algorithm has large filtering errors and a poor stability. The filtering algorithm in this paper can reduce the positioning error caused by colored noise and has a good stability. The filtering algorithm in this paper improves the filtering accuracy by 52.23% compared with the basic KF algorithm.

6.2.6. Filtering Results and Analysis Under Different Initial Positioning Conditions

In the actual ranging-based positioning system, there is always an error between the given initial state value and the optimal initial value required for filtering, which will affect the overall filtering performance. Choosing an accurate initial value of the filter can not only eliminate the adverse effects of the initial value error on the filtering process but also ensure the fast convergence of the filtering results.

The experimental environment is set as follows: the target moves in a uniform linear motion in the three-dimensional plane, and the starting position of the target is accurately estimated by the TOA measurement of the M anchor nodes received by Algorithm 1 as $({\widehat{x}}_0,{\widehat{y}}_0,{\widehat{z}}_0)$, and other settings remain unchanged.

The definition of RMSE is used to calculate the error between the target estimation and the actual position on the x-axis and the y-axis, respectively. Under the premise that the observation noise is a colored noise vector and the parameter setting environment is the same, the self-optimized UKF algorithm based on WLS initial positioning and the self-optimized UKF algorithm based on the parameter analysis initial positioning in this paper are run, respectively, as shown in Figure 9.

It should be noted that the error of the initial positioning in the initial stage is smaller than that of other algorithms without initial positioning, and the error in the steady state is slightly smaller than that of other algorithms without initial positioning.

From the simulation results of Figure 9, in the total simulation time of 200 s, the mean values of RMSE obtained by the two algorithms are 0.051 m and 0.072 m, respectively, and the filtering accuracy is improved by 28.57%. The standard deviation of filtering is 0.418 m and 0.573 m, respectively, which shows that the algorithm in this paper not only has a high positioning accuracy, but also shows that the filtering performance of the initial positioning in this paper is high, and the standard deviation has been maintained at a low level.

The above statistical data can demonstrate that under the same conditions, the algorithm proposed in this paper selects more accurate initial filtering values that are closer to the optimal initial values required for filtering, which can simulate the negative impact of initial value errors on filtering performance. However, based on WLS positioning, there is a significant error between the initial values and the optimal filtering initial values. Therefore, the algorithm proposed in this paper can achieve a higher filtering accuracy, as evidenced by the results shown in Figure 9.

6.2.7. Computation Time

To analyze and compare the complexity of the algorithms proposed in this article, we calculated the average running time of the algorithms. The data simulation processing platform consists of a PC with the following parameters: Intel (R) Core (TM) i7-8700 CPU @ 3.20 GHz, 16.0 GB RAM, 64-bit operating system based on x64 processor, and MATLAB 2019A as the software testing environment for executing the simulation process. The simulation scenario is described in Section 6.2.6. The self-optimized UKF algorithm based on WLS initial positioning and the self-optimized UKF algorithm based on parameter analysis initial positioning in this paper are independently run 50 times each. The average positioning running time is shown in

Table 3. Average running time.

Algorithm	WLS Initial Positioning Filter	Initial Positioning Filter Proposed
Time (ms)	10.78	11.85

. The positioning running time of the two algorithms in the self-optimizing UKF algorithm stage is the same. In the initial positioning stage, the initial positioning algorithm proposed in this paper improves the positioning accuracy, but at the same time, it also comes at a slightly higher cost of increased running time. Therefore, under the condition of limited and enough computing power, it is worthwhile to use the algorithm proposed in this paper for higher accuracy positioning.

7. Conclusions

In this paper, a gain-adaptive two-stage fusion filtering localization algorithm based on prediction residuals is proposed. Firstly, based on the Gaussian noise observation model, the pseudo-linearization equations from TOA measurements and multipath time delays are derived using the nonlinear system parameterization method. The WLS criterion and effective analytical calculation are used to obtain an approximate analytical solution that approximates the CRLB of the target position estimation, which is used as a Kalman filter after the initial positioning input. In the final positioning stage, the colored noise measurement information is processed by reconstructing the Gaussian distribution white noise statistics, and the new measurement model is derived. Subsequently, the Kalman filtering algorithm is used to implement multi-source information fusion, and the new observation is received for filtering error correction. This study explores the construction of measurement models based on time of arrival (TOA) measurement and multipath delay to achieve localization. In order to compensate for modeling errors caused by noise, observation values are introduced to predict residual statistics, covariance matrix tracking operations, and gain matrices in self-optimizing filters to obtain the accurate position of the target, thereby enhancing the filtering performance of the localization system for time measurement errors. The positioning effect of the proposed algorithm is verified by simulation experiments. The experimental results show that compared with other initial positioning algorithms, the average positioning accuracy of the fusion algorithm in this paper is improved by 28.57%, exhibiting better accuracy and filtering stability. In this work, additive colored noise in the localization model is analyzed and whitened, enabling the system to meet the Kalman filter requirements and achieve high-precision estimation. However, non-additive (e.g., multiplicative) effects arise in practical positioning scenarios. Future work will focus on analyzing and whitening non-additive noise to further improve the Kalman filtering performance. Furthermore, extending the proposed framework to handle complex motion trajectories (e.g., circular or nonlinear patterns) represents a critical direction for real-world applications, as such trajectories inherently exist in cluttered environments. This extension will require the development of adaptive motion pattern models and corresponding uncertainty quantification methods to address dynamic constraints under non-ideal path conditions.