Adaptive square root unscented kalman filter spatial bias registration method and system

Abstract

The application belongs to the field of target tracking and positioning, and provides a self-adaptive square root unscented Kalman filtering space bias registration method and system, initializes a sensor of a to-be-measured target, constructs a sensor measurement equation and a target state equation; self-adaptively obtains a square root of a covariance matrix, calculates a sampling point and a weight value; utilizes a self-adaptive unscented Kalman filtering algorithm, estimates a state, measurement and other filtering intermediate parameters at a k time based on a state mean value and a covariance matrix at a k-1 time; according to whether abnormal measurement data is caused by noise and other interferences at a current time of the system, self-adaptively calibrates an adjustable parameter in the state equation; utilizes the self-adaptive unscented Kalman filtering algorithm and a self-adaptive clustering algorithm, constructs a bias pseudo-measurement equation according to an estimated value of the bias at the k-1 time and an error covariance matrix and predicted measurement data, estimates and compensates the bias value; repeats the above steps, forms a closed loop operation, and performs iterative operation until registration of all sensors is completed.

Description

TECHNICAL FIELD

[0001] The present application belongs to the technical field of target tracking and positioning, and particularly relates to a self-adaptive square root unscented Kalman filtering space bias registration method and system. BACKGROUND

[0002] The statements in this section merely provide background information related to the present application and do not necessarily constitute the prior art.

[0003] In a distributed multi-sensor target tracking system, sensor space bias estimation and compensation is an indispensable step before track fusion, which needs to estimate the uncertainty of the scaling and offset bias of the distance and azimuth angle of the sensor in measurement and compensate the measurement to ensure the accuracy of the track of the sensor in target tracking.

[0004] The current research methods for sensor spatial bias are divided into offline and online methods. The existing offline methods include a least square (LS) method, which describes the bias compensation problem as a least square problem to estimate the sensor bias. Another method is an exact maximum likelihood (EML) method that estimates the sensor bias by maximizing the likelihood function of the observation data. Another method is a maximum likelihood registration (MLR) method that solves the bias estimation problem of multiple heterogeneous sensors. However, the above offline methods assume that the bias of the sensor is a constant value, and do not consider the time-varying problem of the bias, so the bias estimation effect is better only when the spatial bias of the sensor changes little. Considering the time-varying or jittering nature of the sensor bias, some scholars propose to use filtering methods to realize online estimation of the bias. The existing technology proposes to estimate the bias based on Kalman filter (KF), which can better estimate the bias under the premise that the spatial bias of the sensor and the attitude bias of the platform are small. For nonlinear scenes, the existing technology proposes two space-time bias estimation methods based on extended Kalman filter (EKF) and unscented Kalman filter (UKF), respectively, which perform dimension expansion processing on the multi-sensor observation vector to realize simultaneous estimation of space-time bias and target state, and realize joint estimation of target state and spatial bias. However, this method is not suitable for distributed scenes, and with the increase of the number of sensors in the sensor system and the working time, the computational complexity will increase greatly, causing the reduction of the spatial bias estimation performance. Considering that the target may keep maneuvering in the region, the existing technology proposes to construct the measurement equation according to the target motion state to ensure that the bias can be accurately estimated even if the target takes maneuvering motion. However, this method can only estimate the bias and cannot compensate the bias to realize accurate estimation of the maneuvering target state.

[0005] In addition, the existing method assumes that the process noise and measurement noise in the system satisfy the Gaussian white noise condition, that is, it does not consider the interference that other noises may cause to the system, which may cause filter divergence in actual scenes, affect the accuracy of bias estimation, and cause inaccurate estimation of the target state. SUMMARY

[0006] In order to solve the above problems, the application provides a self-adaptive square-root unscented Kalman filtering space bias registration method and system, the application considers the limitations of the existing method, proposes a filtering method based on square-root unscented Kalman filtering (Square-root UKF, SRUKF), and performs self-adaptive space bias registration. The system with unknown noise interference is discussed, the maneuverability of the target in the tracking process is considered, and online estimation and compensation of the bias value of the sensor are realized.

[0007] According to some embodiments, the first scheme of the application provides a self-adaptive square-root unscented Kalman filtering space bias registration method, which adopts the following technical scheme:

[0008] The self-adaptive square-root unscented Kalman filtering space bias registration method comprises the following steps:

[0009] Initializing a target sensor to be measured, and constructing a sensor measurement equation and a target state equation;

[0010] Adaptive square root of covariance matrix is obtained, sampling points and weights are calculated, and the covariance matrix is adaptively calibrated;

[0011] Using the adaptive square-root unscented Kalman filtering algorithm, the state, measurement and other filtering intermediate parameters at time k are estimated based on the state mean value and the covariance matrix at time k-1;

[0012] According to whether the current time of the system is caused by abnormal measurement data due to noise and other interferences, the adjustable parameters in the state equation are adaptively calibrated;

[0013] Using the adaptive square-root unscented Kalman filtering algorithm and the adaptive clustering algorithm, the bias pseudo-measurement equation is constructed according to the bias estimation value at time k-1 and the error covariance matrix and the predicted measurement data, and the bias value is estimated and compensated;

[0014] Let k=k+1, repeat the above steps to form a closed loop operation, and perform iterative operation until the registration of all sensors is completed.

[0015] Further, the initialization of the target sensor to be measured, the construction of the sensor measurement equation and the target state equation are specifically as follows:

[0016] The measurement of each sensor of the distributed multi-sensor system is preprocessed;

[0017] Considering the nonlinearity of the target state, the target state equation and the sensor measurement equation are constructed.

[0018] Further, the adaptive square root of the covariance matrix is obtained, the sampling points and the weights are calculated, and the covariance matrix is adaptively calibrated, specifically as follows:

[0019] Obtaining the covariance matrix at k-1 moment, and calculating the eigenvalue of the covariance matrix through matrix operation;

[0020] Determining the positive and negative of the eigenvalue of the covariance matrix, adjusting the covariance matrix based on the diagonal factor to keep the eigenvalue of the covariance matrix non-negative, obtaining the adjusted covariance matrix, and performing Cholesky decomposition;

[0021] Adaptively obtaining the square root of the adjusted covariance matrix, and generating a set of sampling points by using symmetric sampling method;

[0022] According to the correlation between the target maneuverability and the sampling points, adaptively updating the number of sampling points, and calculating the corresponding weight.

[0023] Further, the adaptive square root unscented Kalman filter algorithm is used to estimate the state, measurement and other filter intermediate parameters at k moment based on the state mean value and covariance matrix at k-1 moment, specifically:

[0024] Estimating the state at k moment based on the state mean value at k-1 moment;

[0025] Updating the measurement vector estimated at k moment based on the state at k moment;

[0026] Based on the square root of the measurement error covariance matrix and the target state and observation vector, determining the innovation covariance matrix at k moment, the mutual covariance matrix between the state and observation;

[0027] Updating the filter gain and state estimation at k moment.

[0028] Further, the adaptive calibration of the adjustable parameters in the state equation according to whether the current moment of the system is caused by abnormal measurement data due to noise interference, specifically:

[0029] Using matrix trace operation to calculate the F-norm of the measurement error covariance matrix and the state error covariance matrix;

[0030] Calculating the scaling factor, and updating the covariance matrix by using the scaling factor;

[0031] Using the updated covariance matrix to perform filtering operation at the next moment.

[0032] Further, the updated covariance matrix is specifically:

[0033] S new,k = β k · S M,k|k-1 + (1-β k )· S Z,k|k-1 ;

[0034] Wherein, β ∈ (0, 1) is a scaling factor, and the state error covariance matrix SM,k|k-1 And the measurement error covariance matrix S Z,k|k-1 .

[0035] Furthermore, the method of using the adaptive square root unscented Kalman filter algorithm and the adaptive clustering algorithm to construct a pseudo-measurement equation for the deviation based on the deviation estimate and its error covariance matrix at time k-1 and the predicted measurement data, and to estimate and compensate for the deviation value, specifically is as follows:

[0036] Initial clustering is performed using a distance-based clustering algorithm, and sensors are grouped according to their proximity to the cluster center.

[0037] Using the adaptive square root unscented Kalman filter algorithm, a pseudo-measurement equation for the deviation is constructed based on the deviation estimate at time k-1, its error covariance matrix, and the predicted measurement data.

[0038] Estimating intra-cluster bias values ​​based on the bias pseudo-measurement equation;

[0039] The bias estimates within each cluster are combined to obtain the global bias estimate.

[0040] The measurement values ​​at time k are compensated and registered based on the global bias estimate.

[0041] According to some embodiments, the second aspect of the present invention provides an adaptive square root unscented Kalman filter spatial bias registration system, which adopts the following technical solution:

[0042] An adaptive square root unscented Kalman filter spatial bias registration system includes:

[0043] The initialization unit is configured to initialize the sensor of the target under test and construct the sensor measurement equation and the target state equation.

[0044] The sampling point data calculation unit is configured to adaptively calculate the square root of the covariance matrix and calculate the sampling points and weights;

[0045] The first-level filtering unit is configured to use the adaptive square root unscented Kalman filter algorithm to estimate the state, measurement and other intermediate filtering parameters at time k based on the state mean and covariance matrix at time k-1.

[0046] The correction unit is configured to adaptively calibrate the adjustable parameters in the state equation based on whether the system's current measurement data is abnormal due to noise or other interference.

[0047] The second-level filtering unit is configured to use the adaptive square root unscented Kalman filter algorithm and the adaptive clustering algorithm to construct the deviation pseudo-measurement equation based on the deviation estimate at time k-1 and its error covariance matrix and the predicted measurement data, and to estimate and compensate for the deviation value.

[0048] an iteration unit configured to let k=k+1, repeat the above steps, form a closed loop operation, and perform an iterative operation until registration of all sensors is completed.

[0049] According to some embodiments, a third aspect of the present application provides a computer readable storage medium.

[0050] A computer readable storage medium having stored thereon a computer program which, when executed by a processor, implements the steps of the adaptive unscented Kalman filter spatial bias registration method according to the first aspect.

[0051] According to some embodiments, a fourth aspect of the present application provides a computer device.

[0052] A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, wherein the processor implements the steps of the adaptive unscented Kalman filter spatial bias registration method according to the first aspect when executing the program.

[0053] Compared with the prior art, the present application has the following beneficial effects:

[0054] The present application estimates the spatial bias of sensors based on the adaptive unscented Kalman filter algorithm, considers the sudden changes in data caused by the environment in the actual application scenario of the sensor and the slow time-varying system bias of the sensor in target tracking, such as distance and azimuth, to cause errors in the measurement data, estimates the target state and measurement data through an adaptive filtering method, realizes the bias estimation and compensation of asynchronous sensors in the system, reduces the sudden changes in measurement data caused by unknown interference, significantly reduces the errors in distance and direction angle estimation, and effectively increases the stability of the system according to the adaptive adjustment of the filtering parameters, reduces the influence of measurement noise and other uncertain noise on the system, and improves the accuracy of the navigation and target tracking system. BRIEF DESCRIPTION OF DRAWINGS

[0055] The drawings constituting a part of the specification of the present application are used to provide a further understanding of the present application, and the illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application.

[0056] Figure 1 is a flowchart of the adaptive unscented Kalman filter spatial bias registration method in the embodiments of the present application;

[0057] Figure 2 is a flowchart of the adaptive unscented Kalman filter spatial bias registration method in the embodiments of the present application;

[0058] Figure 3This is a structural diagram of the adaptive square root unscented Kalman filter spatial bias registration system in an embodiment of the present invention. Detailed Implementation

[0059] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0060] It should be noted that the following detailed description is illustrative and intended to provide further explanation of the invention. Unless otherwise specified, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0061] It should be noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to limit the scope of exemplary embodiments according to the invention. As used herein, the singular form is intended to include the plural form as well, unless the context clearly indicates otherwise. Furthermore, it should be understood that when the terms "comprising" and / or "including" are used in this specification, they indicate the presence of features, steps, operations, devices, components, and / or combinations thereof.

[0062] Where there is no conflict, the embodiments and features in the embodiments of the present invention can be combined with each other.

[0063] Example 1

[0064] like Figure 1 As shown, this embodiment provides an adaptive square root unscented Kalman filter spatial bias registration method. This embodiment uses the application of this method to a server as an example for illustration. It can be understood that this method can also be applied to terminals, and can also be applied to systems including terminals, servers, and other components, and can be implemented through the interaction between the terminal and the server. The server can be an independent physical server, a server cluster composed of multiple physical servers, or a distributed system. It can also be a cloud server that provides basic cloud computing services such as cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network servers, cloud communication, middleware services, domain name services, CDN security services, and big data and artificial intelligence platforms. The terminal can be a smartphone, tablet computer, laptop computer, desktop computer, smart speaker, smartwatch, etc., but is not limited to these. The terminal and the server can be directly or indirectly connected through wired or wireless communication, which is not limited in this application. In this embodiment, the method includes the following steps:

[0065] Initialize the sensor of the target under test, and construct the sensor measurement equation and the target state equation;

[0066] Adaptively calculate the square root of the covariance matrix, and then calculate the sampling points and weights;

[0067] Using the adaptive square root unscented Kalman filter algorithm, based on the state mean and covariance matrix at k-1 time, the state, measurement and other filter intermediate parameters at k time are estimated;

[0068] According to whether the current time of the system is abnormal measurement data caused by noise interference, the adjustable parameters in the state equation are adaptively calibrated;

[0069] Using the adaptive square root unscented Kalman filter algorithm and adaptive clustering algorithm, according to the bias estimation value and its error covariance matrix at k-1 time and the predicted measurement data, a bias pseudo measurement equation is constructed to estimate and compensate the bias value;

[0070] Let k=k+1, repeat the above steps to form a closed loop operation, and perform iterative operation until the registration of all sensors is completed.

[0071] As shown in Figure 1 and Figure 2 , in the distributed multi-sensor system, due to the differences in sampling time, transmission rate and other aspects of the sensors, the measurement data are asynchronous, and the present application is designed based on square root unscented Kalman filter (SRUKF) filtering and sequential filtering algorithm to estimate and compensate the spatial bias of the sensors, and the steps are as follows:

[0072] Step 1: initialize the target to be measured, and construct the target state and measurement equation;

[0073] Step 2: improve the filtering algorithm, adaptively obtain the square root of the covariance matrix, and calculate the Sigma (sample) point and weight;

[0074] Step 3: using the adaptive filtering algorithm, based on the state mean and covariance matrix at k-1 time, the state, measurement and other filter intermediate parameters at k time are estimated;

[0075] Step 4: adaptively calibrate the adjustable parameters in the state equation;

[0076] Step 5: using the filtering algorithm, according to the bias estimation value and its error covariance matrix at k-1 time and the predicted measurement data, a bias pseudo measurement equation is constructed to update the bias estimation value and compensate the biased measurement information;

[0077] Step 6: let k=k+1, repeat the above steps to form a closed loop operation, and perform iterative operation.

[0078] In the distributed multi-sensor system, each sensor performs pre-processing of the measurement through an internal computing unit, and transmits the local measurement for subsequent bias processing.

[0079] In step 1, considering the nonlinearity of the target state, the standard form of the target state equation and the measurement equation based on the SRUKF filtering algorithm is as follows:

[0080] α k = f(α k-1 ) + ω k-1

[0081] z k = h(α k ) + v k

[0082] Wherein, f(·), h(·) respectively represent the system state transition function, measurement function, process noise ω k-1 , observation noise v k are Gaussian white noise and independent of each other, Q k represents the covariance matrix of process noise in the target state equation, R k represents the covariance matrix of measurement noise in the measurement equation, α k is the current state.

[0083] Because in the actual scene of target tracking, system error, external disturbance and other factors will affect the sensor measurement value, therefore, a bias term b k-1 is added to the measurement equation, and the measurement equation is rewritten as:

[0084] z k = h(α k ) + b k-1 + v k .

[0085] In step 2, the SRUKF filtering algorithm uses the square root of the covariance matrix to calculate the Sigma point in the filtering process, which realizes higher numerical accuracy than the standard UKF.

[0086] Therefore, when calculating the square root, an appropriate decomposition algorithm needs to be selected. Cholesky decomposition algorithm requires the covariance matrix to be non-negative, and an adaptive improved Cholesky decomposition algorithm is proposed to dynamically adjust the diagonal elements of the covariance matrix to make the matrix non-negative.

[0087] The steps of the adaptive Cholesky decomposition algorithm are as follows:

[0088] Get the covariance matrix P k-1 at k-1 time, calculate the eigenvalue of the matrix by matrix operation;

[0089] The positive and negative of eigenvalues of the covariance matrix is judged, corresponding diagonal factors are set, and the corresponding diagonal factors are added to the diagonal elements of the covariance matrix, so that the eigenvalues of the covariance matrix remain non-negative, the positive definiteness or semi-positive definiteness of the covariance matrix is ensured, and the adjusted covariance matrix P' is obtained k-1 ;

[0090] The adjusted covariance matrix P' k-1 is subjected to Cholesky decomposition,

[0091] S k-1 = chol(P' k-1 ) = L·L * ;

[0092] Wherein, chol represents Cholesky decomposition, L and L * respectively represent lower triangular matrices with positive diagonal elements and their conjugate transpose matrices.

[0093] Then the square root S k-1 of the covariance matrix at the k-1 moment is used to construct Sigma points, and a symmetric sampling method is used to generate a Sigma point set {x i}. The number of Sigma points is determined according to the dimension of the state variable x, if x = n, then the point set {x i} contains 2n+1 points.

[0094] Considering the variability of the target motion state in the target tracking scene, it is proposed to adaptively update the Sigma number according to the correlation between the target maneuverability and the sampling points, and to improve the calculation efficiency. The adaptive update of Sigma points follows the following rules: when the uncertainty of the system is low, that is, the target maneuverability is low, and the target moves at a constant speed or approximately at a constant speed, fewer Sigma points can be used; when the system uncertainty is high, that is, the target maneuverability is high, and the target moves at a variable speed or turns, more Sigma points can be used. The adaptive update formula of Sigma points is:

[0095]

[0096] Wherein, φ is an adjustment factor for controlling the change rate of Sigma points, tr(·) represents the trace operation of a matrix, S k is the square root of the covariance matrix at the k moment (that is, the current moment).

[0097] Sigma points and weights are represented as:

[0098]

[0099] In the formula, α i,k-1 represents the i-th sampling point at the k-1 moment, and the corresponding weight is Wi , and S k-1 represent the mean and square root of the covariance matrix of the state at k-1 time, λ is a scale parameter used to determine the distance of the sigma points from the mean point, and n represents the number of sigma points when the symmetric method is used.

[0100] In step 3, the ASRUKF filtering algorithm is used to calculate the state, measurement and other filtering intermediate values at time k based on the state vector and error covariance matrix at time k-1. The specific steps are as follows:

[0101] Based on the mean state at time k-1, the state at time k is estimated, that is, the prior estimate of the state at time k and its error covariance matrix is updated, and the calculation formula is:

[0102] α i,k|k-1 =f(α i,k-1 ,ω k-1 )

[0103]

[0104]

[0105] wherein, is the predicted state at time k, S M,k|k-1 is the square root of the prior estimate of the error covariance matrix, chol'(·) is the adaptive Cholesky decomposition algorithm, α i,k|k-1 is the prior estimate value of the i-th sampling point at time k, 2n+1 is the number of sampling points, and α i,k-1 is the value of the i-th sampling point at time k-1.

[0106] Based on the state at time k, the measurement vector at time k is updated;

[0107] The measurement vector at time k is updated The calculation formula is:

[0108] ζ i,k|k-1 =h(α i,k|k-1 )+ν k-1

[0109]

[0110] wherein h(·) is the measurement transition matrix, ζ i,k|k-1 is the measurement value corresponding to the sampling point.

[0111] Based on the square root of the measurement error covariance matrix and the target state and observation vector, the innovation covariance matrix at time k, the mutual covariance matrix between the state and the observation is determined;

[0112] The formula for calculating the innovation covariance matrix, the cross-covariance matrix between the state and the observation at time k is:

[0113]

[0114]

[0115] where S Z,k|k-1 is the square root of the measurement error covariance matrix, S M,Z(k|k-1) is the square root of the cross-covariance matrix between the target state and the observation, and T represents the transpose of the matrix.

[0116] The filter gain and state estimation at time k are updated, specifically:

[0117] The filter gain at time k is updated, and the formula is:

[0118] The state estimation is updated, and the formula is:

[0119] In step 4, unknown noise in the actual working scene of the sensor will interfere with the measurement value, and an adaptive calibration method is designed to adaptively adjust the noise covariance matrix according to the noise level, reducing the interference of abnormal measurement data caused by noise on the accuracy of the system.

[0120] In the presence of unknown noise interference, the state error covariance matrix S M,k|k-1 and the measurement error variance matrix S Z,k|k-1 are used to adaptively calibrate the covariance matrix.

[0121] First, the F-norm of the measurement error covariance matrix and the state error covariance matrix is calculated using the matrix trace operation tr(·), and the proportion factor β∈(0,1) is calculated;

[0122] Then, according to S new,k = β k · S M,k|k-1 + (1-β k )· S Z,k|k-1 , the covariance matrix is updated, and the updated covariance matrix S new,k is used for the next time filtering operation.

[0123] That is, in step 5, the ASRUKF filter is combined with the sequential algorithm to perform sequential filtering and registration of multi-sensor bias values. In the target tracking system, the number of sensors will fluctuate due to different targets, and the adaptive clustering algorithm is combined with the bias registration filtering algorithm to improve the efficiency of target tracking.

[0124] The sensor adaptive clustering algorithm, that is, clustering the sensors according to the proximity of the sensors to the target, ensures that the sensors in each cluster are still close to the target, and by periodically recalculating the cluster and adjusting the cluster-related threshold according to the position and speed of the target, the size and shape of the cluster are dynamically adjusted to ensure the effectiveness of the cluster.

[0125] The step 5 is specifically:

[0126] The distance-based clustering algorithm is used for initial clustering, and the sensors are grouped according to the proximity of the sensors to the cluster center;

[0127] An adaptive unscented Kalman filter algorithm is used to construct a bias pseudo-measurement equation according to the bias estimation value and its error covariance matrix at k-1 time and the predicted measurement data;

[0128] The bias value within the cluster is estimated based on the bias pseudo-measurement equation;

[0129] The bias estimates within each cluster are combined to obtain a global bias estimate value;

[0130] The measurement value at k time is compensated and registered based on the global bias estimate value.

[0131] That is, when estimating the bias, first, the distance-based clustering algorithm is used for initial clustering, and the sensors are grouped according to the proximity of the sensors to the cluster center, and the filter algorithm is used to estimate the bias value within the cluster. In the combination of bias value estimation of each cluster, the bias estimation value within the cluster (pseudo-measurement equation) is as follows:

[0132]

[0133] Where, b j,k-1 represents the bias value within the cluster at k-1 time, S j,b,k-1 represents the square root of the covariance matrix of the bias, represents the residual of the bias value, and t represents the tth sensor within the cluster j.

[0134] The bias estimates within each cluster are combined at the fusion center to obtain a global bias estimate value Then, the measurement value at k time is registered, and the calibrated sensor measurement will be used for subsequent track fusion to estimate the target state.

[0135] Step 6 is used to repeat steps 2 to 5 above until the registration of all sensors in the system at k time is completed.

[0136] Embodiment two

[0137] As Figure 3 shown, the embodiment provides an adaptive square root unscented Kalman filter space bias registration system, comprising:

[0138] an initialization unit configured to initialize a target sensor to be measured, construct a sensor measurement equation and a target state equation;

[0139] a sampling point data calculation unit configured to adaptively obtain a square root of a covariance matrix and calculate a sampling point and a weight;

[0140] a first-level filtering unit configured to estimate a state, a measurement and other filtering intermediate parameters at time k based on a state mean value and a covariance matrix at time k-1 by using an adaptive square-root unscented Kalman filtering algorithm;

[0141] a correction unit configured to adaptively calibrate an adjustable parameter in a state equation according to whether abnormal measurement data is caused by noise interference at a current time of a system;

[0142] a second-level filtering unit configured to estimate a bias value and compensate the bias value by constructing a bias pseudo-measurement equation based on a bias estimation value at time k-1 and an error covariance matrix of the bias estimation value and predicted measurement data by using an adaptive square-root unscented Kalman filtering algorithm and an adaptive clustering algorithm;

[0143] a reading unit configured to read a target state and a bias estimation value from an estimated state at time k;

[0144] an iteration unit configured to set k=k+1, repeat the above steps, form a closed-loop operation, and perform an iterative operation until registration of all sensors is completed.

[0145] The above modules and corresponding steps achieve the same examples and application scenarios as the above embodiment one, but are not limited to the content disclosed in the above embodiment one. It should be noted that the above modules, as part of the system, can be executed in a computer system such as a set of computer executable instructions.

[0146] The description of each of the above embodiments has its own focus, and the parts not described in detail in a certain embodiment can be referred to the related description of other embodiments.

[0147] The proposed system can be implemented in other ways. For example, the above-described system embodiments are only illustrative, for example, the division of the above modules is only a logical function division, and in actual implementation, another division method can be used, for example, multiple modules can be combined or integrated into another system, or some features can be ignored or not executed.

[0148] Embodiment three

[0149] The embodiment provides a computer readable storage medium, which stores a computer program, and the program is executed by a processor to realize steps in the adaptive square root unscented Kalman filter spatial bias registration method in the embodiment one.

[0150] Embodiment four

[0151] The embodiment provides a computer device, which comprises a memory, a processor and a computer program stored in the memory and executable on the processor, and the processor realizes steps in the adaptive square root unscented Kalman filter spatial bias registration method in the embodiment one when executing the program.

[0152] Those skilled in the art should understand that the embodiments of the present application can be provided as a method, a system or a computer program product. Therefore, the present application can adopt a hardware embodiment, a software embodiment or an embodiment combining software and hardware aspects. Moreover, the present application can adopt a computer program product implemented on one or more computer usable storage media (including but not limited to disk storage and optical storage) containing computer usable program codes.

[0153] The present application is described with reference to flowcharts and / or block diagrams of the method, device (system) and computer program product according to the embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, and the combination of the flows and / or blocks in the flowcharts and / or block diagrams can be realized by computer program instructions. These computer program instructions can be provided to a general-purpose computer, a special-purpose computer, an embedded processor or other programmable data processing devices to produce a machine, so that the instructions executed by the computer or other programmable data processing devices produce a device implemented in the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 means for performing the function specified by the one or more blocks.

[0154] These computer program instructions can also be stored in a computer readable storage medium capable of guiding the computer or other programmable data processing devices to work in a specific way, so that the instructions stored in the computer readable storage medium produce a product including instruction means, which realizes the flowcharts and / or block diagrams. Figure 1 one or more flows and / or blocks Figure 1 one or more blocks.

[0155] These computer program instructions can also be loaded into a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to produce a computer implemented process, so that the instructions executed on the computer or other programmable device provide a process for realizing the flowcharts and / or block diagrams. Figure 1one or more processes and / or blocks Figure 1 steps of the functions specified in the one or more blocks.

[0156] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiment methods can be completed by instructing the relevant hardware through a computer program. The program can be stored in a computer readable storage medium, and when the program is executed, the program can include the processes of the above-mentioned embodiment methods. The storage medium can be a magnetic disc, an optical disc, a read-only memory (ROM), a random access memory (RAM), or the like.

[0157] Although the specific embodiments of the present application are described above with reference to the accompanying drawings, the description is not a limitation on the scope of protection of the present application. Those skilled in the art should understand that various modifications or changes made by those skilled in the art on the basis of the technical solutions of the present application without creative labor are still within the scope of protection of the present application.

Claims

1. An adaptive square root unscented Kalman filter spatial bias registration method, characterized in that, include: Initialize the sensor of the target under test, and construct the sensor measurement equation and the target state equation; Adaptively calculate the square root of the covariance matrix, and then calculate the sampling points and weights; Using the adaptive square root unscented Kalman filter algorithm, based on the state mean and covariance matrix at time k-1, the state, measurement and other intermediate filter parameters at time k are estimated. Based on whether the system's current measurement data is abnormal due to noise interference, the adjustable parameters in the state equation are adaptively calibrated. Using the adaptive square root unscented Kalman filter algorithm and the adaptive clustering algorithm, based on the deviation estimate and its error covariance matrix at time k-1 and the predicted measurement data, a pseudo-measurement equation for the deviation is constructed to estimate and compensate for the deviation value. Specifically: Initial clustering is performed using a distance-based clustering algorithm, and sensors are grouped according to their proximity to the cluster center. Using the adaptive square root unscented Kalman filter algorithm, a pseudo-measurement equation for the deviation is constructed based on the deviation estimate and its error covariance matrix at time k-1 and the predicted measurement data. Estimating intra-cluster bias values ​​based on the bias pseudo-measurement equation; The bias estimates within each cluster are combined to obtain the global bias estimate. Compensation registration is performed on the measurement values ​​at time k based on the global bias estimate; Let k = k + 1, repeat the above steps to form a closed-loop operation, perform iterative calculations, until the registration of all sensors is completed.

2. The adaptive square root unscented Kalman filter spatial bias registration method as described in claim 1, characterized in that, The initialization of the sensor to be measured, and the construction of the sensor measurement equation and the target state equation, are specifically as follows: Preprocessing of measurements from each sensor in a distributed multi-sensor system; Considering the nonlinearity of the target state, construct the target state equation and the sensor measurement equation.

3. The adaptive square root unscented Kalman filter spatial bias registration method as described in claim 1, characterized in that, The adaptive calculation of the square root of the covariance matrix, and the calculation of sampling points and weights, specifically involves: Obtain the covariance matrix at time k-1, and use matrix operations to calculate the eigenvalues ​​of the covariance matrix; To determine the sign of the eigenvalues ​​of the covariance matrix, the covariance matrix is ​​adjusted based on the diagonal factors to keep the eigenvalues ​​non-negative, resulting in the adjusted covariance matrix, which is then subjected to Cholesky decomposition. The square root of the adjusted covariance matrix is ​​adaptively obtained, and a set of sampling points is generated using a symmetric sampling method. Based on the correlation between target mobility and sampling points, the number of sampling points is adaptively updated, and the corresponding weights are calculated.

4. The adaptive square root unscented Kalman filter spatial bias registration method as described in claim 1, characterized in that, The adaptive square root unscented Kalman filter algorithm estimates the state, measurement, and other intermediate filtering parameters at time k based on the state mean and covariance matrix at time k-1. Specifically: Estimate the state at time k based on the mean state at time k-1. Based on the state at time k, update the estimated measurement vector at time k; Based on the square root of the measurement error covariance matrix and the target state and observation vector, determine the information covariance matrix at time k, and the cross-covariance matrix between the state and the observation; Update the filter gain and state estimate at time k.

5. The adaptive square root unscented Kalman filter spatial bias registration method as described in claim 1, characterized in that, The adaptive calibration of adjustable parameters in the state equation based on whether abnormal measurement data is caused by noise interference at the current system moment is specifically as follows: The F-norm of the measurement error covariance matrix and the state error covariance matrix is ​​calculated using matrix trace operations. Calculate the scaling factor and update the covariance matrix using the scaling factor; The updated covariance matrix is ​​used for filtering operations at the next time step.

6. The adaptive square root unscented Kalman filter spatial bias registration method as described in claim 5, characterized in that, The updated covariance matrix is ​​as follows: ； in, It is a scaling factor, the state error covariance matrix. and measurement error covariance matrix .

7. An adaptive square root unscented Kalman filter spatial bias registration system, characterized in that, include: The initialization unit is configured to initialize the sensor of the target under test and construct the sensor measurement equation and the target state equation. The sampling point data calculation unit is configured to adaptively calculate the square root of the covariance matrix and calculate the sampling points and weights; The first-level filtering unit is configured to use the adaptive square root unscented Kalman filter algorithm to estimate the state, measurement and other intermediate filtering parameters at time k based on the state mean and covariance matrix at time k-1. The correction unit is configured to adaptively calibrate the adjustable parameters in the state equation based on whether the system's current measurement data is abnormal due to noise interference. The second-level filtering unit is configured to use an adaptive square root unscented Kalman filter algorithm and an adaptive clustering algorithm to construct a pseudo-measurement equation for the deviation based on the deviation estimate and its error covariance matrix at time k-1 and the predicted measurement data. This equation is used to estimate and compensate for the deviation value. Specifically: Initial clustering is performed using a distance-based clustering algorithm, and sensors are grouped according to their proximity to the cluster center. Using the adaptive square root unscented Kalman filter algorithm, a pseudo-measurement equation for the deviation is constructed based on the deviation estimate and its error covariance matrix at time k-1 and the predicted measurement data. Estimating intra-cluster bias values ​​based on the bias pseudo-measurement equation; The bias estimates within each cluster are combined to obtain the global bias estimate. Compensation registration is performed on the measurement values ​​at time k based on the global bias estimate; The iterative unit is configured to let k = k + 1, repeat the above steps to form a closed-loop operation, perform iterative calculations, and complete the registration of all sensors.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the program is executed by the processor, it implements the steps in the adaptive square root unscented Kalman filter spatial bias registration method as described in any one of claims 1-6.

9. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps in the adaptive square root unscented Kalman filter spatial bias registration method as described in any one of claims 1-6.

Images