A dynamic target combined positioning method based on adaptive Kalman filtering

Abstract

The application discloses a dynamic target combined positioning method based on adaptive Kalman filtering and belongs to the technical field of calculation, reckoning or counting. The combined positioning method is used for two application scenarios that a dynamic target is in a linear state and a nonlinear state. On the basis of combined positioning of a global positioning system and ultra-wideband ranging information, an adaptive factor and a dynamic window adjustment are introduced based on a covariance matching method, an adaptive Kalman filtering combined positioning method fusing UWB and GPS is provided, under the linear motion state of a target, a sliding window adaptive Kalman filtering method based on covariance matching is used for GPS single point positioning, and under the nonlinear motion state of the target, an adaptive unscented Kalman filtering method with a filter divergence criterion introduced is used for UWB / GPS combined positioning. The application improves the positioning precision and stability of the Kalman filtering method fusing UWB and GPS.

Description

Technical Field

[0001] This invention relates to positioning technology in communication, specifically to a combined positioning scheme that adaptively updates based on a dynamic target motion model and real-time measurement data. It discloses a dynamic combined positioning method based on adaptive Kalman filtering, belonging to the technical field of calculation, estimation, or counting. Background Technology

[0002] With the rise of Industry 4.0, the Internet of Things (IoT) is developing rapidly, with a particularly significant increase in demand for Location Based Services (LBS). LBS utilizes different positioning technologies to obtain the current location of positioning devices and provides information resources and basic services to these devices via the mobile internet, thereby achieving intelligent positioning and tracking. In the context of Industry 4.0, the requirements for the positioning and tracking of dynamic targets are becoming increasingly stringent, making the positioning and tracking technology of dynamic targets in LBS a subject of significant research value.

[0003] When tracking and locating dynamic targets, it is necessary to obtain the target's pose information measurements from positioning sensors. The Global Navigation Satellite System (GNSS) is an airborne radio navigation and positioning system that can provide omnidirectional, all-weather, and all-time coordinate, velocity, and time information. Among GNSS positioning systems, the most widely used is the single-point positioning mode of the Global Positioning System (GPS), which is relatively inexpensive and has a wide coverage area, but its positioning accuracy is not high, especially when obstructed by buildings, where accuracy drops significantly. Ultra-wideband (UWB) is a wireless carrier communication technology that uses nanosecond-level non-sinusoidal narrow pulses to transmit information, providing centimeter-level positioning accuracy through a relatively low-power system. However, its hardware cost is high and the system is complex, limiting its widespread use. Considering the characteristics of GPS and UWB positioning technologies, GPS is suitable for situations with simple motion conditions and low positioning accuracy requirements, and it is relatively inexpensive and has wide coverage. For situations with complex motion conditions and high positioning accuracy requirements, GPS and UWB can be used for fusion positioning to improve positioning accuracy while maintaining cost-effectiveness.

[0004] Currently, the main challenges in dynamic target tracking and localization lie in two aspects: first, the uncertainty of the dynamic target's motion model, meaning the localization system cannot accurately estimate the motion model of the target; and second, the uncertainty of the dynamic target's observation data, meaning the localization system cannot accurately observe the target's pose information. These two uncertainties lead to model noise and measurement noise in the localization system, respectively. Therefore, dynamically compensating for model noise errors and reducing the impact of measurement noise errors are key to improving the accuracy of dynamic target tracking and localization. Dynamic target tracking and localization methods must not only meet high localization accuracy requirements but also ensure real-time filtering processing. The Kalman Filter (KF) algorithm, with its characteristics of simplified model and small data storage requirements, has been widely used in dynamic target tracking and localization.

[0005] The equations of motion or measurement for dynamic targets can be either linear or nonlinear. Therefore, dynamic target tracking and localization can be categorized into linear filtering problems and nonlinear filtering problems. The KF algorithm, combined with the least squares method, proposes a recursive optimal solution for the linear filtering problem based on the target's state space. The optimal solution for the nonlinear filtering problem requires obtaining the conditional posterior probability through an endless array of parameters. In practical applications, it is solved by performing a suboptimal approximation on the nonlinear problem. Based on first-order linearization truncation of the Taylor expansion of the nonlinear function, ignoring higher-order terms, and performing a linear approximation of the nonlinear function, the Extended Kalman Filter (EKF) algorithm was developed. Based on approximating the probability density distribution of the nonlinear function using a sampling strategy, the Unscented Kalman Filter (UKF) algorithm was developed.

[0006] Because EKF performs a first-order Taylor expansion of the nonlinear function near the predicted value in the state space, when the nonlinearity of the target's motion state is high, the higher-order terms of its Taylor expansion cannot be ignored, leading to a large systematic error. Furthermore, EKF requires differentiation of the Jacobian matrix of the nonlinear function, increasing computational complexity. In contrast, UKF, based on the UT transform, approximates the probability density distribution of the nonlinear function through a deterministic sampling strategy, achieving at least second-order accuracy in calculating nonlinear statistics. This improves filtering accuracy while maintaining the same computational complexity as the EKF algorithm.

[0007] The filtering accuracy of linear KF and nonlinear UKF algorithms depends on the precision of the state model and measurement data, as well as the accurate estimation of the statistical characteristics of model noise and measurement noise, namely the covariance matrix Q of the model noise and the covariance matrix R of the measurement noise. Typical KF and UKF algorithms generally obtain the statistical characteristics of the noise beforehand through extensive experimental analysis and fix the Q and R matrices to certain values. However, when applying typical KF or UKF algorithms to dynamic target tracking and localization, changes in the target's motion state affect the statistical characteristics of model noise and measurement noise, leading to increased uncertainty in the Q and R matrices, thus reducing filtering accuracy and even causing filter divergence.

[0008] Adaptive filtering is a filtering method that can adjust system parameters in real time, reduce the impact of errors, and suppress filter divergence. There are many types of adaptive filtering methods. The Sage-Husa adaptive filtering algorithm obtains estimates of state variables based on the observation sequence and calculates the mean and covariance matrices of model noise and measurement noise according to the maximum a posteriori estimation principle. An improved adaptive UKF algorithm can be obtained by using an improved Sage-Husa suboptimal unbiased maximum a posteriori estimator and introducing an adaptive decay factor to correct the prediction error covariance. Based on the fading filtering algorithm, researchers introduce filter convergence criteria and select appropriate forgetting factors to make the actual error smaller than the theoretical error. A few studies have also introduced maximum likelihood criteria based on innovation-based adaptive estimation and multi-model-based adaptive estimation methods to adjust the Kalman gain coefficient of the filter in real time. Other researchers have improved the UKF algorithm from the perspective of adaptively adjusting process noise and measurement noise and correcting the Kalman gain, proposing an adaptive Kalman filtering algorithm based on the covariance matching method, introducing a fixed-width sliding window to update the Q and R matrices, thus improving the robustness of the algorithm. However, existing Sage-Husa adaptive filtering algorithms can only estimate R when Q is known or estimate Q when R is known. Furthermore, since most adaptive algorithms do not introduce filtering divergence criteria, there is a certain possibility of filtering divergence. Therefore, there is still room for improvement in the positioning accuracy of existing Sage-Husa adaptive filtering algorithms.

[0009] Based on the above research, it is clear that the key to improving the localization accuracy of the Kalman filter algorithm for dynamic targets lies in reducing the estimation error caused by the uncertainty of the Q and R matrices. This invention aims to propose a dynamic combined localization method based on adaptive Kalman filtering to overcome the aforementioned shortcomings. Summary of the Invention

[0010] The purpose of this invention is to address the shortcomings of the aforementioned background technology. Based on the research of adaptive filtering, it proposes a dynamic combination localization method based on adaptive Kalman filtering. This method aims to improve the response accuracy of Kalman filtering to dynamic targets and effectively suppress filter divergence, thus solving the technical problem of how to improve the localization accuracy of dynamic target tracking and localization methods while ensuring the real-time performance of filtering processing.

[0011] To achieve the aforementioned objective, this invention employs the following technical solution: a dynamic target combination localization method based on adaptive Kalman filtering, characterized by comprising the following steps:

[0012] Step 1: Determine the motion state of the heading angle based on the heading angle of the dynamic target. If the dynamic target is in a linear motion state, proceed to Step 2; if the dynamic target is in a nonlinear motion state, proceed to Step 3.

[0013] Step 2: Use the sliding window adaptive Kalman filter algorithm based on covariance matching to perform single-point localization of the dynamic target;

[0014] Step 3: Construct measurement vectors by fusing UWB measurement data and GPS measurement data, establish a state-space model under nonlinear motion conditions, and use an adaptive unscented Kalman filter algorithm that introduces two filtering divergence criteria based on covariance matching to perform combined localization of dynamic targets.

[0015] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, the specific method for determining the motion state of the heading angle in step 1 is as follows: when the change of the heading angle of the dynamic target is less than or equal to the threshold, the dynamic target is determined to be in a linear motion state; when the change of the heading angle of the dynamic target is greater than the threshold, the dynamic target is determined to be in a nonlinear motion state.

[0016] As a further optimization of the dynamic target ensemble localization method based on adaptive Kalman filtering, step 2 uses a sliding window adaptive Kalman filtering algorithm based on covariance matching to perform single-point localization of the dynamic target.

[0017] Step 2-1: Initialize the dynamic target state vector and error covariance matrix;

[0018] Step 2-2: Predict the prior estimate of the dynamic target state vector at time k. Prior estimate P of the error covariance matrix at time k (k|k-1) ;

[0019] Steps 2-3: Obtain the innovation error sequence ε at time k based on the covariance matching method. k The covariance matrix C k The adaptive factor α at time kk After introducing the covariance matrix of the innovation error sequence at time k, the Kalman gain coefficient K at time k is estimated. k Calculate the covariance matrix C of the innovation error sequence at time k that satisfies the optimal filter theory. k Adaptive factor α at time k k The sliding window at time k+1 is estimated based on the distance between the mean values ​​of the innovation error sequence at time k and the residual sequences at the previous time k-1. The adaptive factor α at time k is then updated based on the estimated sliding window at time k. k ;

[0020] Step 2-4: Based on the adaptive factor α updated in step 2-3 at time k. k Step 2-3 estimates the Kalman gain coefficient K at time k. k Perform state prediction and measurement updates for dynamic targets.

[0021] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, the specific method for combining and locating dynamic targets in step 3 by introducing two adaptive unscented Kalman filtering algorithms based on covariance matching and filtering divergence criteria is as follows:

[0022] Step 3-1: Initialize the dynamic target state vector and error covariance matrix;

[0023] Step 3-2: Perform the first UT transformation on the predicted state vector of the dynamic target at time k, construct the Sigma point set, and predict the state vector of the dynamic target at time k+1 based on the first-order and second-order statistical characteristic weight coefficients of the Sigma point set. The error covariance matrix P at time k+1 (k+1|k );

[0024] Step 3-3: The state vector of the dynamic target at time k+1 predicted in step 3-2... Perform a second UT transformation to obtain the updated Sigma point set, and predict the measurement vector of the dynamic target at time k+1. The autocovariance matrix of the dynamic target measurement vector at time k+1 The cross-covariance matrix of the dynamic target measurement vector at time k+1 Kalman gain K at time k+1 k+1 According to the Kalman gain K at time k+1 k+1 Perform filter updates;

[0025] Steps 3-4 introduce two filtering divergence criteria to update the covariance matrix R of the measurement noise at time k+1. k+1 :

[0026] When the innovation error sequence at time k satisfies the first filter divergence criterion, update the covariance matrix R of the measurement noise at time k+1. k+1 R is the covariance matrix of the measurement noise at time k. k ,

[0027] When the innovation error sequence at time k does not satisfy the first filtering divergence criterion but satisfies the second filtering divergence criterion, the covariance matrix R of the measurement noise at time k+1 is... k+1 Assign an infinity value and set the Kalman gain K at time k+1 to... k+1 Assigning a value of 0 means performing state prediction only on the dynamic target state vector.

[0028] When the innovation error sequence at time k satisfies the second filter divergence criterion, the covariance matrix R of the measurement noise at time k+1 is estimated based on the sliding window at time k and the innovation error sequences at the previous k times. k+1 Based on the covariance matrix R of the noise measured at time k+1 k+1 Perform state prediction and measurement correction on the dynamic target state vector.

[0029] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, the covariance matrix C of the innovation error sequence at time k in steps 2-3 satisfies the optimal filter theory. k C k =HP k|k-1 H T +α k R k The adaptive factor α at time k satisfies the optimal filter theory. k for Where H is the measurement matrix and tr(*) is the trace operation.

[0030] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, step 2-3 uses the distance between the mean values ​​of the innovation error sequence at time k and the residual sequences at the previous k-1 times to predict the sliding window at time k+1 as follows: Where, N k N k+1 Let ε be the window size at time k and time k+1. k (j) is the information sequence of the j-th measurement dimension at time k. Let L be the average magnitude of the information sequence of the j-th measurement dimension at the first k-1 time points, where L is the measurement dimension of the measurement vector, and [*] is the integer operation of the logarithm.

[0031] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, steps 2-3 update the adaptive factor α at time k based on the estimated sliding window at time k. k The expression is Among them, C k-j Let be the covariance matrix of the innovation error sequence at time kj.

[0032] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, the first filter divergence criterion is: δ is the control coefficient, δ≥1.

[0033] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, the second filter divergence criterion is:

[0034] As a further optimization of the dynamic target combination localization method based on adaptive Kalman filtering, steps 3-4 estimate the covariance matrix R of the measurement noise at time k+1 based on the sliding window at time k and the innovation error sequence of the previous k times. k+1 The expression is: Where, ε k-i Let be the innovation error sequence at time ki, and n be the dimension of the dynamic target state vector. χ is the weight coefficient of the second-order statistical characteristic of the μ-th Sigma point. μ,(k+1) Let h(*) be the μ-th Sigma point at time k+1, and h(*) be the measurement function.

[0035] The present invention, by adopting the above technical solution, has the following beneficial effects:

[0036] (1) An improved adaptive factor α for reducing measurement error is proposed in this invention. k It can improve the tracking accuracy of sudden maneuvers of dynamic targets in real time according to the noise of the current environment of the system. It overcomes the shortcomings of the traditional algorithm in that the fixed value α is difficult to take into account different maneuvering environments. In the localization of dynamic targets, the adaptive Kalman filter method based on dynamic window adjustment can locate more accurately, reduce localization error, and meet the high-precision localization requirements.

[0037] (2) The adaptive unscented Kalman filtering method proposed in this invention effectively filters out abnormal measurement values ​​by introducing two filtering divergence criteria, and calculates the covariance matrix R of the measurement noise of the dynamic sliding window based on the innovation error iteration. k This greatly reduces the possibility of filter divergence, improves the reliability of the adaptive unscented Kalman filter method and the tracking speed for dynamic target state changes, and enhances positioning accuracy. Attached Figure Description

[0038] Figure 1 This is a flowchart of the overall scheme of the dynamic combination localization method based on adaptive Kalman filtering proposed in this invention.

[0039] Figure 2This is a flowchart of the adaptive AKF method proposed in this invention.

[0040] Figure 3 This is a flowchart of the adaptive AUKF method proposed in this invention.

[0041] Figure 4 This is a flowchart illustrating the prediction of the covariance matrix using the adaptive AUKF method proposed in this invention.

[0042] Figure 5 This is a flowchart illustrating the process of updating the posterior covariance matrix using the adaptive AUKF method proposed in this invention.

[0043] Figure 6 This represents the actual trajectory of the dynamic target in this embodiment of the invention.

[0044] Figure 7 This is a measurement trajectory under high signal-to-noise ratio in an embodiment of the present invention.

[0045] Figure 8 This is a measurement trajectory under low signal-to-noise ratio in an embodiment of the present invention.

[0046] Figure 9 This is the positioning trajectory under high signal-to-noise ratio in an embodiment of the present invention.

[0047] Figure 10 This is the positioning trajectory under low signal-to-noise ratio in an embodiment of the present invention.

[0048] Figure 11 This refers to the Euclidean distance error under high signal-to-noise ratio in this embodiment of the invention.

[0049] Figure 12 This refers to the Euclidean distance error under low signal-to-noise ratio in this embodiment of the invention. Detailed Implementation

[0050] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments.

[0051] like Figure 1 As shown, the dynamic target combination localization method based on adaptive Kalman filtering proposed in this invention specifically includes the following sequential steps.

[0052] Step 1: First, determine whether the dynamic target is in a linear or nonlinear motion state based on the heading angle: when the heading angle of the dynamic target remains stable or changes slightly, it is determined that the dynamic target is in a linear motion state, and proceed to Step 2; when the heading angle of the dynamic target changes significantly, it is determined that the dynamic target is in a nonlinear motion state, and proceed to Step 3.

[0053] Step two: For dynamic targets in a linear motion state, a single-point positioning method based on AKF is used for positioning. Specifically, the KF algorithm is used to filter the single-point GPS measurement results. In order to improve the problem of low single-point GPS positioning accuracy, this application adopts an adaptive Kalman filter (AKF) method based on covariance matching, which improves the single-point positioning accuracy while effectively saving costs.

[0054] Step 3: For dynamic targets in a nonlinear turning motion, a combined positioning method based on AUKF is used for positioning. Specifically, UWB and GPS measurement data are fused, and an improved Adaptive Unscented Kalman Filter (AUKF) method is used on the basis of UKF. This method introduces two filtering divergence criteria, preprocesses the measurement data, and then performs combined positioning to improve the tracking and positioning accuracy of dynamic targets.

[0055] When the heading angle of a dynamic target remains stable or changes only slightly (less than or equal to 2° / s), it is considered to be undergoing linear motion. The state-space model for linear states is constructed as follows:

[0056] Consider a linear discrete-time dynamic system:

[0057] X k =FX k-1 +W k-1 (1)

[0058] Z k =HX k +V k (2)

[0059] In equations (1) to (2), X k Z is the n×1 dimensional state vector of the dynamic target at time k. k Let W be the m×1 measurement vector of the dynamic target at time k, and F and H be the state transition matrix and measurement matrix, respectively. k-1 and V k These are uncorrelated, zero-mean random Gaussian white noise sequences, representing the model noise at time k and the measurement noise at time k, respectively:

[0060]

[0061] Then its covariance matrix is:

[0062]

[0063] In equation (4), W i Let Q be the model noise at time i.k R k Let δ be the covariance matrix of the model noise at time k, and δ be the covariance matrix of the measurement noise at time k. i Let be the value of the impulse function at time i.

[0064] The motion model for linear motion states generally adopts the constant acceleration (CA) model. Based on kinematic analysis, using the CA model, the state vector of the dynamic target is generally set as follows:

[0065] X k =[x k y k v xk v yk a xk a yk ] T (5)

[0066] In equation (5), x k v xk a xk These are the position coordinates, velocity, and acceleration of the dynamic target along the X-axis at time k, and y-axis, respectively. k v yk a yk These are the position coordinates, velocity, and acceleration of the dynamic target along the Y-axis at time k.

[0067] The state transition matrix F of the system is:

[0068]

[0069] In equation (6), Δt is the time difference between time k+1 and time k.

[0070] Considering that dynamic target tracking and localization methods are most concerned with the target's position coordinates and have certain requirements for real-time filtering, this paper simplifies the measurement vector to Z. k =[x k y k ] T Then the simplified measurement matrix H is:

[0071]

[0072] The traditional KF solution steps are as follows:

[0073]

[0074] P (k|k-1) =FP (k-1|k-1) F T +Q k-1 (9)

[0075] K k =P (k|k-1) H T *[HP (k|k-1) H T +R k (10)

[0076]

[0077] P (k|k) =(IK (k) H)P (k|k-1) (12)

[0078] In equations (8) to (12), It is a priori estimate of the dynamic target pose state at time k. It is the posterior estimate of the dynamic target's pose state at time k. P (k|k-1) P is the prior estimate of the error covariance matrix at time k. (k|k) It is the posterior estimate of the error covariance matrix at time k, K (k) It is the Kalman gain. This is used to adjust the covariance matrix R of the measurement noise during the recursive optimization process. k Real-time updates are performed, and the covariance matching method is introduced to calculate the innovation sequence ε at time k. k The covariance matrix C k .

[0079] Covariance matching methods include Innovation-based Adaptive Estimation (IAE) and Residual-based Adaptive Estimation (RAE). The innovation error sequence of IAE and the residual sequence of RAE are defined as follows:

[0080]

[0081]

[0082] Substituting equation (2) into equation (13), we obtain the covariance matching formula:

[0083]

[0084] Taking the variance of both sides of equation (15), considering equations (3) and (4), and the orthogonality between measurement error and model error, we can obtain the innovation error sequence ε at time k. k The covariance matrix C k for:

[0085]

[0086] The core of adaptive Kalman filtering lies in the adaptive factor α. k Introducing a new information covariance matrix C k This allows for real-time updates of the measurement noise covariance matrix R. k To reduce the influence of measurement noise, equation (16) can be rewritten as:

[0087] C k ′=HP k|k-1 H T +α k R k (17)

[0088] Substituting equation (17) into equation (10), we obtain the adaptive Kalman gain coefficient:

[0089] K k =P (k|k-1) H T *[HP (k|k-1) H T +α k R k ] = P (k|k-1) H T (C′ k ) -1 (18)

[0090] According to optimal filter theory, the adaptive factor satisfies the following equation:

[0091] P k|k-1 H T -K k C k =0 (19)

[0092] Combining equations (18) and (19), we get:

[0093] P k|k-1 H T -[P k|k-1 H T (C′ k ) -1 )]C k =0 (20)

[0094] Simplifying, we get:

[0095] I-(C′ k ) -1 C k =0 (21)

[0096] Right now:

[0097] C k =C′ k (twenty two)

[0098] Substituting equation (17) into equation (22), we get:

[0099] C k =HP k|k-1 H T +α k R k (twenty three)

[0100] Taking the trace of both sides of equation (23) simultaneously, we obtain the adaptive factor α. k :

[0101]

[0102] Dynamic window adjustment is introduced into equation (24), and the innovation error sequence defined in equation (13) is used as the criterion for evaluating the changes in system state. The change in the noise of the positioning system will directly affect the magnitude and change of the innovation sequence. Therefore, the estimated window size N is dynamically adjusted by using the distance between the innovation sequence at time k and the mean of the innovation sequence at the previous k-1 times as follows:

[0103]

[0104] In equation (25), N k N k+1 Let k be the window size at time k and k+1. It is the average modulus of the innovation sequence in the j-th measurement dimension at the first k-1 time points, ε k (j) is the information sequence of the j-th measurement dimension at time k, L is the measurement dimension of the measurement vector, and [] is the integer part of the value.

[0105] The adaptive factor introduced for dynamic window estimation is:

[0106]

[0107] Considering practical applications, α k It needs to be greater than or equal to 1, therefore:

[0108]

[0109] In summary, the flowchart of the adaptive Kalman filter method based on dynamic window adjustment is as follows: Figure 2 As shown, when the motion model of a dynamic target is determined to be nonlinear based on its heading angle (i.e., when its heading angle change rate is greater than 2° / s), the target's pose model is quite complex, and a single measurement method often struggles to accurately estimate it. The KF algorithm, which fuses information from multiple sensors, can compensate for the shortcomings of individual sensors, thereby enabling accurate estimation of nonlinear motion pose.

[0110] Consider another nonlinear discrete-time dynamic system:

[0111] X k =f(X) k-1 )+W k-1 (28)

[0112] Z k =h(X) k )+V k (29)

[0113] In equations (28) to (29), X k Z is the n×1 dimensional state vector of the dynamic target at time k. k Let W be the m×1 measurement vector of the dynamic target at time k, and let f and h be the state transition function and measurement function, respectively, where f is a nonlinear function. k-1 and V k These are uncorrelated, zero-mean random Gaussian white noise sequences, representing the model noise at time k and the measurement noise at time k, respectively. Their covariance matrix is:

[0114]

[0115] When a dynamic target is in a nonlinear motion state, a Constant Turn Rate and Velocity (CTRV) model is adopted, and its real-time measurement data is obtained through GPS and UWB.

[0116] Based on kinematic analysis and using the CTRV model, the state vector of a dynamic target is generally set as follows:

[0117] X k =[x k y k θ k v k ] T (31)

[0118] In equation (31), x k y k These are the position coordinates of the dynamic target along the X and Y axes at time k, respectively, and θ k It is the heading angle of the dynamic target at time k, v k It is the velocity of the dynamic target at time k.

[0119] Then the system's state transition function f(·) is:

[0120]

[0121] In equation (32), Δt is the time difference between time k+1 and time k, X(Δt) is the pose shift of the dynamic target after time interval Δt, f(·) is the dynamic pose state update equation, v(t) is the linear velocity of the dynamic target at time t, and x(t) and y(t) are the position coordinates of the object under test along the X and Y axes at time t, respectively. These are the position coordinates along the X-axis, position coordinates along the Y-axis, heading angle, and linear velocity offset of the dynamic target over a time interval Δt, respectively, while w is the angular velocity of the dynamic target.

[0122] Measurement vector Z k =[x k y k θ k v k ] T The position coordinates (x, y) of the dynamic target are obtained by UWB acquisition and calculation, while the heading angle θ and linear velocity v of the dynamic target are obtained by parsing GPS messages. The simplified measurement function h is:

[0123]

[0124] After establishing the state-space model under nonlinear motion conditions, a filtering divergence criterion based on the covariance matching method is introduced for the traditional UKF scheme, and the covariance matrix R of the measurement noise is adjusted in real time. k .

[0125] The traditional UKF solution involves the following steps:

[0126] (1) Initialization

[0127]

[0128] In equation (34), P0 is the estimated initial value of the dynamic target pose state, and P0 is the estimated initial value of the covariance matrix.

[0129] (2) Construct the Sigma point set

[0130] The most widely used Sigma point sampling strategy currently is the centrosymmetric sampling strategy. The formula for constructing centrosymmetric Sigma points is as follows:

[0131]

[0132] In equation (35), χ 0,k It is the 0th Sigma point at time k, χ μ,k It is the μ-th Sigma point at time k, where λ is the scaling factor, λ = α 2(n+κ)-n, where n is the dimension of the dynamic target state vector at time k as shown in equation (31), and α(n+κ) represents the variable controlling the distance between the Sigma point and the mean of the dynamic target state vector. Represents the square root of a matrix The μth column. The first-order statistical characteristic weight coefficients of the 0th Sigma point. and the first-order statistical characteristic weight coefficients of the μ-th Sigma point and the weight coefficients of the second-order statistical characteristics at the μ-th Sigma point as follows:

[0133]

[0134] In equation (36), β is the non-negative weight coefficient to be selected, β≥0, and the Gaussian distribution is generally taken as 2.

[0135] (3) Calculate the one-step prediction matrix and covariance matrix at time k:

[0136] χ μ,(k+1|k) =f(χ) μ,(k|k) μ = 0, 1, 2...2n (37)

[0137]

[0138] In equations (37) and (38), Let P be the prior estimate matrix of the state vector of the dynamic target at time k+1. (k+1|k) χ is the prior estimate of the error covariance matrix at time k+1. μ,(k|k) Let χ be the μ-th Sigma point estimated a priori at time k. μ,(k+1|k) Let be the μ-th Sigma point estimated posteriorly at time k.

[0139] (4) Using the UT transformation a second time, we obtain a new Sigma point set and measurement vector:

[0140]

[0141]

[0142] (5) Calculate the covariance matrix of the system prediction:

[0143]

[0144]

[0145] In equations (41) to (42), The measurement vector at time k+1 The autocovariance matrix, The state vector at time k+1 and the measurement vector at time k+1 The cross-covariance matrix.

[0146] (6) Calculate the Kalman gain matrix:

[0147]

[0148] (7) Filter update:

[0149]

[0150]

[0151] To address the issues of nonlinear filtering algorithms being susceptible to abnormal measurement disturbances and typical UKF algorithms easily diverging, a filtering divergence criterion based on the covariance matching method is introduced.

[0152] When the calculated prediction error is much smaller than the actual estimation error, the filter is determined to be in a divergent state. The divergence condition is:

[0153]

[0154] In equation (46), ε k The innovation error is defined by equation (13), representing the true estimation error at time k; δ represents the theoretical estimation error at time k; δ is an adjustable control coefficient (δ≥1). When the actual estimation error is greater than δ times the theoretical estimation error, the filter is considered to be in a divergent state, meaning the error between the estimated value and the true value is too large. In practical applications, The covariance matrix of the measured values ​​is often difficult to obtain. This can be expressed as the expected value of the sum of squares of the measurement residuals, therefore equation (46) can be rewritten as:

[0155]

[0156] Furthermore, in nonlinear motion states, when the measurement error or model error is large, i.e., the innovation error ε k When the value is large, it can lead to significant estimation errors in the filter during the measurement correction phase. In this case, simply updating R adaptively is often insufficient to reduce the measurement error. To filter out abnormal measurement values, a second divergence criterion is introduced:

[0157] |ε k |>ξ (48)

[0158] In equation (48), ξ is a pre-set filtering threshold. When the innovation error ε kWhen the magnitude of the measurement error exceeds the threshold ξ, it is considered an abnormal measurement value. The measurement noise covariance matrix R is then set to infinity. At this point, the Kalman gain K affected by R tends to 0, meaning only state model prediction is performed, without measurement correction, thus reducing the interference caused by abnormal measurements to the filter. When the innovation error ε... k State model prediction and measurement correction are performed simultaneously only when the modulus is less than the threshold ξ, that is, when the model error is low and the measurement accuracy is high.

[0159] An adaptive calculation method for the R matrix based on a sliding window is introduced for UKF. The estimated window size N is dynamically adjusted as shown in Equation (25). The method for dynamically adjusting R is by utilizing the innovation errors ε from multiple previous time steps. k For R at the next moment k+1 Perform recursion:

[0160]

[0161] The adaptive unscented Kalman filtering method proposed in this invention effectively filters out abnormal measurement values ​​by introducing two filtering divergence criteria; and based on the innovation error, a covariance matrix R of the measurement noise of a dynamic sliding window is designed. k Iterative formula. Two filter divergence criteria are introduced. First, it determines whether the filter is in a divergent state. When the filter is in a divergent state, a smaller R is used. k This accelerates the convergence speed of the filter and reduces its divergence. When the filter is in steady state, a threshold ξ is used to determine whether the magnitude of the innovation error is too large. When the error magnitude exceeds the threshold ξ, it is considered an abnormal measurement value, and R is set... k+1 =0, thus causing the Kalman gain to approach infinity, meaning that the prior estimate of the state is not corrected based on the measured value. When the error magnitude is less than or equal to the threshold ξ, it is considered a normal measured value, and R is recalculated through a dynamic sliding window on the innovation sequence. k+1 By correcting the prior estimates of the state using measured values, the filtering accuracy is further improved. The flowchart of the adaptive unscented Kalman filter method based on dynamic window adjustment is shown below. Figure 3 As shown. Figure 3 The specific details of the prediction module are as follows: Figure 4 As shown, the specific details of the measurement update module are as follows: Figure 5 As shown.

[0162] (3) Performance analysis of the final embodiment:

[0163] The simulation performance of the dynamic target combination localization method based on adaptive Kalman filtering proposed in this invention is compared with that of the traditional Kalman filtering localization method and the adaptive Kalman filtering localization method without the introduction of sliding window. The simulation parameters are shown in Table 1, and the average error of the simulation results is shown in Table 2.

[0164] Table 1 Simulation Parameters

[0165]

[0166] Table 2. Average Euclidean distance error of the method (unit: m)

[0167] low signal-to-noise ratio High signal-to-noise ratio Typical methods 1.100781 0.324015 Adaptive Method 1 0.768326 0.27294 Method of the present invention 0.405906 0.152158

[0168] comprehensive Figure 6 , 9 As shown in section 10, regardless of whether the measured values ​​have a low or high signal-to-noise ratio (SNR), the combined positioning method proposed in this invention matches the true trajectory better than the typical Kalman filter method and the adaptive Kalman method 1 without a sliding window. This indicates that the proposed method can effectively suppress errors, resulting in smaller positioning error values ​​and improvements in both positioning accuracy and trajectory smoothness, thus enabling more accurate tracking and positioning of dynamic targets. Typical methods often rely heavily on accurate estimation of noise statistical characteristics. When the noise variance is large, i.e., when noise changes frequently, it is difficult to correct the system model using measured values. Because it does not update the measured noise covariance matrix, the influence of new measured values ​​on measurement correction decreases with iteration, easily leading to filter divergence or reduced filtering accuracy. Although the adaptive method 1 introduces adaptive adjustment, it does not use a dynamic sliding window. When the system is in a relatively steady state, it still only processes the data at the current moment without considering the historical residual sequence, making it susceptible to the influence of some abnormal measured values, resulting in lower filtering accuracy than the improved adaptive positioning method proposed in this paper.

[0169] comprehensive Figures 7-10 It is evident that the combined positioning method proposed in this invention can filter and smooth high- and low-precision position coordinate measurements, accurately model the motion state of dynamic targets through sensors, effectively filter out abnormal measurement values, and correct the measurement values ​​through prior estimates of the motion model, thereby effectively improving positioning accuracy. The trajectory map obtained by the combined positioning method proposed in this invention is significantly closer to the real trajectory than the measurement values, proving that the positioning method of this invention can suppress the influence of measurement value deviation and measurement noise characteristic changes on the stability of filtering and noise reduction, that is, under the premise of efficiently and accurately eliminating error data, it retains the effective measurement data information to the maximum extent.

[0170] comprehensive Figure 11 , 12As shown in Table 2, under conditions of low signal-to-noise ratio in the measured values, the improved adaptive Kalman filter combined positioning method proposed in this invention improves the positioning accuracy by 63.13% compared to the typical Kalman filter positioning system, and by 30.20% compared to the adaptive Kalman filter positioning system without a sliding window. Under conditions of high signal-to-noise ratio in the measured values, the improved adaptive Kalman filter combined positioning method proposed in this invention improves the positioning accuracy by 53.04% compared to the typical Kalman filter positioning system, and by 44.25% compared to the adaptive Kalman filter positioning system without a sliding window. In other words, the data processed by the improved adaptive positioning method proposed in this invention is closer to the true location coordinates, demonstrating the superiority and universality of this method.

[0171] (4) Conclusion:

[0172] This invention proposes an improved adaptive Kalman single-point localization method and an improved unscented Kalman combined localization method for linear and nonlinear motion states of dynamic targets, respectively. Both methods calculate the dynamic sliding window size based on the innovation error and estimate the measurement noise covariance matrix in real time, representing an adaptive adjustment method based on covariance matching. Addressing the issue of nonlinear filters being more prone to divergence and having lower accuracy, this invention introduces two filter divergence criteria in the improved unscented Kalman combined localization method to eliminate error data and prevent it from affecting the noise reduction effect. Furthermore, it can dynamically adjust the filter's tolerance to error by changing the threshold, increasing the filter's universality and stability. Simulation results show that the proposed fusion localization method can suppress the impact of measurement deviations and unknown system noise characteristics on the stability of filtering and noise reduction even when measurement noise characteristics are unknown or complex, achieving a relatively accurate localization effect for dynamic targets.

[0173] The above specific implementation methods and embodiments are specific support for the technical ideas proposed in this invention, and should not be used to limit the scope of protection of this invention. Any equivalent changes or modifications made based on the technical solutions of this invention in accordance with the technical ideas proposed in this invention shall still fall within the scope of protection of this invention.

Claims

1. A dynamic target combination localization method based on adaptive Kalman filtering, characterized in that, Includes the following steps: Step 1: Determine the motion state of the heading angle based on the heading angle of the dynamic target. If the dynamic target is in a linear motion state, proceed to Step 2; if the dynamic target is in a nonlinear motion state, proceed to Step 3. Step 2: A sliding window adaptive Kalman filter algorithm based on covariance matching is used to perform single-point localization of the dynamic target. The sliding window adaptive Kalman filter algorithm based on covariance matching updates the adaptive factor α at time k according to the estimated sliding window at time k. k The expression is Among them, C k-j Let N be the covariance matrix of the innovation error sequence at time kj. k Let R be the window size at time k. k Let P be the covariance matrix of the measurement noise at time k. k|k-1 Here, H is the prior estimate of the error covariance matrix at time k, H is the measurement matrix, and tr(*) is the trace operation; Step 3: Construct measurement vectors by fusing UWB and GPS measurement data, establish a state-space model under nonlinear motion conditions, and use an adaptive unscented Kalman filter algorithm with two covariance-matched filtering divergence criteria to perform combined localization of the dynamic target. The first filtering divergence criterion is... The second filter divergence criterion is: ε k Let k be the innovation error sequence. Let δ be the autocovariance matrix of the dynamic target measurement vector at time k+1, where δ is the control coefficient and δ≥1.

2. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 1, characterized in that, The specific method for determining the motion state of the heading angle in step 1 based on the heading angle of the dynamic target is as follows: when the change of the heading angle of the dynamic target is less than or equal to the threshold, the dynamic target is determined to be in a linear motion state; when the change of the heading angle of the dynamic target is greater than the threshold, the dynamic target is determined to be in a nonlinear motion state.

3. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 2, characterized in that, The specific method for single-point localization of dynamic targets using the sliding window adaptive Kalman filter algorithm based on covariance matching in step 2 is as follows: Step 2-1: Initialize the dynamic target state vector and error covariance matrix; Step 2-2: Predict the prior estimate of the dynamic target state vector at time k. Prior estimate P of the error covariance matrix at time k k|k-1 ; Steps 2-3: Obtain the innovation error sequence ε at time k based on the covariance matching method. k The covariance matrix C k The adaptive factor α at time k k After introducing the covariance matrix of the innovation error sequence at time k, the Kalman gain coefficient K at time k is estimated. k Calculate the covariance matrix C of the innovation error sequence at time k that satisfies the optimal filter theory. k Adaptive factor α at time k k The sliding window at time k+1 is estimated based on the distance between the mean values ​​of the innovation error sequence at time k and the residual sequences at the previous time k-1. The adaptive factor α at time k is then updated based on the estimated sliding window at time k. k ; Step 2-4: Based on the adaptive factor α updated in step 2-3 at time k. k Step 2-3 estimates the Kalman gain coefficient K at time k. k Perform state prediction and measurement updates for dynamic targets.

4. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 3, characterized in that, The specific method for combining and locating dynamic targets in step 3 by introducing two filter divergence criteria based on covariance matching is as follows: Step 3-1: Initialize the dynamic target state vector and error covariance matrix; Step 3-2: Perform the first UT transformation on the predicted state vector of the dynamic target at time k, construct the Sigma point set, and predict the state vector of the dynamic target at time k+1 based on the first-order and second-order statistical characteristic weight coefficients of the Sigma point set. The error covariance matrix P at time k+1 (k+1|k) ; Step 3-3: The state vector of the dynamic target at time k+1 predicted in step 3-2... Perform a second UT transformation to obtain the updated Sigma point set, and predict the measurement vector of the dynamic target at time k+1. The autocovariance matrix of the dynamic target measurement vector at time k+1 The cross-covariance matrix of the dynamic target measurement vector at time k+1 Kalman gain K at time k+1 k+1 According to the Kalman gain K at time k+1 k+1 Perform filter updates; Steps 3-4 introduce two filtering divergence criteria to update the covariance matrix R of the measurement noise at time k+1. k+1 : When the innovation error sequence at time k satisfies the first filter divergence criterion, update the covariance matrix R of the measurement noise at time k+1. k+1 R is the covariance matrix of the measurement noise at time k. k , When the innovation error sequence at time k does not satisfy the first filtering divergence criterion but satisfies the second filtering divergence criterion, the covariance matrix R of the measurement noise at time k+1 is... k+1 Assign an infinity value and set the Kalman gain K at time k+1 to... k+1 Assigning a value of 0 means performing state prediction only on the dynamic target state vector. When the innovation error sequence at time k satisfies the second filter divergence criterion, the covariance matrix R of the measurement noise at time k+1 is estimated based on the sliding window at time k and the innovation error sequences at the previous k times. k+1 Based on the covariance matrix R of the noise measured at time k+1 k+1 Perform state prediction and measurement correction on the dynamic target state vector.

5. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 4, characterized in that, In steps 2-3, the covariance matrix C of the innovation error sequence at time k that satisfies the optimal filter theory is... k C k =HP k|k-1 H T +α k R k The adaptive factor at time k satisfies the optimal filter theory.

6. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 5, characterized in that, The expression for estimating the sliding window at time k+1 based on the distance between the mean of the innovation error sequence at time k and the mean of the residual sequence at the previous k-1 times is as follows: Where, N k+1 Let ε be the window size at time k+1. k (j) is the information sequence of the j-th measurement dimension at time k. Let L be the average magnitude of the information sequence of the j-th measurement dimension at the first k-1 time points, where L is the measurement dimension of the measurement vector, and [*] is the integer operation of the logarithm.

7. The dynamic target combination localization method based on adaptive Kalman filtering according to claim 6, characterized in that, Steps 3-4 estimate the covariance matrix R of the measurement noise at time k+1 based on the k-time sliding window and the innovation error sequence from the previous k-times. k+1 The expression is: Where, ε k-i Let be the innovation error sequence at time ki, and n be the dimension of the dynamic target state vector. χ is the weight coefficient of the second-order statistical characteristic of the μ-th Sigma point. μ,(k+1) Let h(*) be the μ-th Sigma point at time k+1, and h(*) be the measurement function.

Images