Extended Kalman filtering self-tracking method fusing multiple snapshot data
By integrating multiple snapshot data, the extended Kalman filter method solves the problem of insufficient accuracy of traditional Kalman filtering under nonlinear conditions, realizes high-precision self-tracking target localization, and improves the robustness and accuracy of the self-tracking algorithm.
Patent Information
- Application Number
- CN202510921397.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-07-04
- Publication Date
- 2025-11-28
AI Technical Summary
Traditional Kalman filtering methods suffer from significant errors under nonlinear conditions and cannot effectively utilize multi-shot data, resulting in insufficient self-tracking accuracy.
An extended Kalman filter method that integrates multiple snapshots is adopted. By vectorizing the covariance matrix, self-tracking is performed using the received data from multiple snapshots. The motion state equation and measurement equation of the self-tracking target are established, and the target position is estimated by combining the Kalman gain update algorithm.
It achieves high-precision self-tracking in complex environments and effectively utilizes multi-shot data to improve self-tracking accuracy and robustness.
Smart Images
Figure CN121030631A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the field of self-tracking, in particular to an extended Kalman filtering self-tracking method fusing multiple snapshot data. BACKGROUND
[0002] Self-tracking technology mainly comes from the robustness requirement of autonomous navigation system and the development of multi-agent collaborative application. With the wide application of unmanned vehicles, unmanned aerial vehicles and other unmanned systems in complex scenes such as logistics transportation, military reconnaissance and disaster rescue, the traditional navigation mode relying on global positioning system is easy to fail in indoor, tunnel, urban canyon or strong electromagnetic interference environment, and cannot meet the demand of high-precision relative positioning. Therefore, the research on self-tracking technology which does not rely on global positioning system and realizes the tracking of the vehicle itself trajectory by using the signals of the radiation sources around the target, becomes a key challenge to improve the autonomy, collaboration and task reliability of unmanned systems in complex dynamic environment.
[0003] Self-tracking algorithm is usually divided into two parts: estimating the current position of itself and calculating the control variable at the next time. Many digital filters using control variables are used in target tracking field, such as Kalman filter. However, Kalman filter strictly depends on linear system model and Gaussian noise assumption, which leads to significant error or even divergence under nonlinear conditions due to model mismatch. The extended Kalman filter converts the nonlinear problem into a local linear problem by first-order Taylor expansion of the nonlinear function at the current estimated point, so that it can adapt to the nonlinear system of medium and low degree. However, the extended Kalman filter needs to use single snapshot data when applied, which cannot effectively utilize multiple snapshot data in array signal processing problem, resulting in limited performance. Therefore, the present application proposes an extended Kalman filtering self-tracking algorithm fusing multiple snapshot data, which fuses multiple snapshot receiving data to obtain a new measurement equation, so as to realize high-precision target self-tracking. SUMMARY
[0004] The technical problem to be solved by the present application is to provide an extended Kalman filtering self-tracking method fusing multiple snapshot data in view of the defects involved in the background art.
[0005] The present application adopts the following technical solutions to solve the above technical problems:
[0006] An extended Kalman filtering self-tracking method fusing multiple snapshot data, comprising the following steps:
[0007] Step 1), setting the initial state of the self-tracking target by a direct self-positioning algorithm, and initializing the initial covariance matrix of the state of the preset self-tracking target as a unit matrix;
[0008] Step 2), configuring a uniform linear array on the self-tracking target, receiving emission signals of each radiation source in the space, and calculating a covariance matrix of the received signals;
[0009] Step 3), establishing a motion state equation and a measurement equation of the self-tracking target;
[0010] Step 4), at time t, tracking is performed by the following steps:
[0011] Step 4.1), predicting a state vector of the self-tracking target by using a state transition model, and predicting a covariance matrix of the state of the self-tracking target;
[0012] Step 4.2), calculating a Kalman gain according to the predicted state vector and covariance matrix of the self-tracking target, and updating the state vector and covariance matrix of the self-tracking target by using the Kalman gain;
[0013] Step 4.3), obtaining a position of the self-tracking target at time t according to the updated state vector of the self-tracking target.
[0014] As a further optimization scheme of the fusion multi-shot data extended Kalman filtering self-tracking method, the detailed steps of step 2) are as follows:
[0015] Let there be K non-coherent narrowband radiation sources in the space, and the positions are q k =[q xk ,q yk ] T , k = 1, 2,..., K; a uniform linear array with M array elements is placed on the self-tracking target, and the uniform linear array is intercepted at time intervals T s L shot emission signals; the position of the self-tracking target at time t is u t =[x t ,y t ] T , t = 1, 2,..., T;
[0016] The signal received by the uniform linear array at time t is X t =A(u t )S t +N t t = 1, 2,..., T, wherein, represents signal data at time t, and x l,t is the received signal of the lth shot at time t; represents an array manifold, a k,t is a steering vector related to the kth source at time t, j represents an imaginary unit, d represents an array element spacing, and d = λ / 2 is taken, wherein λ represents a signal wavelength; denotes the source matrix, s l,t is the lth source vector at time t; denotes the noise matrix, n l,t is the lth noise vector at time t;
[0017] The covariance matrix of the received signal is where, denotes the covariance matrix of the source, denotes the power of the kth signal at time t, denotes the covariance matrix of the noise, denotes the noise power;
[0018] In practical applications, we usually use the sample covariance as the covariance matrix R xx,t .
[0019] As a further optimization scheme of the fusion multi-shot data extended Kalman filtering self-tracking method of the present application, the detailed steps of step 3) are as follows:
[0020] Step 3.1), the motion state of the self-tracking target at time t is described by using the state vector vx t , vy t respectively represent the velocities of the self-tracking target in the x and y directions in the Cartesian coordinate system; the motion state equation of the self-tracking target is represented as t z t-1 = Fz t , wherein F is a transition matrix, w t is a zero-mean white Gaussian process, and the covariance matrix of w respectively represent the process noise intensities of the velocities along the x and y axes;
[0021] Step 3.2), the covariance matrix R xx,t is vectorized to obtain wherein B(u t ) = A(u t ) * ⊙ A(u t ) = [b 1,t , …, b K,t ], ⊙ denotes the Khatri-rao product operation, denotes the Kronecker product operation, E M = vec(I M ); r xx,t is regarded as a single-shot data of the equivalent signal power vector p t , and the noise term Transformed into deterministic data;
[0022] Step 3.3), based on the state vector z t r xx,t Rewritten as Then r(z) t Write it in matrix form
[0023] Step 3.4), for r(z) t Applying the least squares method, we obtain In the formula, p t For signal power, For noise power, C(z) t )=[B(z t E M ];
[0024] Using predicted state Replace the real state z t Then the estimated signal power and estimated noise power It can be estimated using the following formula:
[0025] Step 3.5), r(z) t Rewrite as The measurement equation for the self-tracking target is given by v(t), which is noise generated by parameter estimation error and follows a Gaussian distribution.
[0026] As a further optimization of the extended Kalman filter self-tracking method that integrates multiple snapshot data according to the present invention, the detailed steps of step 4.1) are as follows:
[0027] The state transition model is as follows: in It is an estimate of the state at time t-1;
[0028] Update the covariance matrix of the self-tracking target according to the following formula. P t-1 Let be the covariance matrix of the state of the self-tracking target at time t-1. Let be the predicted value of the covariance matrix of the self-tracking target's state at time t.
[0029] As a further optimization of the extended Kalman filter self-tracking method that integrates multiple snapshot data according to the present invention, the detailed steps of step 4.2) are as follows:
[0030] Step 4.2.1), calculate r(z) at time t according to the following formula. t The Jacobian matrix D t|t-1 :
[0031]
[0032] in, B(z) represents t ) for z t The partial derivative of the i-th element in the equation.
[0033] Step 4.2.2), calculate the Kalman gain according to the following formula:
[0034]
[0035] in, The covariance matrix representing the measurement noise;
[0036] Step 4.2.2): Update the state vector of the self-tracking target according to the following formula. The covariance matrix P t :
[0037]
[0038] Where I4 represents a 4×4 dimensional identity matrix.
[0039] As a further optimization of the extended Kalman filter self-tracking method that integrates multiple snapshot data according to the present invention, the position of the self-tracking target at time t in step 4.3) is... in[·] i This represents the i-th element of the vector.
[0040] Compared with the prior art, the present invention, employing the above technical solution, has the following technical effects:
[0041] The method of this invention uses the extended Kalman filter to achieve target self-tracking. The algorithm effectively utilizes multi-shot received data as the measurement equation by vectorizing the covariance matrix, which enables accurate target self-tracking. Attached Figure Description
[0042] Figure 1 This is a schematic diagram of the process of the present invention;
[0043] Figure 2 This is a scene diagram illustrating the self-tracking process of the present invention.
[0044] Figure 3 This is a schematic diagram of the target self-tracking results of the present invention;
[0045] Figure 4 This is a schematic diagram comparing the tracking accuracy (RMSE) of this invention with other algorithms at different signal-to-noise ratios;
[0046] Figure 5 This is a schematic diagram comparing the tracking accuracy (RMSE) of this invention with other algorithms at different snapshot numbers. Detailed Implementation
[0047] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:
[0048] This invention can be implemented in many different forms and should not be considered limited to the embodiments described herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete, and will fully express the scope of the invention to those skilled in the art. In the drawings, components are enlarged for clarity.
[0049] Symbol notation: In this text, bold uppercase letters, bold lowercase letters, and italic letters, such as A, a, and a, represent matrices, vectors, and scalars, respectively. (·) T ,(·) H and(·) -1 E[·] denotes the operations of matrix transpose, conjugate transpose, and inversion, respectively, and E[·] denotes the operation of finding the expectation.
[0050] This invention provides an extended Kalman filter self-tracking method that fuses multiple snapshot data, such as... Figure 1 , Figure 2 As shown, the specific steps include:
[0051] Step 1) Set the initial state of the self-tracking target using the direct self-localization algorithm, and initialize the preset initial covariance matrix of the self-tracking target's state to the identity matrix;
[0052] Step 2) Configure a uniform linear array on the self-tracking target to receive the transmitted signals from each radiation source in space and calculate the covariance matrix of the received signals.
[0053] Let there be K incoherent narrowband radiation sources in space, located at positions q. k =[q xk ,q yk ] T k = 1, 2, ..., K; A uniform linear array with M elements is placed on the self-tracking target, and the uniform linear array is spaced at time intervals T. s Extract L snapshots of the transmitted signals; the position of the self-tracking target at time t is u. t =[x t ,y t ] T , t=1,2,…,T;
[0054] The signal received by the uniform linear array at time t is X. t =A(u t )S t +Nt t = 1, 2, ..., T, where, x represents the signal data at time t. l,t It is the received signal of the l-th snapshot at time t; Let a represent an array manifold. k,t It is the steering vector associated with the k-th source at time t. j represents the imaginary unit. d represents the element spacing, and we take d = λ / 2, where λ represents the signal wavelength; Let s represent the source matrix. l,t It is the l-th source vector at time t; Let n represent the noise matrix. l,t It is the l-th noise vector at time t;
[0055] The covariance matrix of the received signal is in, Represents the covariance matrix of the information source. This represents the power of the k-th signal at time t. The covariance matrix representing the noise. Indicates noise power;
[0056] In practical applications, we usually use sampling covariance. Used as the covariance matrix R xx,t ;
[0057] Step 3) Establish the motion state equation and measurement equation of the self-tracking target;
[0058] Step 3.1) Use a state vector to represent the motion state of the self-tracking target at time t. Describe, vx t vy t Let z represent the velocities of the self-tracking target in the x and y directions in Cartesian coordinates, respectively; the equation of motion of the self-tracking target is expressed as z t =Fz t-1 +w t Where F is the transition matrix, w t It is a zero-mean white Gaussian process, and its covariance matrix is... These represent the process noise intensity along the x-axis and y-axis, respectively.
[0059] Step 3.2), for the covariance matrix R xx,t Vectorization Where, B(u) t )=A(u t ) * ⊙A(u t )=[b 1,t ,…,bK,t ], ⊙ represents the Khatri-rao product operation. This represents the Kronecker product operation. E M =vec(I M );r xx,t Considered as an equivalent signal power vector p t Single snapshot data, noise item Transformed into deterministic data;
[0060] Step 3.3), based on the state vector z t r xx,t Rewritten as Then r(z) t Write it in matrix form
[0061] Step 3.4), for r(z) t Applying the least squares method, we obtain In the formula, p t For signal power, For noise power, C(z) t )=[B(z t E M ];
[0062] Using predicted state Replace the real state z t Then the estimated signal power and estimated noise power It can be estimated using the following formula:
[0063] Step 3.5), r(z) t Rewrite as The measurement equation for the self-tracking target is given by v(t), which is noise generated by parameter estimation error and follows a Gaussian distribution.
[0064] Step 4), at time t, the following steps are used for tracking:
[0065] Step 4.1) Predict the state vector of the self-tracking target using the state transition model, and predict the covariance matrix of the self-tracking target's state;
[0066] The state transition model is as follows: in It is an estimate of the state at time t-1;
[0067] Update the covariance matrix of the self-tracking target according to the following formula. P t-1Let be the covariance matrix of the self-tracking target at time t-1. Let be the predicted value of the covariance matrix of the self-tracking target's state at time t;
[0068] Step 4.2): Calculate the Kalman gain based on the predicted state vector and covariance matrix of the self-tracking target, and update the state vector and covariance matrix of the self-tracking target using the Kalman gain;
[0069] Step 4.2.1), calculate r(z) at time t according to the following formula. t The Jacobian matrix D t|t-1 :
[0070]
[0071] in, B(z) represents t ) for z t The partial derivative of the i-th element in the equation.
[0072] Step 4.2.2), calculate the Kalman gain according to the following formula:
[0073]
[0074] in, The covariance matrix representing the measurement noise;
[0075] Step 4.2.2): Update the state vector of the self-tracking target according to the following formula. The covariance matrix P t :
[0076]
[0077] Where I4 represents a 4×4 dimensional identity matrix.
[0078] Step 4.3): Obtain the position of the self-tracking target at time t based on the updated state vector of the self-tracking target. in[·] i This represents the i-th element of the vector.
[0079] The performance estimation standard of this invention is the root mean square error (RMSE), defined as follows:
[0080]
[0081] Where Mc represents the number of Monte Carlo simulation experiments; This represents the estimated position of the target at time t in the i-th Monte Carlo experiment.
[0082] Figure 3 This is a result image of target self-tracking under the conditions of a signal-to-noise ratio of 10dB, a snapshot count of 200, an array element count of 10, and a tracking time of 100s. From Figure 3 It can be seen that the algorithm can achieve accurate self-tracking.
[0083] Figure 4 This is a performance comparison chart of the method of this invention with other algorithms as the signal-to-noise ratio changes. The self-tracking algorithms compared are: 1) EKF: an extended Kalman algorithm using single-shot data; 2) EKF: an extended Kalman algorithm using equal-weighted averaging of multiple-shot data; 3) PF-MUSIC: a particle filtering algorithm using the MUSIC spectrum as particle weights. The simulation conditions are: four radiation sources are located at [(0m,0m),(500m,0m),(1000m,0m),(1500m,0m)], the number of array elements M=10, the number of shots is 100, the tracking time interval is 1s, the total tracking time is 50s, and the number of Monte Carlo simulations is 300. Figure 4 It can be seen that the method of the present invention achieves higher self-tracking accuracy under different signal-to-noise ratio conditions, and the performance is significantly improved as the signal-to-noise ratio conditions improve.
[0084] Figure 5 This is a performance comparison chart of the method of this invention with other algorithms as the number of snapshots changes. The simulation conditions were: four radiation sources located at [(0m,0m),(500m,0m),(1000m,0m),(1500m,0m)], array element number M=10, signal-to-noise ratio of 10dB, tracking time interval of 1s, total tracking time of 50s, and 300 Monte Carlo simulations. Figure 5 As can be seen, this invention achieves higher tracking accuracy and effectively utilizes the advantages of multi-shot data.
[0085] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.
[0086] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention. It should be understood that the above description is only a specific embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.
Claims
1. An extended Kalman filter self-tracking method that integrates multiple snapshot data, characterized in that, Includes the following steps: Step 1) Set the initial state of the self-tracking target using the direct self-localization algorithm, and initialize the preset initial covariance matrix of the self-tracking target's state to the identity matrix; Step 2) Configure a uniform linear array on the self-tracking target to receive the transmitted signals from each radiation source in space and calculate the covariance matrix of the received signals. Step 3) Establish the motion state equation and measurement equation of the self-tracking target; Step 4), at time t, the following steps are used for tracking: Step 4.1) Predict the state vector of the self-tracking target using the state transition model, and predict the covariance matrix of the self-tracking target; Step 4.2): Calculate the Kalman gain based on the predicted state vector and covariance matrix of the self-tracking target, and update the state vector and covariance matrix of the self-tracking target using the Kalman gain; Step 4.3) Obtain the position of the self-tracking target at time t based on the updated state vector of the self-tracking target.
2. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 1, characterized in that, The detailed steps of step 2) are as follows: Let there be K incoherent narrowband radiation sources in space, located at positions q. k =[q xk ,q yk ] T k = 1, 2, ..., K; A uniform linear array with M elements is placed on the self-tracking target, and the uniform linear array is spaced at time intervals T. s Extract L snapshots of the transmitted signals; the position of the self-tracking target at time t is u. t =[x t ,y t ] T , t=1,2,…,T; The signal received by the uniform linear array at time t is X. t =A(u t )S t +N t t = 1, 2, ..., T, where, x represents the signal data at time t. l,t It is the received signal of the l-th snapshot at time t; Let a represent an array manifold. k,t It is the steering vector associated with the k-th source at time t. j represents the imaginary unit. d represents the element spacing, and we take d = λ / 2, where λ represents the signal wavelength; Let s represent the source matrix. l,t It is the l-th source vector at time t; Let n represent the noise matrix. l,t It is the l-th noise vector at time t; The covariance matrix of the received signal is in, Represents the covariance matrix of the information source. This represents the power of the k-th signal at time t. The covariance matrix representing the noise. Indicates noise power.
3. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 2, characterized in that, Using sampling covariance Used as the covariance matrix R xx,t .
4. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 2, characterized in that, The detailed steps of step 3) are as follows: Step 3.1) Use a state vector to represent the motion state of the self-tracking target at time t. Describe, vx t vy t Let z represent the velocities of the self-tracking target in the x and y directions in Cartesian coordinates, respectively; the equation of motion of the self-tracking target is expressed as z t =Fz t-1 +w t Where F is the transition matrix, w t It is a zero-mean white Gaussian process, and its covariance matrix is... These represent the process noise intensity along the x-axis and y-axis, respectively. Step 3.2), for the covariance matrix R xx,t Vectorization Where, B(u) t )=A(u t ) * ⊙A(u t )=[b 1,t ,…,b K,t ], ⊙ represents the Khatri-rao product operation. This represents the Kronecker product operation. E M =vec(I M );r xx,t Considered as an equivalent signal power vector p t Single snapshot data, noise item Transformed into deterministic data; Step 3.3), based on the state vector z t r xx,t Rewritten as Then r(z) t Write it in matrix form Step 3.4), for r(z) t Applying the least squares method, we obtain In the formula, p t For signal power, For noise power, C(z) t )=[B(z t E M ]; Using predicted state Replace the real state z t Then the estimated signal power and estimated noise power It can be estimated using the following formula: Step 3.5), r(z) t Rewrite as The measurement equation for the self-tracking target is given by v(t), which is noise generated by parameter estimation error and follows a Gaussian distribution.
5. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 4, characterized in that, The detailed steps of step 4.1) are as follows: The state transition model is as follows: in It is an estimate of the state at time t-1; Update the covariance matrix of the self-tracking target according to the following formula. P t-1 Let be the covariance matrix of the self-tracking target at time t-1. Let be the predicted value of the covariance matrix of the self-tracking target's state at time t.
6. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 5, characterized in that, The detailed steps of step 4.2) are as follows: Step 4.2.1), calculate r(z) at time t according to the following formula. t The Jacobian matrix D t|t-1 : in, B(z) represents t ) for z t The partial derivative of the i-th element in the equation. Step 4.2.2), calculate the Kalman gain according to the following formula: in, The covariance matrix representing the measurement noise; Step 4.2.2): Update the state vector of the self-tracking target according to the following formula. The covariance matrix P t : Where I4 represents a 4×4 dimensional identity matrix.
7. The extended Kalman filter self-tracking method for fusing multiple snapshot data according to claim 6, characterized in that, In step 4.3), the position of the self-tracking target at time t is... in[·] i This represents the i-th element of the vector.