Combined system error correction and target state estimation method based on multi-angle measurement

Abstract

The invention discloses a combined system error correction and target state estimation method based on multi-angle measurement, which is applied to a three-dimensional passive multi-sensor multi-target tracking system. Comprising the following steps: constructing an augmented state vector based on a target state vector, an angle measurement deviation vector, a time deviation vector and a sensor position vector, and constructing an augmented state transition equation; performing modeling by taking azimuth angle measurement, pitch angle measurement and sensor position measurement as functions of an augmented state vector, constructing an augmented measurement vector and an augmented measurement equation, and determining a nonlinear relationship between the constructed augmented measurement vector and augmented state vector; azimuth angle measurement, pitch angle measurement and sensor position measurement are obtained through actual measurement, and an augmented state vector is estimated through linearization of a non-linear relation between a constructed augmented measurement vector and an augmented state vector and sequential filtering. The method can be applied to multi-source information fusion under a passive multi-sensor system, and the tracking and positioning precision of the target is improved.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of sensor detection, and particularly relates to a joint system error correction and target state estimation method based on multi-angle measurement. BACKGROUND

[0002] In a multi-sensor multi-target tracking system, passive sensors are widely used compared with active sensors due to their robustness in extreme environments. This is because passive sensors can only passively receive echoes emitted or reflected by targets, and have good concealment and strong survivability. Passive sensors can only obtain azimuth and elevation angle measurements, and can only locate targets according to angle information. Passive sensor network tracking system has obvious advantages such as low detectability, long working distance and strong anti-interference ability. However, there are system errors between different sensor nodes, which need to be corrected to improve the performance of the entire sensor network system. The system error of the passive sensor system mainly refers to the azimuth measurement deviation, the elevation angle measurement deviation and the station error of the sensor. There may also be time deviations in the multi-sensor system, mainly caused by internal data transmission and signal processing of each sensor and clock not aligned with absolute time. Therefore, there is a difference between the time axis of each sensor and the absolute time axis, resulting in a time deviation between the time axes of different sensors. The existence of system error and time deviation will lead to inaccurate estimation of the target position, and thus affect the reliability of the system. In order to improve the performance and accuracy of the sensor network system, the research on system error correction and time deviation compensation technology has become an important topic in the current sensor field. Therefore, by solving the problems of sensor network system error correction and time deviation compensation, the target tracking and positioning accuracy of the entire sensor network system can be improved, so as to better meet various actual needs. SUMMARY

[0003] In order to solve the above problems existing in the prior art, the present application provides a joint system error correction and target state estimation method based on multi-angle measurement. The technical problem to be solved by the present application is solved by the following technical scheme: A joint system error correction and target state estimation method based on multi-angle measurement is applied to a fusion center in a three-dimensional passive multi-sensor multi-target tracking system, and the system further comprises one target moving at a nearly uniform speed in a straight line and one stationary sensor;The method comprises: S1, constructing an augmented state vector based on a target state vector, an angle measurement deviation vector, a time deviation vector and a sensor position vector, and constructing a corresponding state transition equation as an augmented state transition equation;Wherein, the angle measurement deviation vector is obtained by constructing a bearing angle and an elevation angle measurement deviation vector; S2, the azimuth angle measurement, the elevation angle measurement and the sensor position measurement are modeled as functions of the augmented state vector, the augmented measurement vector and the augmented measurement equation are constructed, and the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector is determined; S3, the azimuth angle measurement, the elevation angle measurement and the sensor position measurement are obtained through actual measurement, the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector is linearized, and the augmented state vector is estimated by using the sequential filtering.

[0004] The beneficial effects of the present application are: The present application provides a joint system error correction and target state estimation method based on multi-angle measurement, first, an augmented state vector is constructed based on a target state vector, an angle measurement bias vector, a time bias vector and a sensor position vector, and a corresponding state transition equation is constructed as an augmented state transition equation; wherein the angle measurement bias vector is constructed by an azimuth angle and an elevation angle measurement bias vector; then the azimuth angle measurement, the elevation angle measurement and the sensor position measurement are modeled as functions of the augmented state vector, the augmented measurement vector and the augmented measurement equation are constructed, and the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector is determined; finally, the azimuth angle measurement, the elevation angle measurement and the sensor position measurement are obtained through actual measurement, the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector is linearized, and the augmented state vector is estimated by using the sequential filtering.

[0005] The present application aims at the target motion state estimation problem in the passive multi-sensor multi-target tracking system when the system error and measurement bias exist, and studies the joint estimation method of system error correction, time bias compensation and target motion state. By reasonably modeling the system error, the measurement model considering the system error and time bias is constructed, and the joint estimation method of system error correction, time bias compensation and target motion state based on the sequential filtering theory is established. From the azimuth angle observation , the elevation angle observation affected by noise and the obtained inaccurate sensor position information , the target state vector at each time, the unknown measurement bias of each sensor, the time bias and the position of each sensor The experimental data show that the combined estimation method can achieve the corresponding estimation BCRLB under various simulation conditions, especially under the condition of long observation time, the estimation performance of the method is outstanding. In summary, the method has the characteristics of simple implementation, and can be widely applied to the system error correction, time deviation compensation and target state estimation problems in the passive multi-sensor multi-target tracking system. BRIEF DESCRIPTION OF DRAWINGS

[0006] Figure 1 A flowchart of a combined system error correction and target state estimation method based on multi-angle measurement provided by an embodiment of the application is shown in the figure. Figure 2 It is the implementation flowchart of the method of the application. Figure 3 It is a motion scene schematic diagram in the simulation verification of the method of the application. Figure 4 It is a target position estimation mean square error result graph in the simulation verification of the method of the application. Figure 5 It is a target velocity estimation mean square error result graph in the simulation verification of the method of the application. Figure 6 It is a sensor azimuth measurement bias estimation mean square error result graph in the simulation verification of the method of the application. Figure 7 It is a sensor elevation angle measurement bias estimation mean square error result graph in the simulation verification of the method of the application. Figure 8 It is a sensor time deviation estimation mean square error result graph in the simulation verification of the method of the application. Figure 9 It is a sensor position estimation mean square error result graph in the simulation verification of the method of the application. DETAILED DESCRIPTION

[0007] The application will be further described in detail below in combination with specific embodiments, but the embodiments of the application are not limited thereto.

[0008] For the system error correction and time deviation compensation problems in the multi-sensor multi-target tracking system, the system error and time deviation are modeled, the azimuth angle and elevation angle measurement information of the target by each sensor and the inaccurate sensor position are used, the target state, measurement bias, time deviation and sensor station are constructed into an augmented state vector, a combined estimation method of the augmented state vector is proposed based on the sequential filtering theory, the system error can be accurately estimated, the time deviation can be compensated, and the tracking accuracy of the target can be improved.

[0009] Specifically, the embodiment of the present application provides a joint system error correction and target state estimation method based on multi-angle measurement, which is applied to a fusion center in a three-dimensional passive multi-sensor multi-target tracking system, and the system further comprises one target performing near-uniform straight line motion and one static sensor; see Figure 1 and Figure 2 It is understood that the method can comprise the following steps: S1, constructing an augmented state vector based on a target state vector, an angle measurement bias vector, a time bias vector and a sensor position vector, and constructing a corresponding state transition equation as an augmented state transition equation; wherein the angle measurement bias vector is constructed from the azimuth angle and the elevation angle measurement bias vector; In the considered three-dimensional passive multi-sensor multi-target tracking system, all sensors work asynchronously, passively receive signals from the target, and provide azimuth angle and elevation angle measurements of the target. In addition, the fusion center obtains azimuth angle and elevation angle measurements with unreliable time stamps. And the system can only obtain inaccurate sensor position information provided by GPS. S1 can be divided into the following processes in detail: (1) Determine the target state vector and the corresponding state transition equation The dynamic motion of the first m target in the tracking area can be modeled as: ; wherein, is the state vector of the target at time target in the Cartesian coordinate (i.e., the target state vector), denotes the state transition matrix corresponding to the target state vector. is modeled as a zero-mean Gaussian random vector with a known covariance matrix The target state vector of the first target is , which contains the target position and velocity, and is described as: ; wherein the target position term contains x , y and z directional positions, and the target velocity term contains x , y and z directional velocities. The state transition matrix and the covariance matrix of the target state vector are: ; where, denotes the identity matrix of dimension ; denotes the zero matrix of dimension ; denotes the time interval between the th and the th measurement transmitted to the fusion center; is the noise intensity in the target motion process, in units of . denotes the Kronecker product.

[0010] The state vector of all targets in the scenario can be collectively denoted as: ; where, denotes the state vector of the th target, consisting of the position and velocity in x , y and z directions.

[0011] The target state transition equation can be written as:

[0012] where, denotes the state transition matrix of the target state vector from time to time , denotes the state transition noise of the target state vector at time , which is modeled as a Gaussian white noise with covariance matrix : ; where, denotes the blkdiag function; and denote the state transition matrix and the state transition noise covariance matrix of the th target, respectively, denotes the zero vector of dimension .

[0013] (2) Determine the angle measurement bias vector and the corresponding state transition equation At time , the sensor produces an angle measurement value for the target , which can be denoted as: ; where, denote time instant sensor to the target azimuth measurement information (measurements); denote time instant sensor to the target elevation measurement information (measurements). denote time instant, emphasize that the measurements come from sensors . The azimuth and elevation measurements are denoted as: ; where, and denote the true values of the azimuth and elevation measurements, respectively, and denote the bias of the azimuth and elevation measurements of the sensor at time instant , respectively, and are measurement noises. For the measurement bias, considering that it varies slowly in practice, it is modeled as a first-order Markov process, denoted as: ; By integrating the azimuth and elevation measurement biases of all sensors, we have:

[0014] where, denote the angle measurement bias vector at time instant denote the azimuth measurement bias vector at time instant denote the elevation measurement bias vector at time instant ; Since the measurement bias is modeled as a first-order Markov process, the state transition equation of the angle measurement bias vector is obtained as: ; where, is the state transition equation of the angle measurement bias vector from time instant to time instant is the corresponding state transition noise, defined as a Gaussian white noise with covariance matrix , specifically denoted as: ; where, denote The variance of the state transition noise, express The variance of the state transition noise. (3) Determine the time deviation vector and the corresponding state transition equation. Due to differences in data transmission and signal processing, as well as misalignment with the absolute clock, there are discrepancies between the time axes of each sensor and the absolute time axis, resulting in time deviations between the time axes of the various sensors in the system. In a multi-sensor, multi-target system, the first sensor to report measurement information... The reference sensor is designated as the time axis, and the deviations between the time axes of the other sensors and the reference sensor are modeled as time deviations. Since the time axes of each sensor are relatively stable, the time deviations are set to unknown constants.

[0015] Each sensor compared to the reference sensor The time deviation vector can be represented as: ; Its state transition equation is: ; in, for Time's up The state transition matrix of the time deviation vector at time step [time]. Since the time deviation is constant. For an identity matrix, it can be represented as: ; (4) Determine the sensor position vector and the corresponding state transition equation. In multi-sensor systems, GPS-reported sensor positions are inaccurate due to measurement noise. Directly using inaccurate sensor positions for target localization will reduce positioning accuracy. Since the sensors are stationary, therefore... The sensor position vector at time t can be represented as: ; in, Indicates the first The positions of the sensors. And the state transition equation for the sensor position vector can be written as: ; in, The state transition matrix for the sensor position vector can be represented as: ; (5) Construct the augmented state vector and augmented state transition equation Based on the above analysis, considering the target state vector, angle measurement deviation vector, time deviation vector, and sensor position vector together, an augmented state vector is constructed, expressed as: ; in, express The augmented state vector at time step; express The target state vector at any given time; express The angle measurement deviation vector at time t, where express The azimuth measurement deviation vector at time [time]. express The pitch angle measurement deviation vector at any given time; express The time deviation vector at any given moment; express The sensor position vector at any given time; Indicates transpose; The augmented state transition equation is constructed and expressed as: ; in, express Time to The state transition matrix of the augmented state vector at each time step; Indicates that it follows the covariance matrix Gaussian white noise vector; ; ; ; in, This represents the blkdiag function; express Time to The state transition matrix of the target state vector at any given time; express Time to The state transition matrix of the angle measurement deviation vector at time; express Time to The state transition matrix of the time deviation vector at any given moment; express Time to The state transition matrix of the sensor position vector at any given time; express State transition noise of the target state vector at any given time; express State transition noise of the time-angle measurement deviation vector; The dimension is The zero vector; The dimension is The zero matrix; and Indicates that they respectively follow the covariance matrix and Gaussian white noise.

[0016] S2, model the azimuth angle measurement, pitch angle measurement and sensor position measurement as functions of the augmented state vector, construct the augmented measurement vector and augmented measurement equation, and determine the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector; The augmented state vector constructed based on S1 will be used for azimuth measurement. Pitch angle measurement and sensor position measurement Modeled as a function of the augmented state vector to perform system error correction and compensate for the effects of time deviation.

[0017] exist At any time, obtain sensor For the target Azimuth measurement information Pitch angle measurement information and sensors Location measurement information The augmented measurement vector can be constructed as follows: ; in, express The augmented measurement vector at time; express Time sensor For the target Azimuth measurement information; express Time sensor For the target Pitch angle measurement information; express Time sensor Location measurement information; Depend on and Composition, meaning Time sensor Generate target Angle measurement value. Furthermore, the augmented measurement equation is constructed, which can be expressed as: ; in, express The nonlinear relationship between the time-varying augmented measurement vector and the augmented state vector; express At any given time, there is a covariance matrix Random measurement noise; Among them, the nonlinear relationship between the augmented measurement vector and the augmented state vector , can be represented as: ; in, This represents the target position term after compensating for the time bias; that is, using the estimated time bias to compensate for its impact on the measurement. , , These represent the target positions in each direction after time deviation has been compensated; It is the first The location of each sensor , It is a sensor exist Location of time, superscript All represent the true values; and Represent Time sensor The deviation between the azimuth and elevation angle measurements; It is a sensor Compared to the reference sensor Time deviation; It is the target location item. Include x , y and z The position of direction; It is the target speed term. Include x , y and z velocity in the direction; express At any given time, there is a covariance matrix Random Gaussian measurement noise; Indicates azimuth measurement information The variance of the measurement noise; Indicates pitch angle measurement information The variance of the measurement noise; Indicates sensor position measurement information The covariance matrix of the measurement noise; subscript represents the first two elements of the corresponding vector.

[0018] S3, obtaining the azimuth angle measurement, the elevation angle measurement and the sensor position measurement by actual measurement, linearizing the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector, and estimating the augmented state vector by using the sequential filtering.

[0019] Specifically, S3 can include the following steps: S31, performing first-order Taylor expansion on the augmented measurement equation to linearize, to obtain the Jacobian matrix; For the actual estimation scene, based on the obtained azimuth angle measurement, the elevation angle measurement and the sensor position measurement, the relationship between the augmented measurement vector and the augmented state vector is used to estimate the augmented state vector. Due to the highly nonlinear relationship between the two, in order to facilitate calculation, it is necessary to perform first-order Taylor expansion linearization on the augmented measurement equation.

[0020] Specifically, S31 can include the following steps: S311, according to the defined prior estimate value of the augmented state vector , linearize the augmented measurement vector at to obtain: ; Wherein, the prior estimate value is the given initial estimate when processing for the first time, and starting from the second processing, the prior estimate value used each time is the optimal estimate obtained last time, which can be understood in detail later.

[0021] Wherein, the Jacobian matrix is expressed as: ; S312, based on the Jacobian matrix, the linearized augmented measurement equation is obtained, which is expressed as: ; Wherein, the specific mathematical form of the Jacobian matrix is: ; The first row of the matrix represents an azimuth angle gradient matrix, including derivative matrices of azimuth measurement with respect to a target state vector, an angle measurement bias vector, a time bias vector, and a sensor position vector; the second row and the third row represent a pitch angle gradient matrix and a sensor position gradient matrix respectively, including derivative matrices of pitch angle measurement with respect to the target state vector, the angle measurement bias vector, the time bias vector, and the sensor position vector, and derivative matrices of sensor position measurement with respect to the target state vector, the angle measurement bias vector, the time bias vector, and the sensor position vector.

[0022] S313, determining the azimuth angle measurement derivative of the augmented state vector derivative of the augmented state vector derivative of the augmented state vector derivative of the state vector derivative of the state vector , thereby obtaining the Jacobian matrix .

[0023] Specifically, the derivative of the azimuth angle measurement derivative of the augmented state vector is represented as: ; wherein each derivative is calculated by the following formula: .

[0024] Specifically, the derivative of the pitch angle measurement derivative of the augmented state vector is represented as: ; wherein each derivative is calculated by the following formula: .

[0025] Specifically, the derivative of the sensor position measurement derivative of the state vector is represented as: ; wherein, represents a zero matrix; represents an identity matrix of . In this way, the Jacobian matrix .

[0026] S32, after the initial estimate of the augmented state vector and the covariance matrix thereof at the initial time is given based on the Jacobian matrix, the augmented state vector is estimated by using a sequential filter, thereby obtaining an estimated result at each time.

[0027] In S32, the estimation result at each time is obtained, including the following steps: In S321, the state transition matrix and the optimal estimation at time are used to obtain the prior estimation of the augmented state vector at time : ; wherein the optimal estimation is the given initial estimation in the first processing, and the optimal estimation used in each processing after the second processing is obtained in the step S324 in the last processing.

[0028] In S322, the optimal estimation covariance matrix at time and the state transition noise covariance matrix are used to obtain the prior estimation covariance matrix at time : ; wherein the optimal estimation covariance matrix is the given initial estimation in the first processing, and the optimal estimation covariance matrix used in each processing after the second processing is obtained in the step S325 in the last processing. Meanwhile, the state transition noise covariance matrix used in each processing is a known fixed value.

[0029] In S323, the prior estimation covariance matrix at time , the measurement noise covariance matrix and the Jacobian matrix are used to obtain the Kalman gain at time : ; wherein the prior estimation covariance matrix is obtained in the step S322 in the current processing. Meanwhile, the measurement noise covariance matrix used in each processing is a known fixed value.

[0030] In S324, the prior estimation at time , the augmented measurement vector and the Kalman gain are used to obtain the optimal estimation at time : ; wherein the prior estimation at time is , the augmented measurement vector is the observation.

[0031] S325, using the time instant prior estimate covariance matrix and Kalman gain, obtaining the time instant optimal estimation covariance matrix: .

[0032] From the above S321~ S325 can understand how the augmented state vector at a time estimate result is obtained, therefore given the initial estimate of the augmented state vector at a time and its covariance matrix, so that the sequential estimation result of the augmented state vector at each time .

[0033] The present application is aimed at the target motion state estimation problem in the presence of system error and measurement bias in passive multi-sensor multi-target tracking system, the system error correction, time bias compensation and target motion state joint estimation method are researched. By reasonably modeling the system error, the measurement model considering system error and time bias is constructed, and the joint estimation method of system error estimation, time bias compensation and target motion state based on sequential filtering theory is established. From the bearing angle observation , pitch angle observation , and the obtained non-accurate sensor position information , the target state vector at each time, the unknown measurement bias and of each sensor, the time bias and the position of each sensor are estimated simultaneously by using sequential filtering. Experimental data show that the proposed joint estimation method can achieve the corresponding estimation BCRLB under various simulation conditions, especially under the condition of long observation time, the estimation performance of the proposed method is outstanding. In summary, the present application has the characteristics of simple and easy implementation, and can be widely used in passive multi-sensor multi-target tracking system system error correction, time bias compensation and target state estimation problem.

[0034] The effect of the method of the present application can be further illustrated by the following simulation experiment: I. Simulation conditions The simulation scene is set in a three-dimensional space, there are four stationary sensors and three moving targets. The specific parameters of the sensors and targets set in the simulation experiment are shown in Table 1 and Table 2 respectively. It is assumed that the first sensor reporting measurement in the system is sensor 1, so sensor 1 is set as the reference sensor. The observation lasts for 200 seconds, and 500 Monte Carlo experiments are performed to draw more general conclusions.

[0035] Table 1 Sensor parameter settings

[0036] Table 2 Target parameter settings

[0037] wherein, denotes the standard deviation of the directional position measurement noise of the sensor , x denotes the standard deviation of the directional position measurement noise of the sensor , denotes the standard deviation of the azimuth measurement bias of the sensor y , denotes the standard deviation of the state transition noise of the azimuth measurement bias of the sensor , denotes the standard deviation of the state transition noise of the elevation measurement bias of the sensor .

[0038] The estimation performance of the target state, the angle measurement bias, the time bias and the sensor position is measured by the root mean square error (RMSE)

[0039] wherein, denotes the estimate of the true value for the i-th Monte Carlo experiment, denotes the number of Monte Carlo experiments. The simulation compares the methods of ignoring the angle measurement bias, the time bias and the sensor position error respectively, which are named as EDT-KF-NMB, EDT-KF-NTB and EDT-KF-NSPE respectively, to verify the estimation performance of the method (EDT-KF) proposed in the application. II. Simulation results

[0040] As shown in FIG. 3, the real motion trajectories of the three targets, the target positions estimated by the EDT-KF method proposed and the positions of the four sensors are given in detail. It can be seen that the target position estimation result of the EDT-KF algorithm proposed is close to the real motion trajectory of the target. The root mean square errors of the estimations of the target position, the target velocity, the azimuth measurement bias, the elevation measurement bias, the time bias and the sensor position are shown respectively. In addition, the simulation results also include the performance limit that can be theoretically achieved, i.e. BCRLB, to measure the estimation performance of the algorithm. Figure 3 Figures 4-9 and

[0041] Figure 4 Figure 5 ​​The estimation results of the target's position and velocity by four different algorithms are shown. Figure 4 It is clear that among the four algorithms, the proposed EDT-KF algorithm performs best in estimating the target position. Its converged RMSE is approximately 35m, showing the smallest difference from BCRLB. The EDT-KF-NTB algorithm, which ignores time bias, performs second best, with an RMSE of approximately 66m. The performance of the other two methods deteriorates drastically, with RMSEs around 200m. This illustrates the importance of considering time bias, measurement bias, and sensor position errors in target position estimation. Figure 5 It can be seen that the EDT-KF algorithm and the EDT-KF-NTB algorithm (which ignores time bias) perform comparable to, and are the best in, for target velocity estimation. Both estimation results achieve BCRLB. However, the performance of the other two methods is poor. This indicates that time bias has a relatively small impact on target velocity estimation, but measurement bias and sensor position error need to be considered.

[0042] Figure 6 and Figure 7 Estimates of the measurement biases for azimuth and elevation angles are presented separately. From... Figure 6 It can be seen that the EDT-KF algorithm has the best estimation performance. After convergence, the RMSE is 0.14°. Although the EDT-KF-NTB algorithm performs best during initial filtering, its performance gradually decreases with increasing observation time. The EDT-KF-NSPE algorithm performs very poorly. This can be seen from... Figure 7 Similar conclusions were drawn from these studies. These phenomena indicate that sensor position error has the greatest impact on measurement bias estimation, followed by time bias. However, both aspects need to be considered in the algorithm to improve estimation performance.

[0043] Figure 8 The time bias estimation results of three algorithms are shown. Figure 8 As can be seen, the EDT-KF algorithm exhibits stable performance in estimating time bias, with optimal performance in the later stages. Although the EDT-KF-NMB algorithm performs best in the early stages, it exhibits divergence in the later stages. This phenomenon may be due to the accumulation of estimation errors caused by measurement bias. Throughout the entire estimation process, the EDT-KF-NSPE algorithm performs the worst. This illustrates the importance of considering sensor position errors and measurement biases on the estimation performance of the algorithms.

[0044] Figure 9The results of three algorithms for estimating sensor position are shown. It can be seen that the EDT-KF and EDT-KF-NTB algorithms have similar performance in the initial stage, but the performance of the EDT-KF-NTB algorithm slightly decreases as the observation time increases. The EDT-KF algorithm has globally optimal performance, while the EDT-KF-NMB algorithm has the worst performance. This demonstrates the importance of considering measurement bias and time bias in sensor position estimation.

[0045] In summary, simulation results demonstrate that the algorithm proposed in this invention outperforms the other three algorithms in target position estimation, azimuth measurement deviation estimation, time deviation estimation, and sensor position estimation, and achieves the corresponding BCRLB estimates in all cases. The above analysis also illustrates the importance of jointly considering measurement deviation, time deviation, and sensor position uncertainty in a pure azimuth multi-sensor fusion detection system.

[0046] The above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention are included within the scope of protection of the present invention.

Claims

1. A method for joint system error correction and target state estimation based on multi-angle measurements, characterized in that, A fusion center applied in a 3D passive multi-sensor multi-target tracking system, the system also includes... A target undergoing near-uniform linear motion and A stationary sensor; the method includes: S1, an augmented state vector is constructed based on the target state vector, angle measurement deviation vector, time deviation vector, and sensor position vector, and the corresponding state transition equation is constructed as the augmented state transition equation; wherein, the angle measurement deviation vector is constructed from the azimuth angle and pitch angle measurement deviation vectors; S2, model the azimuth angle measurement, pitch angle measurement and sensor position measurement as functions of the augmented state vector, construct the augmented measurement vector and augmented measurement equation, and determine the nonlinear relationship between the constructed augmented measurement vector and the augmented state vector; S3 obtains azimuth, pitch and sensor position measurements through actual measurements, linearizes the nonlinear relationship between the constructed augmented measurement vector and augmented state vector, and estimates the augmented state vector using sequential filtering.

2. The method according to claim 1, characterized in that, In S1, the constructed augmented state vector is represented as: ； in, express The augmented state vector at time step; express The target state vector at time t; express The angle measurement deviation vector at time t, where express The azimuth measurement deviation vector at time [time]. express The pitch angle measurement deviation vector at any given time; express The time deviation vector at any given moment; express The sensor position vector at any given time; Indicates transpose; The constructed augmented state transition equation is expressed as: ； in, express Time to The state transition matrix of the augmented state vector at each time step; Indicates that it follows the covariance matrix Gaussian white noise vector; ； ； ； in, This represents the blkdiag function; express Time to The state transition matrix of the target state vector at any given time; express Time to The state transition matrix of the angle measurement deviation vector at time; express Time to The state transition matrix of the time deviation vector at any given moment; express Time to The state transition matrix of the sensor position vector at any given time; express State transition noise of the target state vector at any given time; express State transition noise of the time-angle measurement deviation vector; The dimension is The zero vector; The dimension is The zero matrix; and Indicates that they respectively follow the covariance matrix and Gaussian white noise.

3. The method according to claim 1 or 2, characterized in that, In S2, the constructed augmented measurement vector is represented as: ； in, express The augmented measurement vector at time; express Time sensor For the target Azimuth measurement information; express Time sensor For the target Pitch angle measurement information; express Time sensor Location measurement information; Depend on and Composition, meaning Time sensor Generate target Angle measurement value.

4. The method according to claim 3, characterized in that, In S2, the augmented measurement equation is constructed as follows: ； in, express The nonlinear relationship between the time-varying augmented measurement vector and the augmented state vector; express At any given time, there is a covariance matrix Random measurement noise; Nonlinear relationship between augmented measurement vector and augmented state vector , is represented as: ； in, This represents the target position after compensating for time deviation. , , These represent the positions in each direction after time deviation has been compensated; It is the first The location of each sensor , It is a sensor exist Location of time, superscript All represent the true values; and Represent Time sensor The deviation between the azimuth and elevation angle measurements; It is a sensor Compared to the reference sensor Time deviation; It is the target location item. Include x , y and z The position of direction; It is the target speed term. Include x , y and z velocity in the direction; express At any given time, there is a covariance matrix Random Gaussian measurement noise; Indicates azimuth measurement information The variance of the measurement noise; Indicates pitch angle measurement information The variance of the measurement noise; Indicates sensor position measurement information The covariance matrix of the measurement noise; subscript This represents the first two elements of the corresponding vector.

5. The method according to claim 4, characterized in that, S3 obtains azimuth, elevation, and sensor position measurements through actual measurements. It linearizes the nonlinear relationship between the constructed augmented measurement vector and augmented state vector, and estimates the augmented state vector using sequential filtering, including: S31, linearize the augmented measurement equation by performing a first-order Taylor expansion to obtain the Jacobian matrix; S32, based on the Jacobian matrix, after giving an initial estimate of the augmented state vector and its covariance matrix at the initial time, the augmented state vector is estimated using sequential filtering to obtain the estimation result at each time.

6. The method according to claim 5, characterized in that, S31, the linearization of the augmented measurement equation by performing a first-order Taylor expansion to obtain the Jacobian matrix includes: S311, based on the defined augmented state vector Prior estimate , will augment the measurement vector exist Linearization yields: ； Among them, the Jacobian matrix Represented as: ； S312, the linearized augmented measurement equation obtained based on the Jacobian matrix is ​​expressed as: ； Among them, the Jacobian matrix The specific mathematical form is: ； The first row of the above matrix represents the azimuth gradient matrix, which includes the derivative matrix of the azimuth measurement with respect to the target state vector, the angle measurement deviation vector, the time deviation vector, and the sensor position vector; the second and third rows represent the pitch gradient matrix and the sensor position gradient matrix, respectively. S313, Determine azimuth measurement For augmented state vectors Differentiation of pitch angle measurement For augmented state vectors Differentiation of the sensor position measurement For the state vector By differentiating the derivative, we obtain the Jacobian matrix. .

7. The method according to claim 6, characterized in that, Azimuth measurement For augmented state vectors The derivative of is expressed as: ； Each derivative is calculated by the following formula: 。 8. The method according to claim 7, characterized in that, Pitch angle measurement For augmented state vectors The derivative of is expressed as: ； Each derivative is calculated by the following formula: 。 9. The method according to claim 8, characterized in that, Sensor position measurement For the state vector The derivative of is expressed as: ； in, Represents a zero matrix; express The identity matrix.

10. The method according to claim 9, characterized in that, In S32, based on the Jacobian matrix, after providing an initial estimate of the augmented state vector and its covariance matrix at an initial time, sequential filtering is used to estimate the augmented state vector, obtaining the estimation results at each time step, including: S321, using the state transition matrix and Time-optimal estimation ,get Prior estimates of the augmented state vector at time step: ； S322, utilizing The optimal covariance matrix at any given time and state transition noise covariance matrix ,get Prior estimation of covariance matrix at time points: ； S323, use Prior estimation of covariance matrix at time step Measurement noise covariance matrix And Jacobi matrix ,get Time-based Kalman gain: ； S324, utilizing The prior estimates, augmented measurement vector, and Kalman gain at time t are used to obtain... Optimal time estimate: ； S325, utilizing The covariance matrix and Kalman gain of the prior estimates at each time step are obtained. The optimal covariance matrix at any given time: 。

Images