Multi-station passive positioning method and device based on TDOA direction finding and Chan-EKF fusion algorithm, and medium

Abstract

The invention provides a multi-station passive positioning method and device based on TDOA direction finding and a Chan-EKF fusion algorithm, and a medium, and belongs to the technical field of passive positioning, in order to solve the problem that in the prior art, a Chan algorithm highly depends on the distance measurement precision of a currently measured TDOA value. The method comprises the following steps: an observation station calculates a TDOA (Time Difference of Arrival) distance measurement result according to a signal arrival time difference; estimating the position coordinates of the object by using a Chan algorithm; an EKF filtering algorithm is fused to obtain an estimated position coordinate; and the observation station obtains the signal arrival time of a new round again, and positioning of the next round is carried out. The Chan-EKF provided by the invention integrates the results of Kalman filtering and Chan algorithm positioning, and solves the problem that the Chan algorithm has high dependence on the distance measurement precision of the currently measured TDOA value in the prior art.

Description

Technical Field

[0001] The present invention relates to the technical field of passive positioning, and in particular to a multi-station passive positioning method, device and medium based on TDOA direction finding and Chan-EKF fusion algorithm. Background Art

[0002] Multi-station passive positioning technology has been widely used in various fields due to its advantages such as long range, good concealment, diverse radiation source signals, and high positioning accuracy. In order to determine the location of the target, the multi-station passive positioning system needs to find the direction of the radiation source signals in different directions. In the passive positioning and direction finding technology, the direction finding technology based on the time difference of arrival (TDOA) requires fewer signal transmissions, does not rely on the absolute arrival time of the signal, and has high direction finding accuracy. It is a commonly used direction finding technology for passive positioning.

[0003] In the current positioning algorithm based on TDOA direction finding technology, the Chan algorithm is highly dependent on the ranging accuracy of the currently measured TDOA value. Once the measured TDOA value deviates, that is, the ranging accuracy cannot be guaranteed, then the positioning result calculated based on this will inevitably be inaccurate. Summary of the invention

[0004] The present invention solves the problem in the prior art that the Chan algorithm is highly dependent on the ranging accuracy of the currently measured TDOA value, and provides a multi-station passive positioning method, device and medium based on TDOA direction finding and Chan-EKF fusion algorithm.

[0005] The technical solution adopted by the present invention is:

[0006] A multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm, comprising the steps of:

[0007] Step S1, select one observation station from multiple observation stations as the reference base station, and the remaining observation stations as non-reference base stations; perform the first round of ranging on the positioning target at time t, and obtain the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t to form a vector , locate the target observation speed information , and positioning target acceleration information ;

[0008] The target observation speed information at time t Expressed as: ;

[0009] in, is the moving speed of the positioning target on the x-axis at time t;

[0010] is the moving speed of the positioning target on the y-axis at time t;

[0011] is the moving speed of the positioning target on the z-axis at time t;

[0012] Positioning target acceleration information at time t Expressed as: ;

[0013] in, is the acceleration of the positioning target on the x-axis at time t;

[0014] is the acceleration of the positioning target on the y-axis at time t;

[0015] is the acceleration of the positioning target on the z-axis at time t;

[0016] Step S2, using the vector composed of TDOA ranging values The Chan algorithm is integrated to calculate the estimated position coordinates of the Chan algorithm at time t ;

[0017] The position coordinates estimated by Chan algorithm at time t and the observed velocity information of the positioning target at time t Combined with the initial system state calculated by the EKF filter algorithm, the EKF filter algorithm calculation is initialized to obtain the posterior system state calculated by the EKF filter algorithm at time t , expressed as:

[0018] ;

[0019] Step S3, perform the next round of ranging, that is, measure the distance of the positioning target at time t+1: using the method of step S1, obtain the TDOA ranging value vector at time t+1 , t+1 time positioning target observation speed information , and the acceleration information of the positioning target at time t+1 ;

[0020] Using the method in step S2, the estimated position coordinates of the Chan algorithm at time t+1 are obtained ; The final estimated position coordinates of the positioning target are calculated by integrating the EKF filtering algorithm , which is the result of this passive positioning method, and output;

[0021] Update the posterior covariance of the EKF filter algorithm , prepare for the next distance measurement of the positioning target; step S4, repeat step S3 until the preset number of iterations is reached.

[0022] Furthermore, step S1 specifically performs the following steps:

[0023] Step S11, at time t, the first round of ranging is performed on the positioning target: one observation station is selected from multiple observation stations as the reference base station, and the position coordinates of the reference base station are marked as vector , the i-th non-reference base station position coordinates are marked as vector ;

[0024] Step S12, assuming the position coordinates of the positioning target at time t to be vector , the propagation speed of the radiation source signal is C, and the position coordinates of the positioning target at time t are , and the radiation source signal propagation speed C are substituted into the formula: ;

[0025] Calculate the first round, that is, the non-reference base station position coordinates at time t Coordinates of the base station location TDOA distance measurement value between ;

[0026] in, for The second norm of ;

[0027] for The second norm of ;

[0028] is the position coordinate of the non-reference base station at time t Coordinates of the base station location The arrival time difference of the radiation source signals between them;

[0029] Step S13, using step S12 to calculate the TDOA distance value at time t The TDOA distance measurement values ​​between n non-reference base stations and the reference base station at time t are calculated by the formula and form a vector , expressed as: ;

[0030] Calculate the TDOA ranging value at time t sum , expressed as: ;

[0031] The observed speed information of the positioning target at time t is obtained through the speedometer and accelerometer of the positioning target , expressed as: ;

[0032] in, is the moving speed of the positioning target on the x-axis at time t;

[0033] is the moving speed of the positioning target on the y-axis at time t;

[0034] is the moving speed of the positioning target on the z-axis at time t;

[0035] And the acceleration information of the positioning target at time t , expressed as: ;

[0036] in, is the acceleration of the positioning target on the x-axis at time t;

[0037] is the acceleration of the positioning target on the y-axis at time t;

[0038] is the acceleration of the positioning target on the z-axis at time t.

[0039] Furthermore, step S2 specifically performs the following steps:

[0040] Step S21, in executing the Chan algorithm, a two-step weighted least squares method is used to calculate the initial solution result of the positioning target at time t , expressed as: ;

[0041] Arbitrarily remove the preliminary solution results at time t Take one of the solutions and take the remaining solution as the estimated position coordinates of the positioning target at time t , expressed as: ;

[0042] Use the estimated position coordinates of the positioning target at time t The expression calculates 8 estimated position coordinates at time t, which are expressed as:

[0043] ;

[0044] ;

[0045] ;

[0046] ;

[0047] ;

[0048] ;

[0049] ;

[0050] ;

[0051] Step S22: Based on the estimated position coordinates of the i-th target to be located at time t , where 0 < i ≤ 8, obtain the distance between the position coordinates of the j-th non-reference base station at time t and the distance to the position coordinates of the reference base station , where 0 < j ≤ n. That is, the TDOA ranging value of the radiation source signal of at the position coordinates of the j-th non-reference base station and the position coordinates of the reference base station is , expressed as: ;

[0052] Step S23: Calculate the sum of all nat time t , expressed as: ;

[0053] Step S24: Combine the 8 at time t to form a vector , expressed as: ;

[0054] Step S25: Calculate the error between and at time t , expressed as:

[0055] Among them,

[0056] is the absolute value of;

[0057] Select the estimated coordinate corresponding to the value with the smallest error from as the estimated position coordinate at time t obtained by the Chan algorithm ;

[0058] Combine the estimated position coordinate at time t obtained by the Chan algorithm and the observed velocity information of the target to be located at time t as the initial system state for the EKF filtering algorithm calculation, initialize the EKF filtering algorithm calculation, and obtain the posterior system state calculated by the EKF filtering algorithm at time t , expressed as:

[0059] .

[0060] Furthermore, step S3 specifically performs the following steps:

[0061] Step S31, at time t+1, the next round of ranging is performed on the positioning target, that is, ranging at time t+1: using the calculation process from step S11 to step S13, the vector composed of the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t+1 is calculated , positioning target speed information , acceleration information ;

[0062] Among them, the vector composed of the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t+1 is Expressed as: ;

[0063] Expressed as: ;

[0064] in, The moving speed of the positioning target on the x-axis at time t+1;

[0065] The moving speed of the positioning target on the y-axis at time t+1;

[0066] The moving speed of the positioning target on the z-axis at time t+1;

[0067] Using the calculation process from step S21 to step S24, the Chan algorithm estimated position coordinates at time t+1 are calculated , expressed as: ;

[0068] in, The Chan algorithm estimates the position coordinates of the target on the x-axis at time t+1;

[0069] The Chan algorithm estimates the position coordinates of the target on the y-axis at time t+1;

[0070] The Chan algorithm estimates the position coordinates of the target on the z-axis at time t+1;

[0071] Estimate the position coordinates according to the Chan algorithm at time t+1 , t+1 time positioning target observation speed information , and the acceleration information of the positioning target at time t+1 The state space equation of the EKF filtering algorithm is constructed as follows: ;

[0072] in, is the state vector in the state space equation at time t+1, including the position coordinates estimated by the Chan algorithm at time t+1 , and the observed speed information of the positioning target at time t+1 ;

[0073] At time t+1, the mean is 0 and the variance is , the process noise vector in the state space equation obeys the normal distribution;

[0074] is the state transfer matrix in the state space equation;

[0075] is the control input matrix in the state space equation;

[0076] is the control input vector in the state space equation at time t+1;

[0077] Step S32, taking all TDOA ranging values ​​at time t+1 as the observation input of the positioning system, constructing the system observation equation in the state space equation, expressed as: ;

[0078] in, is the observation vector in the state space equation at time t+1;

[0079] is the observation function in the state space equation;

[0080] is the observation noise vector in the state space equation with a mean of 0, a variance of R, and a normal distribution at time t+1;

[0081] Step S33: transform the observation vector in the state space equation at time t+1 In Substituting into the formula: ;

[0082] Calculate the observation matrix of the EKF filtering algorithm at time t+1 ;

[0083] in, , and They are the prior position coordinates of the x-axis, y-axis and z-axis calculated by the EKF filtering algorithm at time t+1;

[0084] The observation matrix of the EKF filtering algorithm at time t+1 Columns 1 to 3 are partial derivatives of the prior position coordinates at time t;

[0085] Step S34: convert the state vector in the state space equation at time t+1 , the state transfer matrix in the state space equation , the control input matrix in the state space equation , the control input vector in the state space equation at time t+1 Substituting into the formula: ;

[0086] Predict the system prior state of the EKF filter algorithm at time t for time t+1 ;

[0087] in, is the prediction of the system state at time t, that is, the a posteriori system state calculated by the EKF filtering algorithm at time t; the state transfer matrix in the state space equation ,variance Substituting into the formula: ;

[0088] Predict the prior covariance of the EKF filter algorithm at time t for time t+1 ;

[0089] in, is the predicted covariance of the system state at time t;

[0090] The state vector in the state space equation at time t+1 is ,variance , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filter algorithm at time t to time t+1 Substituting into the formula: ;

[0091] Update the Kalman gain of the EKF filter algorithm at time t+1 ;

[0092] The system prior state of the EKF filtering algorithm at time t is calculated based on the time t+1. , Kalman gain of the EKF filter algorithm at time t+1 , the observation matrix of the EKF filtering algorithm at time t+1 , the vector composed of the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t+1 Substituting into the formula: ;

[0093] Update the system posterior state of the EKF filtering algorithm ;

[0094] Step S35: The position coordinates estimated by the Chan algorithm at time t+1 Substituting into the formula: ;

[0095] Calculate the final estimated position coordinates of the positioning target , and output;

[0096] in, Estimate the position coordinates of the positioning target at the current moment;

[0097] Step S36: The Kalman gain of the EKF filter algorithm at time t+1 is , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filter algorithm at time t to time t+1 Substituting into the formula: ;

[0098] Update the posterior covariance of the EKF filter algorithm , prepare for the next distance measurement of the positioning target;

[0099] in is the identity matrix.

[0100] Furthermore, the state transfer matrix in the state space equation is The expression is: ;

[0101] in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1.

[0102] Furthermore, the control input matrix in the state space equation is The expression is: ;

[0103] in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1.

[0104] Furthermore, the control input vector in the state space equation at time t+1 is Expressed as: ;

[0105] in, is the acceleration of the positioning target on the x-axis at time t+1;

[0106] is the acceleration of the positioning target on the y-axis at time t+1;

[0107] is the acceleration of the positioning target on the z-axis at time t+1;

[0108] Furthermore, the state vector in the state space equation at time t+1 is The estimated position coordinates of the target at time t+1 And the speed information of the positioning target at time t+1 The expression of the form is: .

[0109] Based on the same inventive concept, the present invention also provides a computer device, including: a memory and a processor, the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the aforementioned multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm by executing the computer instructions.

[0110] Based on the same inventive concept, the present invention also provides a computer-readable storage medium, on which computer instructions are stored, and the computer instructions are used to enable a computer to execute the aforementioned multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm.

[0111] The beneficial effects of the present invention are:

[0112] The present invention combines the positioning results of Kalman filtering and Chan algorithm. Kalman filtering infers the current positioning result based on the past positioning results and the current TDOA value, that is, the filtering algorithm uses the information of the past time. If the current TDOA is not accurately measured, it will not produce a particularly large error like the Chan algorithm. Therefore, Chan-EKF solves the problem in the prior art that the Chan algorithm is highly dependent on the ranging accuracy of the currently measured TDOA value.

[0113] The present invention makes comprehensive use of the positioning results obtained by Kalman filtering and Chan algorithm. When the positioning error generated by Kalman filtering is at a large amplitude, Chan-EKF can correct the error to a certain extent, making the positioning result more accurate; similarly, if the positioning result obtained by Chan algorithm shows a large error, Chan-EKF can pull back some of the deviation and optimize the positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0114] In order to more clearly illustrate the technical solutions in the present application or the prior art, the drawings required for use in the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0115] Attached Figure 1 This is a flow chart of the method of this embodiment.

[0116] Attached Figure 2 The figure is a comparison chart of the local positioning trajectory of the method in this embodiment, the Chan algorithm and the EKF algorithm.

[0117] Attached Figure 3 This is a time domain comparison diagram of the positioning error between the method of this embodiment and the Chan algorithm and the EKF algorithm.

[0118] Attached Figure 4 This is a comparison chart of the cumulative distribution curves of positioning errors of the method in this embodiment, the Chan algorithm, and the EKF algorithm. DETAILED DESCRIPTION

[0119] Combined with Figure 1 The method flow chart of this embodiment describes this embodiment in detail.

[0120] The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm disclosed in this embodiment includes the following steps:

[0121] Step S1, select one observation station from multiple observation stations as the reference base station, and the remaining observation stations as non-reference base stations; perform the first round of ranging on the positioning target at time t, and obtain the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t to form a vector , locate the target observation speed information , and positioning target acceleration information ;

[0122] The target observation speed information at time t Expressed as: ;

[0123] in, is the moving speed of the positioning target on the x-axis at time t;

[0124] is the moving speed of the positioning target on the y-axis at time t;

[0125] is the moving speed of the positioning target on the z-axis at time t;

[0126] Positioning target acceleration information at time t Expressed as: ;

[0127] in, is the acceleration of the positioning target on the x-axis at time t;

[0128] is the acceleration of the positioning target on the y-axis at time t;

[0129] is the acceleration of the positioning target on the z-axis at time t;

[0130] Step S2, using the vector composed of TDOA ranging values The Chan algorithm is integrated to calculate the estimated position coordinates of the Chan algorithm at time t ;

[0131] The position coordinates estimated by Chan algorithm at time t and the observed velocity information of the positioning target at time t Combined with the initial system state calculated by the EKF filter algorithm, the EKF filter algorithm calculation is initialized to obtain the posterior system state calculated by the EKF filter algorithm at time t , expressed as: ;

[0132] Step S3, perform the next round of ranging, that is, measure the distance of the positioning target at time t+1: using the method of step S1, obtain the TDOA ranging value vector at time t+1 , t+1 time positioning target observation speed information , and the acceleration information of the positioning target at time t+1 ;

[0133] Using the method in step S2, the estimated position coordinates of the Chan algorithm at time t+1 are obtained ; The final estimated position coordinates of the positioning target are calculated by integrating the EKF filtering algorithm , i.e., the result of the passive positioning method of this embodiment, and output;

[0134] Update the posterior covariance of the EKF filter algorithm , prepare for the next distance measurement of the positioning target;

[0135] Step S4, repeating step S3 until a preset number of iterations is reached.

[0136] Furthermore, step S1 specifically performs the following steps:

[0137] Step S11, at time t, the first round of ranging is performed on the positioning target: one observation station is selected from multiple observation stations as the reference base station, and the position coordinates of the reference base station are marked as vector , the i-th non-reference base station position coordinates are marked as vector ;

[0138] Step S12, assuming the position coordinates of the positioning target at time t to be vector , the propagation speed of the radiation source signal is C, and the position coordinates of the positioning target at time t are , and the radiation source signal propagation speed C are substituted into the formula: ;

[0139] Calculate the first round, that is, the non-reference base station position coordinates at time t Coordinates of the base station location TDOA distance measurement value between ;

[0140] in, for The second norm of ;

[0141] for The second norm of ;

[0142] is the position coordinate of the non-reference base station at time t Coordinates of the base station location The arrival time difference of the radiation source signals between them;

[0143] Step S13, using step S12 to calculate the TDOA distance value at time t The TDOA distance measurement values ​​between n non-reference base stations and the reference base station at time t are calculated by the formula and form a vector , expressed as: ;

[0144] Calculate the TDOA ranging value at time t sum , expressed as: ;

[0145] The observed speed information of the positioning target at time t is obtained through the speedometer and accelerometer of the positioning target , expressed as: ;

[0146] in, is the moving speed of the positioning target on the x-axis at time t;

[0147] is the moving speed of the positioning target on the y-axis at time t;

[0148] is the moving speed of the positioning target on the z-axis at time t;

[0149] And the acceleration information of the positioning target at time t , expressed as: ;

[0150] in, is the acceleration of the positioning target on the x-axis at time t;

[0151] is the acceleration of the positioning target on the y-axis at time t;

[0152] is the acceleration of the positioning target on the z-axis at time t.

[0153] Furthermore, step S2 is specifically executed as follows:

[0154] Step S21, in the execution of the Chan algorithm, the preliminary solution result of the positioning target at time t is calculated using the two-step weighted least squares method , expressed as: ;

[0155] Adding the solution result to the position coordinates of the reference base station can obtain the final position estimation result of the Chan algorithm. The above formula shows that the Chan algorithm has two solutions, and one of them must be a false solution. Arbitrarily remove one of the solutions in the preliminary solution result at time t and take the remaining solution as the estimated position coordinates of the positioning target at time t , expressed as: ;

[0156] Using the estimated position coordinates expression of the positioning target at time t, eight estimated position coordinates at time t are calculated, and are respectively expressed as:

[0157] ;

[0158] ;

[0159] ;

[0160] ;

[0161] ;

[0162] ;

[0163] ;

[0164] ;

[0165] Step S22, according to the i-th estimated position coordinates of the positioning target at time t, 0 < i ≤ 8, obtain the position coordinates from the j-th non-reference base station at time t The distance to and the coordinates of the reference base station The difference between the distances, where 0 < j ≤ n, that is The radiation source signal at the j-th non-reference base station location coordinates And the reference base station location coordinates The TDOA ranging value between them Expressed as: ;

[0166] Step S23, calculate the sum of all n At time t Expressed as: ;

[0167] Step S24, form a vector from the 8 At time t Expressed as: ;

[0168] Step S25, calculate the error Between And At time t ;

[0169] Among them, Is The absolute value of; if the estimated position Is closer to the actual position of the object Then The error between And

[0170] Select The value corresponding to the smallest error between And As the estimated coordinate obtained by the Chan algorithm at time t, which is the estimated position coordinate of the Chan algorithm

[0171] Combine the estimated position coordinate of the Chan algorithm at time t And the observed velocity information of the positioning target at time t As the initial system state for the EKF filtering algorithm calculation, initialize the EKF filtering algorithm calculation, and obtain the posterior system state calculated by the EKF filtering algorithm at time t Expressed as: ;

[0172] Furthermore, step S3 specifically performs the following steps:

[0173] Step S31, at time t+1, the next round of ranging is performed on the positioning target, that is, ranging at time t+1: using the calculation process from step S11 to step S13, the vector composed of the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t+1 is calculated , positioning target speed information , acceleration information ;

[0174] Among them, the vector composed of the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t+1 is Expressed as: ;

[0175] Expressed as: ;

[0176] in, The moving speed of the positioning target on the x-axis at time t+1;

[0177] The moving speed of the positioning target on the y-axis at time t+1;

[0178] The moving speed of the positioning target on the z-axis at time t+1;

[0179] Using the calculation process from step S21 to step S24, the Chan algorithm estimated position coordinates at time t+1 are calculated , expressed as: ;

[0180] in, The Chan algorithm estimates the position coordinates of the target on the x-axis at time t+1;

[0181] The Chan algorithm estimates the position coordinates of the target on the y-axis at time t+1;

[0182] The Chan algorithm estimates the position coordinates of the target on the z-axis at time t+1;

[0183] Estimate the position coordinates according to the Chan algorithm at time t+1 , t+1 time positioning target observation speed information , and the acceleration information of the positioning target at time t+1 The state space equation of the EKF filtering algorithm is constructed as follows: ;

[0184] in, is the state vector in the state space equation at time t+1, including the position coordinates estimated by the Chan algorithm at time t+1 , and the observed speed information of the positioning target at time t+1 ;

[0185] At time t+1, the mean is 0 and the variance is , the process noise vector in the state space equation obeys the normal distribution;

[0186] is the state transfer matrix in the state space equation;

[0187] is the control input matrix in the state space equation;

[0188] is the control input vector in the state space equation at time t+1;

[0189] Step S32, taking all TDOA ranging values ​​at time t+1 as the observation input of the positioning system, constructing the system observation equation in the state space equation, expressed as: ;

[0190] in, is the observation vector in the state space equation at time t+1;

[0191] is the observation function in the state space equation;

[0192] It is the observation noise vector in the state space equation with a mean of 0, a variance of R, and a normal distribution at time t+1. It is determined by the quality of the sensor at the observation station. A small variance indicates that the sensor has good performance and high ranging accuracy. Otherwise, it indicates that the sensor quality is poor and the ranging accuracy is low.

[0193] Step S33: transform the observation vector in the state space equation at time t+1 In Substituting into the formula: ;

[0194] Calculate the observation matrix of the EKF filtering algorithm at time t+1 ;

[0195] in, , and They are the prior position coordinates of the x-axis, y-axis and z-axis calculated by the EKF filtering algorithm at time t+1;

[0196] Observation Matrix middle express right Find partial derivatives; observation matrix Other items are similar;

[0197] The observation matrix of the EKF filtering algorithm at time t+1 Columns 1 to 3 are partial derivatives of the prior position coordinates at time t;

[0198] in, The expression of the partial derivative of the x-axis coordinate is: , ;

[0199] The calculation method of the partial derivative of the y-axis coordinate and the z-axis coordinate and The same is true for the x-axis coordinates.

[0200] Step S34: convert the state vector in the state space equation at time t+1 , the state transfer matrix in the state space equation , the control input matrix in the state space equation , the control input vector in the state space equation at time t+1 Substituting into the formula: ;

[0201] Predict the system prior state of the EKF filter algorithm at time t for time t+1 ;

[0202] in, is the prediction of the system state at time t, that is, the a posteriori system state calculated by the EKF filtering algorithm at time t; the state transfer matrix in the state space equation ,variance Substituting into the formula: ;

[0203] Predict the prior covariance of the EKF filter algorithm at time t for time t+1 ;

[0204] in, is the predicted covariance of the system state at time t;

[0205] The state vector in the state space equation at time t+1 is ,variance , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filter algorithm at time t to time t+1 Substituting into the formula: ;

[0206] Update the Kalman gain of the EKF filter algorithm at time t+1 ;

[0207] The system prior state of the EKF filtering algorithm at time t is calculated for time t+1. , Kalman gain of the EKF filter algorithm at time t+1 , the observation matrix of the EKF filtering algorithm at time t+1 , the vector composed of the TDOA ranging values ​​between n non-reference base stations and the reference base station at time t+1 Substituting into the formula: ;

[0208] Update the system posterior state of the EKF filtering algorithm ;

[0209] Step S35: The position coordinates estimated by the Chan algorithm at time t+1 Substituting into the formula: ;

[0210] Calculate the final estimated position coordinates of the positioning target , and output;

[0211] From the attached Figure 2 It can be seen from the figure that the proposed scheme has better prediction accuracy than the Chan algorithm and the EKF algorithm. Figure 3 It can be seen from the figure that the method of this embodiment has a smaller prediction error than the Chan algorithm and the EKF algorithm. Figure 4 It can be seen that the method of this embodiment has a smaller prediction error accumulation compared with the Chan algorithm and the EKF algorithm.

[0212] in, Estimate the position coordinates of the positioning target at the current moment;

[0213] Step S36: The Kalman gain of the EKF filter algorithm at time t+1 is , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filter algorithm at time t to time t+1 Substituting into the formula: ;

[0214] Update the posterior covariance of the EKF filter algorithm , to prepare for the next distance measurement of the positioning target; is the identity matrix.

[0215] Furthermore, the state transfer matrix in the state space equation is The expression is: ;

[0216] in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1, that is, the time interval between time t and time t+1 It is 0.03125 seconds.

[0217] Furthermore, the control input matrix in the state space equation is The expression is: ;

[0218] in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1.

[0219] Furthermore, the control input vector in the state space equation at time t+1 is Expressed as: ;

[0220] in, is the acceleration of the positioning target on the x-axis at time t+1;

[0221] is the acceleration of the positioning target on the y-axis at time t+1;

[0222] is the acceleration of the positioning target on the z-axis at time t+1;

[0223] Furthermore, the state vector in the state space equation at time t+1 is The estimated position coordinates of the target at time t+1 And the speed information of the positioning target at time t+1 The expression of the form is: .

[0224] Based on the same inventive concept, this embodiment also provides a computer device, including: a memory and a processor, the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the aforementioned multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm by executing the computer instructions.

[0225] Based on the same inventive concept, this embodiment also provides a computer-readable storage medium, on which computer instructions are stored, and the computer instructions are used to enable the computer to execute the aforementioned multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm.

[0226] Special note: When the positioning algorithm performs the first round of positioning, the EKF algorithm cannot complete initialization because there is no initial system state. At this time, only the Chan algorithm is used for positioning, and then the first round of positioning results are used as the initial system state of the EKF algorithm. The positioning algorithm proposed in the present invention is run starting from the second round of positioning.

[0227] This embodiment brings the following beneficial effects:

[0228] The Chan algorithm is highly dependent on the ranging accuracy of the currently measured TDOA value. Once the measured TDOA value deviates, that is, the ranging accuracy cannot be guaranteed, then the positioning result calculated based on this will inevitably be inaccurate. This embodiment combines the positioning results of the Kalman filter and the Chan algorithm. The Kalman filter infers the current positioning result based on the past positioning results and the current TDOA value, that is, the filtering algorithm uses the information of the past time. If the current TDOA is not accurate, it will not produce a particularly large error like the Chan algorithm. Therefore, Chan-EKF solves the problem of the Chan algorithm in the prior art being highly dependent on the ranging accuracy of the currently measured TDOA value.

[0229] In the process of using the Chan algorithm for positioning calculations, there are pseudo solutions in the calculation results. When processing the results output by the Chan algorithm, if one of the results is randomly eliminated, based on the characteristics of the Chan algorithm, 8 potentially correct positioning results can be derived from the remaining result. After that, these 8 potentially correct positioning results need to be identified and eliminated one by one in a specific order to determine the final accurate positioning result. It is worth noting that no matter which result output by the Chan algorithm is eliminated in the initial stage, the set of 8 potentially correct positioning results derived subsequently must include the previously eliminated solution. This feature has a key reference significance in the process of using the Chan algorithm for high-precision positioning and result screening, and provides important clues for obtaining subsequent accurate positioning results.

[0230] The extended Kalman filter (EKF), from the perspective of its inherent mathematical principles, naturally has an inherent defect of accumulated errors. The divergence problem of the Kalman filter is the original drawback of the filtering algorithm. The root cause is that although the positioning results of the past moments have a certain reference significance for the calculations at the current moment, it cannot be ignored that the positioning results of the past moments themselves inevitably carry error components. In this way, as the Kalman filter algorithm continues to run, these errors accumulate one by one, causing the cumulative error to continue to increase over time. If you rely solely on the extended Kalman filter (EKF) to perform positioning tasks, when the algorithm runs for a certain period of time, the goal of accurate positioning will not be achieved due to the rapid expansion of the accumulated error. This phenomenon has been strongly confirmed in the simulation experiment results, as shown in the attached figure. Figure 4 It is clearly shown that after the EKF algorithm runs for 10 seconds, its positioning error shows a completely divergent trend, and the positioning accuracy is seriously out of control. In comparison, in this dilemma where the Kalman filter diverges due to accumulated errors, the Chan algorithm can play a unique role and effectively suppress and correct the EKF error. Specifically, after the Chan algorithm intervenes, it can pull back the excessive error generated by the EKF, so that when the positioning operation is performed at the next moment, the past positioning results that the Kalman filter relies on for reference have a relatively small error value, thereby significantly improving the problem of Kalman filter divergence and out of control, and improving the overall positioning accuracy and stability.

[0231] The above are only specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. For any technician familiar with the technical field, any changes or substitutions that can be easily thought of within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm, characterized in that: Includes steps: Step S1, selecting one observation station from multiple observation stations as a reference base station, and the remaining observation stations as non-reference base stations; The first round of ranging is performed on the positioning target at time t, and the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t are obtained to form a vector , locate the target observation speed information , and positioning target acceleration information ; The target observation speed information at time t Expressed as: ； in, is the moving speed of the positioning target on the x-axis at time t; is the moving speed of the positioning target on the y-axis at time t; is the moving speed of the positioning target on the z-axis at time t; The acceleration information of the positioning target at time t Expressed as: ； in, is the acceleration of the positioning target on the x-axis at time t; is the acceleration of the positioning target on the y-axis at time t; is the acceleration of the positioning target on the z-axis at time t; Step S2, using the vector composed of TDOA ranging values The Chan algorithm is integrated to calculate the estimated position coordinates of the Chan algorithm at time t ; The position coordinates estimated by the Chan algorithm at time t and the observed velocity information of the positioning target at time t Combined with the initial system state calculated by the EKF filter algorithm, the EKF filter algorithm calculation is initialized to obtain the posterior system state calculated by the EKF filter algorithm at time t , expressed as: ； Step S3, perform the next round of ranging, that is, measure the distance of the positioning target at time t+1: using the method of step S1, obtain the TDOA ranging value vector at time t+1 , t+1 time positioning target observation speed information , and the acceleration information of the positioning target at time t+1 ; Using the method in step S2, the position coordinates estimated by the Chan algorithm at time t+1 are obtained. ; The EKF filtering algorithm is integrated to calculate the final estimated position coordinates of the positioning target , which is the result of the passive positioning method, and output; Update the posterior covariance of the EKF filter algorithm , to prepare for the next distance measurement of the positioning target; Step S4, repeating step S3 until a preset number of iterations is reached.

2. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 1 is characterized in that: The step S1 specifically performs the following steps: Step S11, at time t, the first round of ranging is performed on the positioning target: one observation station is selected from multiple observation stations as the reference base station, and the position coordinates of the reference base station are marked as vector , the position coordinates of the i-th non-reference base station are marked as vector ; Step S12, assuming the position coordinates of the positioning target at time t to be vector , the propagation speed of the radiation source signal is C, and the position coordinates of the positioning target at time t are , and the radiation source signal propagation speed C are substituted into the formula: ； Calculate the first round, that is, the non-reference base station position coordinates at time t The reference base station position coordinates TDOA distance measurement value between ; in, for The second norm of ; for The second norm of ; is the position coordinate of the non-reference base station at time t The reference base station position coordinates The arrival time difference of the radiation source signals between them; Step S13, first, using the TDOA ranging value at time t in step S12 The TDOA distance measurement values ​​between the n non-reference base stations and the reference base station at time t are calculated by the formula and constituted into a vector , expressed as: ； Calculate the TDOA ranging value at time t sum , expressed as: ； The observed speed information of the positioning target at time t is obtained by using the speedometer and accelerometer of the positioning target. , expressed as: ； in, is the moving speed of the positioning target on the x-axis at time t; is the moving speed of the positioning target on the y-axis at time t; is the moving speed of the positioning target on the z-axis at time t; And the acceleration information of the positioning target at time t , expressed as: ； in, is the acceleration of the positioning target on the x-axis at time t; is the acceleration of the positioning target on the y-axis at time t; is the acceleration of the positioning target on the z-axis at time t.

3. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 1 is characterized in that: The step S2 specifically performs the following steps: Step S21, in executing the Chan algorithm, a two-step weighted least squares method is used to calculate the preliminary solution result of the positioning target at time t , expressed as: ； Arbitrarily remove the preliminary solution results at time t Take one of the solutions and take the remaining solution as the estimated position coordinates of the positioning target at time t , expressed as: ； Use the estimated position coordinates of the positioning target at time t The expression is calculated to obtain the 8 estimated position coordinates at time t, which are expressed as: ； ； ； ； ； ； ； ； Step S22, according to the estimated position coordinates of the i-th positioning target at time t , where 0 < i ≤ 8, obtain the distance from the position coordinate of the to the j-th non-reference base station position coordinate and the difference from the distance to the reference base station position coordinate , where 0 < j ≤ n, that is, the TDOA ranging value of the radiation source signal of the at the j-th non-reference base station position coordinate and the reference base station position coordinate , expressed as: ​ ； Step S23, calculate all the n said sum , expressed as: ； Step S24, the eight Composition vector , expressed as: ; Step S25, calculate the With the The error between , expressed as: ； in, for The absolute value of Select the In and described The estimated coordinates corresponding to the value with the smallest error are used as the Chan algorithm to calculate the Chan algorithm estimated position coordinates at time t ; The position coordinates estimated by the Chan algorithm at time t and the observed speed information of the positioning target at time t Combined with the initial system state calculated by the EKF filter algorithm, the EKF filter algorithm calculation is initialized to obtain the posterior system state calculated by the EKF filter algorithm at the time t , expressed as: 。 4. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 3 is characterized in that: The step S3 specifically performs the following steps: Step S31, performing the next round of ranging for the positioning target at time t+1, that is, ranging at time t+1: using the calculation process of steps S11 to S13, calculate and obtain a vector composed of the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t+1 , positioning target speed information , acceleration information ; Among them, the vector composed of the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t+1 is Expressed as: ; Said Expressed as: ； in, The moving speed of the positioning target on the x-axis at time t+1; The moving speed of the positioning target on the y-axis at time t+1; The moving speed of the positioning target on the z-axis at time t+1; Then, the Chan algorithm estimated position coordinates at time t+1 are calculated using the calculation process from step S21 to step S24. , expressed as: ； in, The Chan algorithm estimates the position coordinates of the target on the x-axis at time t+1; The Chan algorithm estimates the position coordinates of the target on the y-axis at time t+1; The Chan algorithm estimates the position coordinates of the target on the z-axis at time t+1; Estimate the position coordinates according to the Chan algorithm at time t+1 , the observed speed information of the positioning target at time t+1 , and the acceleration information of the positioning target at time t+1 The state space equation of the EKF filtering algorithm is constructed as follows: ； in, is the state vector in the state space equation at time t+1, including the position coordinates estimated by the Chan algorithm at time t+1 , and the observed speed information of the positioning target at time t+1 ; At time t+1, the mean is 0 and the variance is , the process noise vector in the state space equation obeys the normal distribution; is the state transfer matrix in the state space equation; is the control input matrix in the state space equation; is the control input vector in the state space equation at time t+1; Step S32, taking all TDOA ranging values ​​at time t+1 as the observation input of the positioning system, constructing the system observation equation in the state space equation, expressed as: ； Among them, the is the observation vector in the state space equation at time t+1; is the observation function in the state space equation; is the observation noise vector in the state space equation with a mean of 0, a variance of R, and a normal distribution at time t+1; Step S33: transform the observation vector in the state space equation at time t+1 into In Substitute into the formula ； Calculate the observation matrix of the EKF filtering algorithm at time t+1 ; in, , and They are the prior position coordinates of the x-axis, y-axis and z-axis calculated by the EKF filtering algorithm at time t+1; The observation matrix of the EKF filtering algorithm at time t+1 is Columns 1 to 3 are partial derivatives of the prior position coordinates at time t; Step S34, convert the state vector in the state space equation at time t+1 , the state transfer matrix in the state space equation , the control input matrix in the state space equation , the control input vector in the state space equation at time t+1 Substitute into the formula ； Predict the system prior state of the EKF filter algorithm at time t for time t+1 ; in, is the prediction of the system state at time t, that is, the a posteriori system state calculated by the EKF filtering algorithm at time t; the state transfer matrix in the state space equation is , the variance Substitute into the formula ； Predict the prior covariance of the EKF filter algorithm at time t for time t+1 ; in, is the predicted covariance of the system state at time t; The state vector in the state space equation at time t+1 is , the variance , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filtering algorithm at time t to time t+1 Substitute into the formula ； Update the Kalman gain of the EKF filter algorithm at time t+1 ; The system prior state of the EKF filtering algorithm at time t to time t+1 , the Kalman gain of the EKF filtering algorithm at time t+1 , the observation matrix of the EKF filtering algorithm at time t+1 , a vector composed of the TDOA ranging values ​​between the n non-reference base stations and the reference base station at time t+1 Substitute into the formula ; Update the system posterior state of the EKF filtering algorithm ; Step S35: the position coordinates estimated by the Chan algorithm at time t+1 Substitute into the formula ； Calculate the final estimated position coordinates of the positioning target , and output; in, Estimate the position coordinates of the positioning target at the current moment; Step S36: The Kalman gain of the EKF filtering algorithm at time t+1 is , the observation matrix of the EKF filtering algorithm at time t+1 , the prior covariance of the EKF filtering algorithm at time t to time t+1 Substitute into the formula ； Update the posterior covariance of the EKF filter algorithm , to prepare for the next distance measurement of the positioning target; in is the identity matrix.

5. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 4 is characterized in that: The state transfer matrix in the state space equation The expression is: ； in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1.

6. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 4 is characterized in that: The control input matrix in the state space equation The expression is: ； in, is the sampling interval of the positioning system, that is, the time interval between time t and time t+1.

7. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to claim 4 is characterized in that: The control input vector in the state space equation at time t+1 Expressed as: ； in, is the acceleration of the positioning target on the x-axis at time t+1; is the acceleration of the positioning target on the y-axis at time t+1; It is the acceleration of the positioning target on the z-axis at time t+1.

8. The multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm according to any one of claims 4 to 7, characterized in that: The state vector in the state space equation at time t+1 The estimated position coordinates of the positioning target at time t+1 And the speed information of the positioning target at time t+1 The expression of the form is: .

9. A computer device, characterized in that: include: A memory and a processor, wherein the memory and the processor are communicatively connected to each other, the memory stores computer instructions, and the processor executes the multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm as described in any one of claims 1 to 8 by executing the computer instructions.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores computer instructions, and the computer instructions are used to enable a computer to execute a multi-station passive positioning method based on TDOA direction finding and Chan-EKF fusion algorithm as described in any one of claims 1 to 8.

Images