CN116520380A - Dynamic target combined positioning method based on self-adaptive Kalman filtering - Google Patents

Abstract

The invention discloses a dynamic target combined positioning method based on adaptive Kalman filter, and belongs to the technical field of calculation, calculation or counting. This combined positioning method is used in two application scenarios where the dynamic target is in a linear state and a nonlinear state. Based on the combined positioning of the global positioning system and ultra-wideband ranging information, an adaptive factor and a dynamic window adjustment are introduced based on the covariance matching method. An adaptive Kalman filter combined positioning method that combines UWB and GPS is proposed. In the state of linear motion of the target, the sliding window adaptive Kalman filter method based on covariance matching is used for GPS single-point positioning; in the state of target nonlinear motion , UWB/GPS combined positioning based on the adaptive unscented Kalman filter method with filter divergence criterion. The invention improves the positioning accuracy and stability of the Kalman filtering method based on the fusion of UWB and GPS.

Description

A Dynamic Target Combination Localization Method Based on Adaptive Kalman Filter

technical field

The invention relates to positioning technology in communication, in particular to a combined positioning scheme adaptively updated according to a dynamic target motion model and real-time measurement data, and discloses a dynamic combined positioning method based on adaptive Kalman filtering, which belongs to calculation and calculation Or count technical fields.

Background technique

With the rise of Industry 4.0, the Internet of Things is developing rapidly, and the demand for location-based services (Location Based Services, LBS) is particularly growing. LBS uses different positioning technologies to obtain the current location of the positioning device, and provides information resources and basic services to the positioning device through the mobile Internet, thereby realizing intelligent positioning and tracking. In the context of Industry 4.0, people have higher and higher requirements for the positioning and tracking of dynamic targets, which makes the positioning and tracking technology of dynamic targets in LBS have important research value.

When tracking and positioning a dynamic target, it is necessary to obtain the measurement value of the target's pose information from the positioning sensor. Global Navigation Satellite System (GNSS) is a space-based radio navigation and positioning system that can provide all-round, all-weather, all-time coordinates, speed, and time information. Among the GNSS positioning systems, the most widely used is the global positioning system (Global Positioning System, GPS) in single-point positioning mode, which has low price and wide coverage, but the positioning accuracy is not high, especially in the case of building occlusion , the positioning accuracy is severely degraded. Ultra Wide Band (UWB) is a wireless carrier communication technology that uses nanosecond-level non-sinusoidal narrow pulses to transmit information, and provides centimeter-level positioning accuracy through a relatively low-power system, but the hardware cost is high and The system is complex and cannot be used in a large area. Combining the characteristics of GPS and UWB positioning technology, it can be seen that GPS is suitable for the situation where the motion state is simple and the positioning accuracy is not high, and its cost is low and the coverage is wide; GPS and UWB can be used for fusion positioning, and the positioning accuracy can be improved under the premise of taking into account the cost.

At present, the difficulty of dynamic target tracking and positioning mainly lies in two points: one is the uncertainty of the dynamic target motion model, that is, the positioning system cannot accurately estimate the motion model of the positioned target; the other is the uncertainty of the dynamic target observation data, that is, the positioning The system cannot accurately observe the pose information of the positioned target. The above two uncertainties lead to model noise and measurement noise in the positioning system respectively. Therefore, dynamic compensation of model noise errors and reduction of the impact of measurement noise errors is the key to improving the accuracy of dynamic target tracking and positioning. The dynamic target tracking and positioning method not only needs to meet high positioning accuracy, but also needs to ensure the real-time performance of filtering processing. The Kalman Filter algorithm (Kalman Filter, KF) has the characteristics of simple model and small data storage, and has been widely used in dynamic target tracking and positioning.

The motion equations or measurement equations of dynamic targets are divided into linear and nonlinear. Therefore, dynamic target tracking and positioning can be divided into linear filtering problems and nonlinear filtering problems. The KF algorithm combined with the least square method proposes a recursive optimal solution to the linear filtering problem based on the state space of the target. The optimal solution of the nonlinear filtering problem needs to be obtained by obtaining the conditional posterior probability through endless parameters. In practical applications, it is solved by suboptimal approximation to the nonlinear problem. Based on the first-order linear truncation of the Taylor expansion of the nonlinear function, ignoring the higher-order terms, and linear approximation of the nonlinear function, the Extended Kalman Filter algorithm (Extended Kalman Filter, EKF) is developed. Based on the sampling strategy to approximate the probability density distribution of nonlinear functions, the Unscented Kalman Filter (UKF) algorithm is developed.

Since the EKF is a first-order Taylor expansion of the nonlinear function near the predicted value of the state space, when the nonlinearity of the target's motion state is high, the high-order terms of the Taylor expansion cannot be ignored, resulting in a large system error. In addition, EKF needs to derive the Jacobian matrix of the nonlinear function, which increases the difficulty of calculation. The UKF is based on the UT transformation, and approximates the probability density distribution of the nonlinear function through a deterministic sampling strategy, so that the calculation accuracy of the nonlinear statistics can reach at least the second order, so that the calculation amount is the same as that of the EKF algorithm. filtering precision.

The filtering accuracy of the linear KF algorithm and the nonlinear UKF algorithm depends on the accuracy of the state model and measurement data, as well as the accurate estimation of the statistical characteristics of the model noise and measurement noise, that is, the covariance matrix Q of the model noise and the measurement noise Covariance matrix R. The typical KF algorithm and UKF algorithm generally obtain the statistical characteristics of the noise through comprehensive analysis of a large number of experiments in advance, and fix the Q and R matrices to a certain value. However, when the typical KF algorithm or typical UKF algorithm is applied to dynamic target tracking and positioning, the change of the target motion state will affect the statistical characteristics of the model noise and measurement noise, resulting in an increase in the uncertainty of the Q and R matrices, thereby reducing the filtering efficiency. accuracy, and even cause the filter to diverge.

Adaptive filtering is a filtering method that can adjust system parameters in real time, reduce the influence of errors, and suppress the divergence of filters. There are many kinds of adaptive filtering methods. The Sage-Husa adaptive filtering algorithm obtains the estimated value of the state quantity based on the observation sequence, and calculates the mean value and covariance matrix of model noise and measurement noise according to the principle of maximum a posteriori estimation. Using the improved Sage-Husa suboptimal unbiased maximum a posteriori estimator and introducing an adaptive attenuation factor to modify the covariance of prediction errors, an improved adaptive UKF algorithm can be obtained. On the basis of the fading filter algorithm, the researchers introduced filter convergence criteria and selected an appropriate forgetting factor to make the actual error smaller than the theoretical error. There are also a few studies based on innovation-based adaptive estimation and multi-model-based adaptive estimation methods, introducing the maximum likelihood criterion to adjust the Kalman gain coefficient of the filter in real time. Another researcher improved the UKF algorithm from the perspective of adaptively adjusting process noise and measurement noise and correcting Kalman gain, and proposed an adaptive Kalman filter algorithm based on covariance matching method, introducing a fixed-width sliding window to update Q, R matrix, which improves the robustness of the algorithm. However, the existing Sage-Husa adaptive filtering algorithm can only estimate R when Q is known or estimate Q when R is known, and since most adaptive algorithms do not introduce filter divergence criteria, there is a certain filter divergence Therefore, the positioning accuracy of the existing Sage-Husa adaptive filtering algorithm still has room for improvement.

Based on the above research, it can be seen that the key to improving the positioning accuracy of Kalman filter algorithm for dynamic targets is to reduce the estimation error caused by the uncertainty of Q and R matrices. The present invention aims to propose a dynamic combined positioning method based on adaptive Kalman filter to overcome the above-mentioned defects.

Contents of the invention

The purpose of the present invention is to address the deficiencies of the above-mentioned background technology, on the basis of the research of adaptive filtering, propose a dynamic combined positioning method based on adaptive Kalman filtering, realize the improvement of the response accuracy of Kalman filtering to dynamic targets and effectively The purpose of the invention to suppress the divergence of the filter is to solve the technical problem of how to improve the positioning accuracy of the dynamic target tracking and positioning method and ensure the real-time performance of filtering processing.

The present invention adopts following technical scheme for realizing above-mentioned invention purpose: a kind of dynamic target combination localization method based on self-adaptive Kalman filter, it is characterized in that, comprises the following steps:

Step 1, determine the motion state of the heading angle according to the heading angle of the dynamic target, enter step 2 when the dynamic target is in a linear motion state, and enter step 3 when the dynamic target is in a non-linear motion state;

Step 2, using the sliding window adaptive Kalman filter algorithm based on covariance matching to perform single-point positioning on the dynamic target;

Step 3: Fusion of UWB measurement data and GPS measurement data to construct a measurement vector, establish a state space model under nonlinear motion, and adopt an adaptive unscented Kalman filter algorithm that introduces two filtering divergence criteria based on covariance matching Combined positioning with dynamic targets.

As a further optimization scheme of the dynamic target combined positioning method based on adaptive Kalman filtering, the specific method of step 1 to determine the motion state of the heading angle according to the heading angle of the dynamic target is: when the change of the heading angle of the dynamic target is less than or equal to the threshold When , it is determined that the dynamic target is in a state of linear motion; when the change of the heading angle of the dynamic target is greater than the threshold, it is determined that the dynamic target is in a state of nonlinear motion.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filter, the specific method of single-point positioning 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 estimation of the dynamic target state vector at time k Predict the prior estimate P _(k|k-1) of the error covariance matrix at time k;

Step 2-3: Obtain the covariance matrix C _k of the innovation error sequence ε _k at time k based on the covariance matching method, introduce the adaptive factor α _k at time k into the covariance matrix of the innovation error sequence at time k, and estimate Kalman at time k Gain coefficient K _k , calculate the covariance matrix C _k of the innovation error sequence at time k satisfying the optimal filter theory, and the adaptive factor α k at time _k , according to the mean value of the innovation error sequence at time k and the residual sequence at time k-1 Estimate the sliding window at time k+1, and update the adaptive factor α k at time _k according to the estimated sliding window at time k;

Step 2-4: Carry out state prediction and measurement update for the dynamic target according to the adaptive factor α _k at time k updated in step 2-3 and the Kalman gain coefficient K _k at time k estimated in step 2-3.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filter, in step 3, the adaptive unscented Kalman filter algorithm that introduces two filter divergence criteria based on covariance matching is used to carry out the dynamic target The specific method of combined positioning 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 prediction result of the dynamic target state vector at time k, construct a Sigma point set, and predict the dynamics at time k+1 according to the first-order statistical characteristic weight coefficient and the second-order statistical characteristic weight coefficient of the Sigma point set target's state vector The error covariance matrix P _(k+1|k ) at time k+1;

Step 3-3, the state vector of the dynamic target at time k+1 predicted by step 3-2 Perform the second UT transformation, obtain the updated Sigma point set, and predict the measurement vector of the dynamic target at time k+1 /> Autocovariance matrix of dynamic target measurement vector at time k+1/> Cross-covariance matrix of dynamic target measurement vector at time k+1/> Kalman gain K _{k+1 at time k+1} , filter and update according to Kalman gain K _{k+1 at time k+1} ;

Step 3-4, introduce two filter divergence criteria, and update the covariance matrix R k+1 of the measurement noise at time _k+1 :

When the innovation error sequence at time k satisfies the first filtering divergence criterion, update the covariance matrix R k+1 of the measurement noise at time _k+1 to be the covariance matrix R _k of the measurement noise at time k,

When the innovation error sequence at time k does not satisfy the first filter divergence criterion but satisfies the second filter divergence criterion, the covariance matrix R _k+1 of the measurement noise at time k+1 is assigned an infinite value, and at time k+1 The Kalman gain K _k+1 is assigned a value of 0, and only the dynamic target state vector is used for state prediction.

When the innovation error sequence at time k satisfies the second filtering divergence criterion, the covariance matrix R _k+1 of the measurement noise at time k+1 is estimated based on the sliding window at time k and the innovation error sequence at time k+1, and according to k+1 The covariance matrix R _k+1 of the measurement noise at all times performs state prediction and measurement correction on the dynamic target state vector.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filtering, the covariance matrix C _k of the innovation error sequence at time k that satisfies the optimal filter theory in steps 2-3 is C _k = HP _{k |k-1} H ^T +α _k R _k , the adaptive factor α _k at time k satisfying the optimal filter theory is Among them, H is the measurement matrix, and tr(*) is the trace operation.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filtering, step 2-3 estimates the k+1 time according to the distance between the innovation error sequence at time k and the mean value of the residual sequence at time k-1 The expression of the sliding window is Among them, N _k and N _k+1 are the window sizes at time k and k+1, ε _k (j) is the innovation sequence of the jth measurement dimension at time k, /> is the average modulus value of the innovation sequence of the jth measurement dimension at the previous k-1 time, L is the measurement dimension of the measurement vector, and [*] is the rounding operation of the logarithmic value.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filter, the expression of step 2-3 to update the adaptive factor α _k at time k according to the estimated sliding window at time k is: Among them, C _kj is the covariance matrix of the innovation error sequence at time kj.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filter, the first filter divergence criterion is δ is the control coefficient, δ≥1.

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filter, the second filter divergence criterion is

As a further optimization scheme of the dynamic target combination positioning method based on adaptive Kalman filtering, steps 3-4 estimate the covariance matrix of the measurement noise at k+1 time based on the sliding window at time k and the innovation error sequence at time k The expression of R _k+1 is: Among them, ε _ki is the innovation error sequence at time ki, n is 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) is the μth Sigma point at k+1 time, h(*) is the measurement function.

The present invention adopts the above-mentioned technical scheme, and has the following beneficial effects:

(1) An improved adaptive factor α _k that weakens the measurement error proposed by the present invention can improve the tracking accuracy of sudden maneuvers for dynamic targets in real time according to the noise situation in the current environment of the system, and improves the fixed value in the traditional algorithm α is difficult to take into account the defects in different maneuvering environments. In the positioning of dynamic targets, the adaptive Kalman filter method based on dynamic window adjustment can locate more accurately, reduce positioning errors, and meet high-precision positioning requirements.

(2) The adaptive unscented Kalman filter method proposed by the present invention effectively filters out abnormal measurement values by introducing two filter divergence criteria, and iteratively calculates the covariance matrix of the measurement noise of the dynamic sliding window based on the innovation error R _k , which greatly reduces the divergence possibility of the filter, improves the reliability of the adaptive unscented Kalman filter method and the tracking speed for dynamic target state changes, and improves the positioning accuracy.

Description of drawings

Fig. 1 is a flow chart of the overall scheme of the dynamic combined positioning method based on adaptive Kalman filtering proposed by the present invention.

Fig. 2 is a flow chart of the adaptive AKF method proposed by the present invention.

Fig. 3 is a flowchart of the adaptive AUKF method proposed in the present invention.

Fig. 4 is a flow chart of predicting the covariance matrix by the adaptive AUKF method proposed in the present invention.

Fig. 5 is a flow chart of updating the posterior covariance matrix by the adaptive AUKF method proposed in the present invention.

Fig. 6 is the real track of the dynamic target in the embodiment of the present invention.

FIG. 7 is a measurement trace at a high signal-to-noise ratio in an embodiment of the present invention.

FIG. 8 is a measurement trace at a low signal-to-noise ratio in an embodiment of the present invention.

Fig. 9 is a positioning track at a high signal-to-noise ratio in an embodiment of the present invention.

Fig. 10 is a positioning track at a low signal-to-noise ratio in an embodiment of the present invention.

Fig. 11 is the Euclidean distance error under high SNR in the embodiment of the present invention.

Fig. 12 is the Euclidean distance error under low signal-to-noise ratio in the embodiment of the present invention.

Detailed ways

The technical solution of the present invention will be further described in detail below in conjunction with the accompanying drawings and embodiments.

As shown in FIG. 1 , the present invention proposes a dynamic target combination positioning method based on adaptive Kalman filter, which specifically includes the following sequential steps.

Step 1, first judge the dynamic target is in a linear motion state or nonlinear motion state based on the heading angle: when the dynamic target heading angle remains stable or changes slightly, it is determined that the dynamic target is in a linear motion state, and then go to step 2; when the dynamic target heading angle occurs When there is a large change, it is determined that the dynamic target is in a nonlinear motion state, and then go to step 3.

Step 2, for the dynamic target in the state of linear motion, use the AKF-based single-point positioning method for positioning, specifically: use the KF algorithm to filter the single-point GPS measurement results, in order to improve the low accuracy of single-point GPS positioning Problem, this application adopts a sliding window adaptive Kalman filter (Adaptive Kalman Filter, AKF) method based on covariance matching to improve its single-point positioning accuracy on the premise of effectively saving costs.

Step 3: For the dynamic target in the turning nonlinear motion state, the combined positioning method based on AUKF is used for positioning. Kalman filter (Adaptive Unscented Kalman Filter, AUKF) method, this method introduces two filter divergence criteria, and then performs combined positioning after preprocessing the measurement data, so as to improve the tracking and positioning accuracy of dynamic targets.

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

Consider a linear discrete-time dynamical system:

X _k ＝ FX _k-1 +W _k-1 (1)

Z _k ＝HX _k +V _k (2)

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

Then its covariance matrix is:

In formula (4), W _i is the model noise at time i, Q _k and R _k are the covariance matrix of the model noise at time k, the covariance matrix of the measurement noise at time k, and δ _i is the value of the impulse function at time i.

The motion model of the linear motion state generally adopts the Constant Acceleration (CA) model. According to the kinematic analysis, the CA model is adopted, and the state vector of the dynamic target is generally set as:

X _k ＝[x _k y _k v _xk v _yk a _xk a _yk ] ^T (5)

In formula (5), x _k , v _xk , a _xk are the position coordinates, velocity, and acceleration of the dynamic target along the X axis at time k, respectively, and y _k , v _yk , a _{yk are} respectively Axis position coordinates, velocity, acceleration.

Then the state transition matrix F of the system is:

In formula (6), Δt is the time difference between time k+1 and time k.

Considering that the dynamic target tracking and positioning method is most concerned with the position coordinates of the target, and has 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 for:

The traditional KF scheme steps are as follows:

P _(k|k-1) = FP _(k-1|k-1) F ^T +Q _k-1 (9)

K _k ＝P _(k|k-1) H ^T *[HP _(k|k-1) H ^T +R _k ] (10)

P _(k|k) = (IK _(k) H)P _(k|k-1) (12)

From formula (8) to formula (12), is the prior estimate of the dynamic target pose state at time k, /> is the posterior estimate of the dynamic target pose state at time k. P _(k|k-1) is the prior estimate of the error covariance matrix at time k, P _(k|k) is the posterior estimate of the error covariance matrix at time k, and K _(k) is the Kalman gain. In order to update the covariance matrix R _k of the measurement noise in real time during the recursive optimization process, a covariance matching method is introduced to calculate the covariance matrix C _k of the innovation sequence ε _k at time k.

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

Substituting formula (2) into formula (13), the covariance matching formula is obtained:

Taking variance on both sides of equation (15), considering equations (3), (4), and the orthogonality between measurement error and model error, the covariance matrix C _k of the innovation error sequence ε _k at time k can be obtained as:

The core of adaptive Kalman filtering is to introduce the adaptive factor α _k into the innovation covariance matrix C _k , so as to update the measurement noise covariance matrix R _k in real time and weaken the influence of measurement noise. Formula (16) is rewritten as:

C _k ′＝HP _k|k-1 H ^T +α _k R _k (17)

Substituting Equation (17) into Equation (10), the adaptive Kalman gain coefficient is obtained:

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)

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

P _k|k-1 H ^T -K _k C _k ＝0 (19)

Simultaneous formula (18) and formula (19) get:

P _k|k-1 H ^T -[P _k|k-1 H ^T (C′ _k ) ^-1 )]C _k ＝0 (20)

Simplified to get:

I-(C′ _k ) ^-1 C _k ＝0 (21)

Right now:

C _k =C' _k (22)

Substitute formula (17) into formula (22):

C _k ＝HP _k|k-1 H ^T +α _k R _k (23)

Take traces on both sides of formula (23) at the same time, and get the adaptive factor α _k :

Introduce dynamic window adjustment to Equation (24), and use the innovation error sequence defined by Equation (13) as the judgment standard for evaluating system state changes. The change of the noise of the positioning system will directly affect the modulus and change of the innovation sequence. Therefore, the distance between the innovation sequence at time k and the mean value of the innovation sequence at time k-1 is used to dynamically adjust the estimation window size N as follows:

In formula (25), N _k and N _k+1 are the window sizes at time k and k+1, is the average modulus value of the innovation sequence of the jth measurement dimension at time k-1 before, ε _k (j) is the innovation sequence of the jth measurement dimension at time k, L is the measurement dimension of the measurement vector, and [] is Round the value.

The adaptive factor introduced into the dynamic window estimation is:

Considering that in practical applications, α _k needs to be greater than or equal to 1, so:

In summary, the flowchart of the adaptive Kalman filtering method based on dynamic window adjustment is shown in Figure 2. When the motion model of the dynamic target is judged based on the heading angle is a nonlinear motion state, that is, when the rate of change of the heading angle is greater than 2°/s, the pose model of the dynamic target is relatively complicated, and it is often difficult to achieve accurate measurement by a single measurement method. estimate. The information fusion of multiple sensors through the KF algorithm can make up for the shortcomings of a single sensor, so as to achieve accurate estimation of nonlinear motion pose.

Consider again a nonlinear discrete-time dynamical system:

X _k ＝f(X _k-1 )+W _k-1 (28)

Z _k ＝h(X _k )+V _k (29)

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

When the dynamic target is in a nonlinear motion state, the Constant Turn Rate and Velocity (CTRV) model is used to obtain its real-time measurement data through GPS and UWB.

According to the kinematic analysis, using the CTRV model, the state vector of the dynamic target is generally set as:

X _k ＝[x _k y _k θ _k v _k ] ^T (31)

In formula (31), x _k and y _k are the position coordinates of the dynamic target along the X and Y axes at time k, respectively, θ _k is the heading angle of the dynamic target at time k, and v _k is the velocity of the dynamic target at time k.

Then the state transition function f( ) of the system is:

In formula (32), Δt is the time difference between time k+1 and time k, X(Δt) is the pose offset of the dynamic target after the time interval Δt, and f(·) is the dynamic pose state update equation , v(t) is the linear velocity of the dynamic target at time t, x(t) and y(t) are the position coordinates of the object to be measured along the X and Y axes at time t, respectively, They are the position coordinates of the dynamic target along the X axis, the position coordinates along the Y axis, the heading angle, and the offset of the linear velocity in the time interval Δt, and w is the angular velocity of the dynamic target.

Measurement vector Z _k ＝[x _k y _k θ _k v _k ] ^T , where the position coordinates (x, y) of the dynamic target are obtained by UWB acquisition and solution, and the heading angle θ and linear velocity v of the dynamic target are determined by the GPS message parsed to get. The simplified measurement function h is:

After establishing the state space model under the nonlinear motion state, the traditional UKF scheme introduces the filter divergence basis based on the covariance matching method, and adjusts the covariance matrix R _k of the measurement noise in real time.

The steps of the traditional UKF scheme are as follows:

(1) Initialization

In formula (34), is the estimated initial value of the dynamic target pose state, and P ₀ is the estimated initial value of the covariance matrix.

(2) Construct Sigma point set

The most widely used Sigma point sampling strategy is the centrosymmetric sampling strategy. The formula for calculating the Sigma point of the construction center sampling is as follows:

In formula (35), χ _{0, k} is the 0th Sigma point at time k, χ _{μ, k} is the μth Sigma point at k time, λ is the scaling factor, λ=α ² (n+κ)-n, n is the dimension of the dynamic target state vector at time k shown in formula (31), α(n+κ) represents the variable controlling the distance between the Sigma point and the mean value of the dynamic target state vector, represents the square root of a matrix /> The μth column of . The first-order statistical characteristic weight coefficient of the 0th Sigma point/> and the first-order statistical characteristic weight coefficient of the μth Sigma point/> And the second-order statistical characteristic weight coefficient of the μth Sigma point /> as follows:

In formula (36), β is the non-negative weight coefficient to be selected, β≥0, and Gaussian distribution generally takes 2.

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

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

From formula (37) to formula (38), is the prior estimation matrix of the state vector of the dynamic target at time k+1, P _(k+1|k) is the prior estimation of the error covariance matrix at time k+1, χ _μ,(k|k) is the prior estimation matrix at time k χ _μ,(k+1|k) is the μth Sigma point of posterior estimation at time k.

(4) Secondary use of UT transformation to obtain a new Sigma point set and measurement vector:

(5) Calculate the covariance matrix predicted by the system:

From formula (41) to formula (42), Measure the vector for time k+1 /> The autocovariance matrix of , /> is the state vector at time k+1 /> and measurement vector at time k+1 /> The cross-covariance matrix of .

(6) Calculate the Kalman gain matrix:

(7) Filter update:

Aiming at the problem that the nonlinear filtering algorithm is easily disturbed by abnormal measurement values and the typical UKF is easy to diverge, a filter divergence basis based on the covariance matching method is introduced.

When the calculated prediction error is much smaller than the real estimation error, the decision filter is in a divergent state, and the divergence condition is:

In Equation (46), ε _k is the innovation error defined in Equation (13), which represents the real 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 judgment filter is in a divergent state, that is, the difference between the estimated value and the real value The error between is too large. In practical applications, /> It is often difficult to obtain, and the covariance matrix of measured values /> can be expressed as the expected value of the sum of squared residuals, so formula (46) can be rewritten as:

In addition, in the nonlinear motion state, when the measurement error or model error is large, that is, the innovation error ε _k is large, it will cause the filter to generate a large estimation error in the measurement and correction stage. At this time, only for The adaptive update of R often cannot reduce the measurement error very well. In order to filter out abnormal measurement values, a second divergence criterion is introduced:

|ε _k |＞ξ (48)

In formula (48), ξ is the preset filtering threshold. When the modulus value of the innovation error ε _k is greater than the threshold ξ, it is considered to be an abnormal measurement value, and the measurement noise covariance matrix R is set to infinity. At this time, the Kalman gain K affected by R tends to 0, that is At this time, only state model prediction is performed, and measurement correction is not performed, thereby weakening the interference caused by abnormal measurement values to the filter. When the modulus value of the innovation error ε _k is smaller than the threshold ξ, that is, when the model error is low and the measurement accuracy is high, the state model prediction and measurement correction are carried out at the same time.

A sliding window-based R matrix adaptive calculation method is introduced for UKF. Dynamically adjust the estimated window size N as shown in Equation (25). The method of dynamically adjusting R is to recurse R _k+1 at the next moment by using multiple innovation errors ε _k at the previous moment:

The adaptive unscented Kalman filter method proposed by the present invention effectively filters out abnormal measurement values by introducing two filter divergence criteria; and based on the innovation error, the covariance matrix R _k of the measurement noise of the dynamic sliding window is designed iteration formula. The two filter divergence criteria introduced are firstly judged whether the filter is in a divergent state, and when the filter is in a divergent state, a smaller R _k is used to speed up the convergence speed of the filter and reduce its divergence degree. When the filter is in a steady state, the threshold ξ is used to judge whether the modulus of the innovation error is too large. When the error modulus exceeds the threshold ξ, it is considered to be an abnormal measurement value, and R _k+1 = 0, so that Karl The Mann gain tends to infinity, that is, the prior estimate of the state is not corrected according to the measured value. When the error modulus is less than or equal to the threshold ξ, it is considered to be a normal measurement value, and R _k+1 is recalculated through the dynamic sliding window of the innovation sequence, and the prior estimation value of the state is corrected by the measurement value to further improve the filtering precision. The flowchart of the adaptive unscented Kalman filter method based on dynamic window adjustment is shown in Fig. 3 . The specific details of the prediction module in FIG. 3 are shown in FIG. 4 , and the specific details of the measurement update module are shown in FIG. 5 .

(3) final embodiment performance analysis:

The dynamic target combination positioning method based on adaptive Kalman filter proposed by the present invention is compared with the traditional Kalman filter positioning method and the adaptive Kalman filter positioning method without sliding window. The simulation parameter settings are shown in Table 1. The average error of the simulation results is shown in Table 2.

Table 1 Simulation parameters

Table 2 Method Euclidean distance average error unit: m

low signal to noise ratio high signal to noise ratio typical method 1.100781 0.324015 Adaptive Method 1 0.768326 0.27294 The method of the invention 0.405906 0.152158

Combining Figures 6, 9, and 10, it can be seen that no matter in the case of low SNR or high SNR of the measured value, the combined positioning method proposed by the present invention is better than the typical Kalman filtering method and the adaptive Kalman without sliding window. Method 1 must better match the real trajectory, which shows that the method proposed in this paper can better suppress the error, the error value of the positioning result is smaller, and the positioning accuracy and trajectory smoothness are improved, that is, it is more accurate for dynamic target tracking. Typical methods are often highly dependent on the accurate estimation of noise statistical characteristics. When the noise variance is large, that is, the noise changes frequently, it is difficult to correct the system model through the measurement value; because it does not update the measurement noise covariance matrix, As the iteration progresses, the influence of the new measurement value in the measurement correction becomes smaller and smaller, which easily leads to filter divergence or a decrease in filtering accuracy. Although 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, regardless of the historical residual sequence, making it vulnerable to certain anomalies The influence of the measured value leads to its filtering accuracy lower than the improved adaptive positioning method proposed in this paper.

From Figures 7 to 10, it can be seen that the combined positioning method proposed by the present invention can filter and smooth the measurement values of high and low precision position coordinates, accurately model the motion state of dynamic targets through sensors, and effectively filter out abnormal measurements. Value, the measured value is corrected by the prior estimation value of the motion model, thereby effectively improving the positioning accuracy. The trajectory map obtained by the combined positioning method proposed by the present invention is obviously more in line with the real trajectory than the measured value, which proves that the The invented positioning method can suppress the impact of measurement value deviation and measurement noise characteristic change on the stability of filter noise reduction, that is, under the premise of efficient and accurate elimination of error data, the effective measurement data information can be retained to the maximum extent.

Combining Figures 11, 12 and Table 2, it can be seen that in the case of low signal-to-noise ratio of the measured value, the improved adaptive Kalman filter combined positioning method proposed by the present invention can improve the positioning accuracy by 63.13% compared with the typical Kalman filter positioning system, Compared with the adaptive Kalman filter positioning system without sliding window, the positioning accuracy is increased by 30.20%; in the case of high signal-to-noise ratio of the measured value, the improved adaptive Kalman filter combined positioning method proposed by the present invention is compared with the typical Kalman filter positioning method. The positioning accuracy of the Mann filter positioning system is increased by 53.04%. Compared with the adaptive Kalman filter positioning system without sliding window, the positioning accuracy is increased by 44.25%. That is, the filtered data processed by the improved adaptive positioning method proposed by the present invention is closer to the real position coordinates, which proves the superiority and universality of the method.

(4 Conclusion:

The invention proposes an improved self-adaptive Kalman single-point positioning method and an improved unscented Kalman combined positioning method respectively for the linear motion state and the nonlinear motion state of the dynamic target. Both calculate the dynamic sliding window size according to the innovation error and estimate the measurement noise covariance matrix in real time, which is an adaptive adjustment method based on the covariance matching method. Aiming at the problem that nonlinear filters are more likely to diverge and have lower precision, the present invention introduces two filter divergence criteria into the improved unscented Kalman combination positioning method to eliminate error data and avoid its influence on the noise reduction effect; and can By changing the threshold, the tolerance degree of the filter to the error is dynamically adjusted, which increases the universality and stability of the filter. Simulation results show that using the fusion positioning method proposed by the present invention can suppress the influence of measurement value deviation and unknown system noise characteristics on the stability of filtering and noise reduction in the case of unknown measurement noise characteristics and complex noise characteristics, and greatly improve the realization of dynamic targets. More precise positioning effect.

The above specific implementation methods and examples are specific support for the technical ideas proposed by the present invention, and cannot limit the protection scope of the present invention. Any equivalent changes made on the basis of the technical solutions of the present invention according to the technical ideas proposed by the present invention Or equivalent changes, all still belong to the scope of protection of the technical solution of the present invention.

Claims (10)

1. The dynamic target combined positioning method based on the adaptive Kalman filtering is characterized by comprising the following steps of:

step 1, judging the motion state of a course angle according to the course angle of a dynamic target, entering step 2 when the dynamic target is in a linear motion state, and entering step 3 when the dynamic target is in a nonlinear motion state;

step 2, performing single-point positioning on the dynamic target by adopting a sliding window self-adaptive Kalman filtering algorithm based on covariance matching;

and 3, fusing UWB measurement data and GPS measurement data to construct a measurement vector, establishing a state space model in a nonlinear motion state, and adopting a self-adaptive unscented Kalman filtering algorithm which introduces two filtering divergence criteria based on covariance matching to carry out combined positioning on a dynamic target.

2. The method for positioning a dynamic target combination based on adaptive kalman filtering according to claim 1, wherein the specific method for determining the motion state of the heading angle according to the heading angle of the dynamic target in step 1 is as follows: when the course angle change of the dynamic target is smaller than or equal to a threshold value, judging that the dynamic target is in a linear motion state; and when the course angle change of the dynamic target is larger than the threshold value, judging that the dynamic target is in a nonlinear motion state.

3. The method for positioning a dynamic target combination based on adaptive kalman filtering according to claim 2, wherein the specific method for performing single-point positioning on the dynamic target by adopting a sliding window adaptive kalman filtering algorithm based on covariance matching in step 2 is as follows:

step 2-1, initializing a dynamic target state vector and an error covariance matrix;

step 2-2, prior estimation of the predicted dynamic target state vector at time kPrior estimation P of predictive k-moment error covariance matrix _(k|k-1) ；

Step 2-3, obtaining an innovation error sequence epsilon at k moment based on a covariance matching method _k Covariance matrix C of (2) _k The k moment is self-adaptive to the factor alpha _k Estimating K moment Kalman gain coefficient K after introducing K moment innovation error sequence covariance matrix _k Calculating a k-moment innovation error sequence covariance matrix C meeting an optimal filter theory _k Adaptive factor alpha at time k _k According to the mean value of k moment innovation error sequence and previous k-1 moment residual error sequenceThe distance of the k+1 moment sliding window is estimated, and the k moment self-adaptive factor alpha is updated according to the estimated k moment sliding window _k ；

Step 2-4, the k-time adaptive factor alpha updated according to step 2-3 _k Step 2-3, the estimated K moment Kalman gain factor K _k And carrying out state prediction and measurement updating on the dynamic target.

4. The method for combined positioning of dynamic targets based on adaptive kalman filtering according to claim 3, wherein the specific method for combined positioning of dynamic targets by adopting an adaptive unscented kalman filtering algorithm introducing two filtering divergence criteria based on covariance matching in the step 3 is as follows:

step 3-1, initializing a dynamic target state vector and an error covariance matrix;

step 3-2, carrying out first UT conversion on the state vector prediction result of the dynamic target at the moment k, constructing a Sigma point set, and predicting the state vector of the dynamic target at the moment k+1 according to the first-order statistical characteristic weight coefficient and the second-order statistical characteristic weight coefficient of the Sigma point setk+1 time error covariance matrix P _(k+1|k) ；

Step 3-3, predicting the state vector of the dynamic target at the time k+1 in step 3-2Performing UT conversion for the second time, obtaining an updated Sigma point set, and predicting the measurement vector +.>Auto-covariance matrix of dynamic target measurement vector at time k+1>Cross covariance matrix of 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 Filtering and updating;

step 3-4, introducing two filtering divergence criteria, and updating covariance matrix R of measuring noise at k+1 moment _k+1 ：

Updating covariance matrix R of k+1 moment measurement noise when k moment innovation error sequence meets first filtering divergence criterion _k+1 Covariance matrix R for measuring noise at k time _k ，

When the k moment innovation error sequence does not meet the first filter divergence criterion but meets the second filter divergence criterion, measuring a covariance matrix R of noise at the k+1 moment _k+1 Assigning infinity and applying the Kalman gain K at time k+1 _k+1 Assigning 0, performing state prediction only on the dynamic target state vector,

estimating a covariance matrix R of the k+1 moment measurement noise based on the k moment sliding window and the previous k moment innovation error sequence when the k moment innovation error sequence meets a second filtering divergence criterion _k+1 Covariance matrix R of noise measured according to k+1 time _k+1 And carrying out state prediction and measurement correction on the dynamic target state vector.

5. The method for positioning a dynamic target combination based on adaptive Kalman filtering according to claim 4, wherein the k moment innovation error sequence covariance matrix C satisfying the optimal filter theory in the step 2-3 _k Is C _k ＝HP _k|k-1 H ^T +α _k R _k K-moment self-adaptive factor alpha meeting optimal filter theory _k Is thatWhere H is the measurement matrix and tr is the trace-taking operation.

6. An adaptive Kalman filtering-based dynamic objective according to claim 5The mark combination positioning method is characterized in that the step 2-3 predicts the expression of a k+1 moment sliding window according to the distance between the k moment innovation error sequence and the mean value of the previous k-1 moment residual error sequence as followsWherein N is _k 、N _k+1 The window size is k time and k+1 time epsilon _k (j) Innovative sequence for the j-th measurement dimension at time k,>for the average modulus of the information sequence of the jth measurement dimension at the previous k-1 moment, L is the measurement dimension of the measurement vector, [ #]The numerical value is rounded.

7. The method for combining and positioning dynamic targets based on adaptive Kalman filtering according to claim 6, wherein the steps 2-3 update the k-time adaptive factor alpha according to the estimated k-time sliding window _k The expression of (2) isWherein C is _k-j And the covariance matrix of the innovation error sequence at the moment k-j.

8. The method for combining and positioning dynamic targets based on adaptive Kalman filtering according to claim 7, wherein the first filter divergence criterion isDelta is a control coefficient, and delta is more than or equal to 1.

9. The method for dynamic target combined positioning based on adaptive Kalman filtering according to claim 8, wherein the second filter divergence criterion is

10. The method for combining and positioning dynamic targets based on adaptive Kalman filtering according to claim 9, wherein the steps 3-4 estimate covariance matrix R of measured noise at k+1 time based on k time sliding window and previous k time innovation error sequence _k+1 The expression of (2) is:

wherein ε _k-i For the k-i moment innovation error sequence, n is the dimension of the dynamic target state vector, ++>For the second order statistical characteristic weight coefficient of the mu th Sigma point, χ _μ,(k+1) Mu Sigma point at time k+1, h (x) is the measurement function.