CN116840777A - Adaptive square root unscented Kalman filter spatial deviation registration method and system - Google Patents

Adaptive square root unscented Kalman filter spatial deviation registration method and system Download PDF

Info

Publication number
CN116840777A
CN116840777A CN202310699258.4A CN202310699258A CN116840777A CN 116840777 A CN116840777 A CN 116840777A CN 202310699258 A CN202310699258 A CN 202310699258A CN 116840777 A CN116840777 A CN 116840777A
Authority
CN
China
Prior art keywords
covariance matrix
square root
deviation
moment
measurement
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202310699258.4A
Other languages
Chinese (zh)
Inventor
汪付强
徐歌星
吴晓明
张鹏
金星
张旭
马晓凤
张建强
郝秋赟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Qilu University of Technology
Shandong Computer Science Center National Super Computing Center in Jinan
Original Assignee
Qilu University of Technology
Shandong Computer Science Center National Super Computing Center in Jinan
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Qilu University of Technology, Shandong Computer Science Center National Super Computing Center in Jinan filed Critical Qilu University of Technology
Priority to CN202310699258.4A priority Critical patent/CN116840777A/en
Publication of CN116840777A publication Critical patent/CN116840777A/en
Pending legal-status Critical Current

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0205—Details
    • G01S5/021—Calibration, monitoring or correction
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/23—Clustering techniques
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H21/0025—Particular filtering methods
    • H03H21/0029—Particular filtering methods based on statistics
    • H03H21/003—KALMAN filters
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H21/0043—Adaptive algorithms
    • H—ELECTRICITY
    • H03—ELECTRONIC CIRCUITRY
    • H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
    • H03H21/00—Adaptive networks
    • H03H21/0012—Digital adaptive filters
    • H03H2021/0085—Applications
    • H03H2021/0087—Prediction

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Theoretical Computer Science (AREA)
  • Mathematical Physics (AREA)
  • Pure & Applied Mathematics (AREA)
  • Computational Mathematics (AREA)
  • Mathematical Analysis (AREA)
  • Mathematical Optimization (AREA)
  • General Engineering & Computer Science (AREA)
  • Algebra (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • Computing Systems (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • Feedback Control In General (AREA)

Abstract

The application belongs to the field of target tracking and positioning, and provides a self-adaptive square root unscented Kalman filtering space deviation registration method and a system, which initialize a target sensor to be detected and construct a sensor measurement equation and a target state equation; self-adaptively solving the square root of the covariance matrix, and calculating sampling points and weights; estimating state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using a self-adaptive unscented Kalman filtering algorithm; according to whether the current moment of the system causes abnormal measurement data due to noise and other interference, the adjustable parameters in the state equation are adaptively calibrated; constructing a deviation pseudo-measurement equation according to a deviation estimated value at the moment k-1, an error covariance matrix and predicted measurement data by using a self-adaptive unscented Kalman filtering algorithm and a self-adaptive clustering algorithm, and estimating and compensating the deviation value; repeating the steps to form a closed loop operation, and performing iterative operation until the registration of all the sensors is completed.

Description

Adaptive square root unscented Kalman filter spatial deviation registration method and system
Technical Field
The application belongs to the technical field of target tracking and positioning, and particularly relates to a self-adaptive square root unscented Kalman filtering spatial deviation registration method and system.
Background
The statements in this section merely provide background information related to the present disclosure and may not necessarily constitute prior art.
The estimation and compensation of the sensor space deviation in the distributed multi-sensor target tracking system are indispensable steps before track fusion, and the uncertainty such as the scaling and offset deviation of the distance and azimuth angle of the sensor in measurement is required to be estimated and the measurement is required to be compensated, so that the accuracy of the track of the sensor in target tracking is ensured.
The current research methods about the sensor space deviation are divided into an off-line mode and an on-line mode. Existing offline approaches have been based on Least Square (LS) methods, describing the offset compensation problem as a Least Square problem, estimating the sensor offset. Another approach implements an accurate maximum likelihood method (Exact Maximum Likelihood, EML) of sensor bias estimation by maximizing the likelihood function of the observed data. Another approach has also been to propose a maximum likelihood registration (Maximum Likelihood Registration, MLR) approach to solve the bias estimation problem for a plurality of heterogeneous sensors. However, the off-line method described above assumes that the sensor deviation is a constant value, and does not take into account the time-varying problem of the deviation, so that the deviation estimation effect is only good when the sensor spatial deviation change is small. Considering the time-varying or jitter nature of the sensor bias, some scholars have proposed to implement online estimation of bias using filtering methods. The prior art proposes to estimate the deviation based on Kalman Filter (KF), and can better estimate the deviation on the premise that the sensor space deviation and the platform posture deviation are not large. Aiming at a nonlinear scene, two space-time deviation estimation methods based on extended Kalman filtering (Extended Kalman Filter, EKF) and unscented Kalman filtering (Unscented Kalman Filter, UKF) are provided in the prior art, and the multi-sensor observation vector is subjected to dimension expansion processing to realize simultaneous estimation of space-time deviation and target state, so that joint estimation of the target state and the space deviation is realized. However, this method is not suitable for a distributed scenario, and as the number of sensors in the sensor system and the working time increase, the computational complexity increases greatly, resulting in a decrease in the performance of spatial deviation estimation. In view of the fact that the target may remain mobile in the area, the prior art proposes to construct the measurement equation according to the state of motion of the target, in order to ensure that the deviation can be accurately estimated also in the case of a mobile motion of the target. However, the method can only estimate the deviation, and cannot compensate the deviation to realize accurate estimation of the maneuvering target state.
In addition, when the space deviation estimation and compensation are studied, the existing method assumes that the process noise and the measurement noise in the system meet the Gaussian white noise condition, namely interference possibly caused by other noise on the system is not considered, and in an actual scene, filtering divergence can be possibly caused, the accuracy of the deviation estimation is influenced, and the target state estimation is inaccurate.
Disclosure of Invention
In order to solve the above problems, the present application provides a method and a system for adaptive Square root unscented Kalman filtering spatial deviation registration, which considers the limitations of the existing method and provides a filtering mode based on Square Root Unscented Kalman Filtering (SRUKF) for adaptive spatial deviation registration. The system with the nonlinear unknown noise interference is discussed, and the mobility of the target in the tracking process is considered, so that the on-line estimation and compensation of the deviation value of the sensor are realized.
According to some embodiments, the first scheme of the present application provides an adaptive square root unscented kalman filter spatial deviation registration method, which adopts the following technical scheme:
the adaptive square root unscented Kalman filtering spatial deviation registering method includes:
initializing a target sensor to be detected, and constructing a sensor measurement equation and a target state equation;
self-adaptively solving square root of covariance matrix, calculating sampling point and weight, and self-adaptively calibrating covariance matrix;
estimating state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using a self-adaptive square root unscented Kalman filtering algorithm;
according to whether the current moment of the system causes abnormal measurement data due to noise and other interference, the adjustable parameters in the state equation are adaptively calibrated;
constructing a deviation pseudo-measurement equation according to a deviation estimated value at the moment k-1, an error covariance matrix and predicted measurement data by using a self-adaptive square root unscented Kalman filtering algorithm and a self-adaptive clustering algorithm, and estimating and compensating the deviation value;
let k=k+1, repeat the above-mentioned step, form the closed loop circulation operation, carry on the iterative operation, until finish the registration of all sensors.
Further, the initializing the target sensor to be detected, and constructing a sensor measurement equation and a target state equation, specifically:
preprocessing measurement of each sensor of the distributed multi-sensor system;
the target state equation and the sensor measurement equation are constructed in consideration of the nonlinearity of the target state.
Further, the self-adaption obtains square root of covariance matrix, calculates sampling point and weight, and self-adaption calibrates covariance matrix, specifically:
acquiring a covariance matrix at the moment k-1, and calculating a covariance matrix eigenvalue by matrix operation;
judging the positive and negative of the characteristic value of the covariance matrix, adjusting the covariance matrix based on a diagonal factor, keeping the characteristic value of the covariance matrix non-negative, obtaining an adjusted covariance matrix, and performing Cholesky decomposition;
self-adaptively solving the square root of the adjusted covariance matrix, and generating a sampling point set by adopting a symmetrical sampling method;
and according to the relevance between the target mobility and the sampling points, the number of the sampling points is adaptively updated, and the corresponding weight is calculated.
Further, the method utilizes an adaptive square root unscented Kalman filtering algorithm to estimate the state, measurement and other filtering intermediate parameters of the k moment based on the state mean and covariance matrix of the k-1 moment, specifically:
estimating the state of the moment k based on the state average value of the moment k-1;
based on the state of the k moment, updating the measurement vector of the k moment;
based on the square root of the measurement error covariance matrix and the target state and observation vector, determining an innovation covariance matrix at the moment k, and a cross covariance matrix between the state and the observation;
the filter gain and state estimate at time k are updated.
Further, 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 calibrated in a self-adaptive manner, specifically:
calculating F-norms of the measurement error covariance matrix and the state error covariance matrix by utilizing matrix trace operation;
calculating a scale factor, and updating a covariance matrix by using the scale factor;
and performing filtering operation at the next moment by using the updated covariance matrix.
Further, the updated covariance matrix is specifically:
S new,k ＝β k ·S M,k|k-1 +(1-β k )·S Z,k|k-1 ；
wherein β ε (0, 1) is the scale factor, state error covariance matrix S M,k|k-1 And a measurement error covariance matrix S Z,k|k-1 。
Further, the method utilizes an adaptive square root unscented Kalman filtering algorithm and an adaptive clustering algorithm to construct a deviation pseudo-measurement equation according to a deviation estimated value at the time of k-1, an error covariance matrix thereof and predicted measurement data, and estimates and compensates the deviation value, specifically comprising the following steps:
performing initial clustering based on a distance-based clustering algorithm, and grouping the sensors according to the proximity degree of the sensors to the cluster center;
constructing a deviation pseudo-measurement equation according to a deviation estimated value at the moment k-1, an error covariance matrix thereof and predicted measurement data by using a self-adaptive square root unscented Kalman filtering algorithm;
estimating a bias value in the cluster based on a bias pseudo-measurement equation;
combining the deviation estimates in each cluster to obtain a global deviation estimated value;
and carrying out compensation registration on the measurement value at the moment k based on the global deviation estimated value.
According to some embodiments, a second aspect of the present application provides an adaptive square root unscented kalman filter spatial deviation registration system, which adopts the following technical scheme:
an adaptive square root unscented kalman filter spatial deviation registration system comprising:
the initialization unit is configured to initialize the target sensor to be detected and construct a sensor measurement equation and a target state equation;
a sampling point data calculation unit configured to calculate a square root of the covariance matrix adaptively, and calculate sampling points and weights;
the first-stage filtering unit is configured to estimate state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using an adaptive square root unscented Kalman filtering algorithm;
the correction unit is configured to adaptively calibrate adjustable parameters in the state equation according to whether abnormal measurement data is caused by noise and other interference at the current moment of the system;
the secondary filtering unit is configured to construct a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using an adaptive square root unscented Kalman filtering algorithm and an adaptive clustering algorithm, and estimate and compensate the deviation value;
and the iteration unit is configured to make k=k+1, repeat the steps to form closed loop operation, and perform iteration operation until the registration of all the sensors is completed.
According to some embodiments, a third aspect of the present application provides a computer-readable storage medium.
A computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps in the adaptive square root unscented kalman filter spatial deviation registration method as described in the first aspect above.
According to some embodiments, a fourth aspect of the application provides a computer device.
A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps in the adaptive square root unscented kalman filter spatial deviation registration method as defined in the first aspect above when the program is executed.
Compared with the prior art, the application has the beneficial effects that:
according to the application, the estimation of the sensor space deviation is carried out based on the self-adaptive square root unscented Kalman filtering algorithm, the mutation of data caused by the environment in the actual application scene of the sensor and the slow time variation of the systematic deviation of the distance, azimuth angle and the like of the sensor during target tracking are considered, the measurement data are error, the estimation of the target state and the measurement data is carried out through the self-adaptive filtering method, the deviation estimation and compensation of the asynchronous sensor in the system are realized, the mutation situation of the measurement data caused by unknown interference is reduced, and the measurement error in the distance and direction angle estimation is obviously reduced. According to the adaptive adjustment of the filtering parameters of the system scene, the stability of the system can be effectively increased, the influence of measurement noise and other uncertainty noise on the system is reduced, and the accuracy of the navigation and target tracking system is improved.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application.
FIG. 1 is a flow chart of an adaptive square root unscented Kalman filter spatial deviation registration method in an embodiment of the application;
FIG. 2 is a block flow diagram of a method for adaptive square root unscented Kalman filter spatial bias registration in accordance with an embodiment of the application;
FIG. 3 is a block diagram of an adaptive square root unscented Kalman filter spatial deviation registration system in accordance with an embodiment of the application.
Detailed Description
The application will be further described with reference to the drawings and examples.
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the application. Unless defined otherwise, 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 application belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present application. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
Embodiments of the application and features of the embodiments may be combined with each other without conflict.
Example 1
As shown in fig. 1, this embodiment provides a method for adaptive square root unscented kalman filter spatial deviation registration, and this embodiment is illustrated by applying the method to a server, and it can be understood that the method may also be applied to a terminal, and may also be applied to a system and a terminal, and implemented through interaction between the terminal and the server. The server can be an independent physical server, a server cluster or a distributed system formed by a plurality of physical servers, and can also be a cloud server for providing cloud services, cloud databases, cloud computing, cloud functions, cloud storage, network servers, cloud communication, middleware services, domain name services, security services CDNs, basic cloud computing services such as big data and artificial intelligent platforms and the like. The terminal may be, but is not limited to, a smart phone, a tablet computer, a notebook computer, a desktop computer, a smart speaker, a smart watch, etc. The terminal and the server may be directly or indirectly connected through wired or wireless communication, and the present application is not limited herein. In this embodiment, the method includes the steps of:
initializing a target sensor to be detected, and constructing a sensor measurement equation and a target state equation;
self-adaptively solving the square root of the covariance matrix, and calculating sampling points and weights;
estimating state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using a self-adaptive square root unscented Kalman filtering algorithm;
according to whether abnormal measurement data is caused by noise interference at the current moment of the system, the adjustable parameters in the state equation are adaptively calibrated;
constructing a deviation pseudo-measurement equation according to a deviation estimated value at the moment k-1, an error covariance matrix and predicted measurement data by using a self-adaptive square root unscented Kalman filtering algorithm and a self-adaptive clustering algorithm, and estimating and compensating the deviation value;
let k=k+1, repeat the above-mentioned step, form the closed loop circulation operation, carry on the iterative operation, until finish the registration of all sensors.
As shown in fig. 1 and 2, in the distributed multi-sensor system, because of the difference of the sensors in terms of sampling time, transmission rate and the like, the measured data has asynchronism, and the application designs based on Square Root Unscented Kalman Filter (SRUKF) filtering and sequential filtering algorithms to estimate and compensate the sensor space deviation, and the steps are as follows:
step 1: initializing a target to be measured, and constructing a target state and a measurement equation;
step 2: the filtering algorithm is improved, the square root of the covariance matrix is obtained in a self-adaptive mode, and Sigma (sampling) points and weights are calculated;
step 3: estimating state, measurement and other filtering intermediate parameters of the k moment based on the state mean value and covariance matrix of the k-1 moment by using an adaptive filtering algorithm;
step 4: adaptively calibrating adjustable parameters in a state equation;
step 5: constructing a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using a filtering algorithm, updating the deviation estimated value and compensating the biased measurement information;
step 6: let k=k+1, repeat the above steps, form the closed loop operation, and perform the iterative operation.
In the distributed multi-sensor system, each sensor performs measurement pretreatment through an internal computing unit, and transmits local measurement to perform subsequent deviation treatment.
In the step 1, considering the nonlinearity of the target state, the standard forms of the target state equation and the measurement equation based on the SRUKF filtering algorithm are respectively:
α k ＝f(α k-1 )+ω k-1
z k ＝h(α k )+ν k
wherein, f (·) and h (·) respectively represent a system state transfer function, a measurement function, and a process noise ω k-1 Observation noise v k Are all Gaussian white noise and are mutually independent, Q k Covariance matrix of process noise in represented target state equation, R k Covariance matrix, alpha, representing measurement noise in measurement equation k Is the current time state.
Because systematic errors, external interference and the like can influence the measurement value of the sensor in the actual scene of target tracking, a deviation term b is added for a measurement equation k-1 The measurement equation is rewritten as:
z k ＝h(α k )+b k-1 +ν k 。
in the step 2, the SRUKF filtering algorithm calculates Sigma points by using square root of covariance matrix in the filtering process, so as to realize higher numerical precision than the standard UKF.
Therefore, when square root calculation is performed, an appropriate decomposition algorithm needs to be selected, the Cholesky decomposition algorithm needs to keep the covariance matrix non-negative, and an adaptive improved Cholesky decomposition algorithm is provided, and diagonal elements of the covariance matrix are dynamically adjusted to make the matrix non-negative.
The steps of the adaptive Cholesky decomposition algorithm are as follows:
obtaining covariance matrix P at k-1 moment k-1 Matrix operation is carried out to calculate matrix characteristic values;
judging the positive and negative of the characteristic value of the covariance matrix, setting corresponding diagonal factors, adding the corresponding diagonal factors into diagonal elements of the covariance matrix, keeping the characteristic value of the covariance matrix nonnegative, ensuring the positive or semi-positive of the covariance matrix, and obtaining an adjusted covariance matrix P '' k-1 ；
For the adjusted covariance matrix P' k-1 The decomposition of Cholesky was performed and,
S k-1 ＝chol(P' k-1 )＝L·L * ；
wherein chol represents Cholesky decomposition, L and L * Respectively representThe diagonal elements are the lower triangular matrix of positive numbers and the conjugate transpose matrix thereof.
Then use the square root S of the covariance matrix at time k-1 k-1 Constructing Sigma points, and generating a Sigma point set { x ] by adopting a symmetrical sampling method i }. The Sigma point number is determined according to the inner dimension of the state quantity x, and if x=n, the point set { x } i 2n+1 points are included in }.
Considering the variability of the target motion state in the target tracking scene, the Sigma quantity is adaptively updated according to the relevance between the target mobility and the sampling point, and the calculation efficiency is improved. The adaptive updating of Sigma points follows the following rule, and when the uncertainty of the system is lower, namely the target mobility is lower, and uniform or nearly uniform motion is performed, fewer Sigma points can be utilized; and when the uncertainty of the system is higher, namely the target mobility is high, and the speed change or turning movement is performed, more Sigma points can be utilized. The Sigma point adaptive update formula is:
wherein phi is an adjustment factor for controlling the rate of change of Sigma points, tr (·) represents the trace calculation of the matrix, S k Is the square root of the covariance matrix at time k (i.e., the current time).
Sigma points and weights are expressed as:
wherein alpha is i,k-1 The ith sampling point of the moment k-1 is represented, and the corresponding weight value is W i ，And S is k-1 And respectively representing the square root of the mean value and the covariance matrix of the k-1 moment state, wherein lambda is a scale parameter for determining the distance between a Sigma point and a mean point, and n is the representation when the Sigma point is calculated by using a symmetrical method.
In the step 3, an ASRUKF filtering algorithm is utilized to calculate the state, measurement and other filtering intermediate values of the k moment according to the state vector and the error covariance matrix of the k-1 moment, and the specific steps are as follows:
based on the state mean value at the time k-1, estimating the state at the time k, that is, updating the prior estimation of the state at the time k and the error covariance matrix thereof, wherein the calculation formula is as follows:
α i,k|k-1 ＝f(α i,k-1 ,ω k-1 )
wherein,,s is the predicted state at time k M,k|k-1 Representing the square root of the a priori estimate of the error covariance matrix, chol' (. Cndot.) represents the adaptive Cholesky decomposition algorithm, α i,k|k-1 The a priori estimated value of the ith sampling point at the k moment is expressed, 2n+1 is the number of sampling point sets and alpha i,k-1 The value of the i-th sample point at time k-1 is indicated.
Based on the state of the k moment, updating the measurement vector of the k moment;
updating the measurement vector at time kThe calculation formula is as follows:
ζ i,k|k-1 ＝h(α i,k|k-1 )+ν k-1
wherein h (·) is the measurement transfer matrix,ζ i,k|k-1 is its corresponding measurement value obtained from the sampling point.
Based on the square root of the measurement error covariance matrix and the target state and observation vector, determining an innovation covariance matrix at the moment k, and a cross covariance matrix between the state and the observation;
the formula for calculating the innovation covariance matrix at the moment k and the cross covariance matrix between the state and the observation is as follows:
wherein S is Z,k|k-1 S is the square root of the measurement error covariance matrix M,Z(k|k-1) T represents the transpose of the matrix, which is the square root of the cross covariance matrix between the target state and the observations.
Updating the filter gain and state estimation at the k moment, specifically:
updating the filter gain at the moment k, wherein the formula is as follows:
updating the state estimation, wherein the formula is as follows:
in the step 4, unknown noise in the actual working scene of the sensor can cause interference to the measured value, an adaptive calibration method is designed, adaptive adjustment of a noise covariance matrix is performed according to the noise level, and interference of abnormal measured data caused by noise to the accuracy of the system is reduced.
In the presence of unknown noise interference, a state error covariance matrix S is utilized M,k|k-1 And a measurement error variance matrix S Z,k|k-1 And performing adaptive calibration of the covariance matrix.
Firstly, calculating F-norms of a measurement error covariance matrix and a state error covariance matrix by utilizing matrix trace operation tr (), and calculating a scaling factor beta epsilon (0, 1);
then according to S new,k ＝β k ·S M,k|k-1 +(1-β k )·S Z,k|k-1 Updating covariance matrix, using updated covariance matrix S new,k And performing filtering operation at the next moment.
In step 5, the ASRUKF filtering and the sequential algorithm are combined to perform sequential filtering registration of the multi-sensor offset values. The number of sensors in the target tracking system can fluctuate due to different targets, and the self-adaptive clustering algorithm and the deviation registration filtering algorithm are combined, so that the target tracking efficiency is improved.
And a sensor self-adaptive clustering algorithm is used for clustering the sensors according to the proximity degree of the sensors to the target, ensuring that the sensors in each cluster are still close to the target, and dynamically adjusting the size and shape of the cluster by periodically recalculating the cluster and adjusting the relevant threshold value of the cluster according to the position and the speed of the target so as to ensure the effectiveness of the cluster.
The step 5 specifically comprises the following steps:
performing initial clustering based on a distance-based clustering algorithm, and grouping the sensors according to the proximity degree of the sensors to the cluster center;
constructing a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using a self-adaptive unscented Kalman filtering algorithm;
estimating a bias value in the cluster based on a bias pseudo-measurement equation;
combining the deviation estimates in each cluster to obtain a global deviation estimated value;
and carrying out compensation registration on the measurement value at the moment k based on the global deviation estimated value.
That is, when bias estimation is performed, first, initial clustering is performed by a distance-based clustering algorithm, sensor grouping is performed according to the proximity of the sensors to the cluster center, and the intra-cluster bias value is estimated by a filtering algorithm. In the estimation of the deviation value by each cluster in parallel, the intra-cluster deviation estimation value (pseudo measurement equation) is expressed as follows:
wherein b j,k-1 Representing the deviation value in the k-1 time cluster, S j,b,k-1 The square root of the covariance matrix representing the deviation,representing the residual of the offset value, t represents the t-th sensor within cluster j.
Combining the bias estimates in each cluster at the fusion center to obtain a global bias estimateAnd registering the measured value at the moment k, wherein the calibrated sensor measurement is used for subsequent track fusion, and the target state is estimated.
And 6, repeating the steps 2 to 5 until the registration of all the sensors in the system at the moment k is completed.
Example two
As shown in fig. 3, the present embodiment provides an adaptive square root unscented kalman filter spatial deviation registration system, including:
the initialization unit is configured to initialize the target sensor to be detected and construct a sensor measurement equation and a target state equation;
a sampling point data calculation unit configured to calculate a square root of the covariance matrix adaptively, and calculate sampling points and weights;
the first-stage filtering unit is configured to estimate state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using an adaptive square root unscented Kalman filtering algorithm;
the correction unit is configured to adaptively calibrate adjustable parameters in the state equation according to whether abnormal measurement data is caused by noise interference at the current moment of the system;
the secondary filtering unit is configured to construct a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using an adaptive square root unscented Kalman filtering algorithm and an adaptive clustering algorithm, and estimate and compensate the deviation value;
a reading unit configured to read a target state and a spatial deviation estimation equivalent from the estimated state at the time k;
and the iteration unit is configured to make k=k+1, repeat the steps to form closed loop operation, and perform iteration operation until the registration of all the sensors is completed.
The above modules are the same as examples and application scenarios implemented by the corresponding steps, but are not limited to what is disclosed in the first embodiment. It should be noted that the modules described above may be implemented as part of a system in a computer system, such as a set of computer-executable instructions.
The foregoing embodiments are directed to various embodiments, and details of one embodiment may be found in the related description of another embodiment.
The proposed system may be implemented in other ways. For example, the system embodiments described above are merely illustrative, such as the division of the modules described above, are merely a logical function division, and may be implemented in other manners, such as multiple modules may be combined or integrated into another system, or some features may be omitted, or not performed.
Example III
The present embodiment provides a computer readable storage medium having stored thereon a computer program which when executed by a processor performs the steps in the adaptive square root unscented kalman filter spatial deviation registration method as described in the above embodiment one.
Example IV
The present embodiment provides a computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, the processor implementing the steps in the adaptive square root unscented kalman filter spatial deviation registration method as described in the above embodiment one when executing the program.
It will be appreciated by those skilled in the art that embodiments of the present application may be provided as a method, system, or computer program product. Accordingly, the present application may take the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present application may take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, magnetic disk storage, optical storage, and the like) having computer-usable program code embodied therein.
The present application is described with reference to flowchart illustrations and/or block diagrams of methods, apparatus (systems) and computer program products according to embodiments of the application. It will be understood that each flow and/or block of the flowchart illustrations and/or block diagrams, and combinations of flows and/or blocks in the flowchart illustrations and/or block diagrams, can be implemented by computer program instructions. These computer program instructions may be provided to a processor of a general purpose computer, special purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be stored in a computer-readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable memory produce an article of manufacture including instruction means which implement the function specified in the flowchart flow or flows and/or block diagram block or blocks.
These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart flow or flows and/or block diagram block or blocks.
Those skilled in the art will appreciate that implementing all or part of the above-described methods in accordance with the embodiments may be accomplished by way of a computer program stored on a computer readable storage medium, which when executed may comprise the steps of the embodiments of the methods described above. The storage medium may be a magnetic disk, an optical disk, a Read-Only Memory (ROM), a random access Memory (Random Access Memory, RAM), or the like.
While the foregoing description of the embodiments of the present application has been presented in conjunction with the drawings, it should be understood that it is not intended to limit the scope of the application, but rather, it is intended to cover all modifications or variations within the scope of the application as defined by the claims of the present application.

Claims (10)

1. The adaptive square root unscented Kalman filtering spatial deviation registering method is characterized by comprising the following steps:
initializing a target sensor to be detected, and constructing a sensor measurement equation and a target state equation;
self-adaptively solving the square root of the covariance matrix, and calculating sampling points and weights;
estimating state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using a self-adaptive square root unscented Kalman filtering algorithm;
according to whether abnormal measurement data is caused by noise interference at the current moment of the system, the adjustable parameters in the state equation are adaptively calibrated;
constructing a deviation pseudo-measurement equation according to a deviation estimated value at the moment k-1, an error covariance matrix and predicted measurement data by using a self-adaptive square root unscented Kalman filtering algorithm and a self-adaptive clustering algorithm, and estimating and compensating the deviation value;
let k=k+1, repeat the above-mentioned step, form the closed loop circulation operation, carry on the iterative operation, until finish the registration of all sensors.
2. The adaptive square root unscented kalman filter spatial deviation registration method of claim 1, wherein the initializing the target sensor to be measured, constructing a sensor measurement equation and a target state equation, specifically comprises:
preprocessing measurement of each sensor of the distributed multi-sensor system;
the target state equation and the sensor measurement equation are constructed in consideration of the nonlinearity of the target state.
3. The adaptive square root unscented kalman filter spatial deviation registration method of claim 1, wherein the adaptively solving for square root of covariance matrix calculates sampling points and weights, specifically:
acquiring a covariance matrix at the moment k-1, and calculating a covariance matrix eigenvalue by matrix operation;
judging the positive and negative of the characteristic value of the covariance matrix, adjusting the covariance matrix based on a diagonal factor, keeping the characteristic value of the covariance matrix non-negative, obtaining an adjusted covariance matrix, and performing Cholesky decomposition;
self-adaptively solving the square root of the adjusted covariance matrix, and generating a sampling point set by adopting a symmetrical sampling method;
and according to the relevance between the target mobility and the sampling points, the number of the sampling points is adaptively updated, and the corresponding weight is calculated.
4. The method for spatial deviation registration of adaptive square root unscented kalman filter according to claim 1, wherein the estimating the state, measurement and other filtering intermediate parameters at k time based on the state mean and covariance matrix at k-1 time by using the adaptive square root unscented kalman filter algorithm is specifically as follows:
estimating the state of the moment k based on the state average value of the moment k-1;
based on the state of the k moment, updating the measurement vector of the k moment;
based on the square root of the measurement error covariance matrix and the target state and observation vector, determining an innovation covariance matrix at the moment k, and a cross covariance matrix between the state and the observation;
the filter gain and state estimate at time k are updated.
5. The method for adaptive square root unscented kalman filter spatial deviation registration according to claim 1, wherein the adjustable parameters in the adaptive calibration state equation are specifically:
calculating F-norms of the measurement error covariance matrix and the state error covariance matrix by utilizing matrix trace operation;
calculating a scale factor, and updating a covariance matrix by using the scale factor;
and performing filtering operation at the next moment by using the updated covariance matrix.
6. The adaptive square root unscented kalman filter spatial deviation registration method of claim 5, wherein the updated covariance matrix is:
S new,k ＝β k ·S M,k|k-1 +(1-β k )·S Z,k|k-1 ；
wherein β ε (0, 1) is the scale factor, state error covariance matrix S M,k|k-1 And a measurement error covariance matrix S Z,k|k-1 。
7. The method for spatial deviation registration of adaptive square root unscented kalman filter according to claim 1, wherein the method for estimating and compensating the deviation value by using the adaptive square root unscented kalman filter algorithm and the adaptive clustering algorithm constructs a deviation pseudo-measurement equation according to the deviation estimated value at the time of k-1, the error covariance matrix thereof and the predicted measurement data, specifically:
performing initial clustering based on a distance-based clustering algorithm, and grouping the sensors according to the proximity degree of the sensors to the cluster center;
constructing a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using a self-adaptive square root unscented Kalman filtering algorithm;
estimating a bias value in the cluster based on a bias pseudo-measurement equation;
combining the deviation estimates in each cluster to obtain a global deviation estimated value;
and carrying out compensation registration on the measurement value at the moment k based on the global deviation estimated value.
8. An adaptive square root unscented kalman filter spatial deviation registration system, comprising:
the initialization unit is configured to initialize the target sensor to be detected and construct a sensor measurement equation and a target state equation;
a sampling point data calculation unit configured to calculate a square root of the covariance matrix adaptively, and calculate sampling points and weights;
the first-stage filtering unit is configured to estimate state, measurement and other filtering intermediate parameters at the moment k based on the state mean and covariance matrix at the moment k-1 by using an adaptive square root unscented Kalman filtering algorithm;
the correction unit is configured to adaptively calibrate adjustable parameters in the state equation according to whether abnormal measurement data is caused by noise interference at the current moment of the system;
the secondary filtering unit is configured to construct a deviation pseudo-measurement equation according to the deviation estimated value at the moment k-1, the error covariance matrix and the predicted measurement data by using an adaptive square root unscented Kalman filtering algorithm and an adaptive clustering algorithm, and estimate and compensate the deviation value;
and the iteration unit is configured to make k=k+1, repeat the steps to form closed loop operation, and perform iteration operation until the registration of all the sensors is completed.
9. A computer readable storage medium, having stored thereon a computer program, which when executed by a processor performs the steps in the adaptive square root unscented kalman filter spatial deviation registration method as defined in any of the claims 1-7.
10. A computer device comprising a memory, a processor and a computer program stored on the memory and executable on the processor, characterized in that the processor implements the steps in the adaptive square root unscented kalman filter spatial deviation registration method as claimed in any of the claims 1-7 when the program is executed.
CN202310699258.4A 2023-06-13 2023-06-13 Adaptive square root unscented Kalman filter spatial deviation registration method and system Pending CN116840777A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310699258.4A CN116840777A (en) 2023-06-13 2023-06-13 Adaptive square root unscented Kalman filter spatial deviation registration method and system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310699258.4A CN116840777A (en) 2023-06-13 2023-06-13 Adaptive square root unscented Kalman filter spatial deviation registration method and system

Publications (1)

Publication Number Publication Date
CN116840777A true CN116840777A (en) 2023-10-03

Family

ID=88164438

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310699258.4A Pending CN116840777A (en) 2023-06-13 2023-06-13 Adaptive square root unscented Kalman filter spatial deviation registration method and system

Country Status (1)

Country Link
CN (1) CN116840777A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117493775A (en) * 2023-12-29 2024-02-02 北京华龙通科技有限公司 Relative navigation method and device of data chain, electronic equipment and storage medium

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117493775A (en) * 2023-12-29 2024-02-02 北京华龙通科技有限公司 Relative navigation method and device of data chain, electronic equipment and storage medium
CN117493775B (en) * 2023-12-29 2024-05-14 北京华龙通科技有限公司 Relative navigation method and device of data chain, electronic equipment and storage medium

Similar Documents

Publication Publication Date Title
CN112013836B (en) Attitude heading reference system algorithm based on improved adaptive Kalman filtering
Nazarahari et al. 40 years of sensor fusion for orientation tracking via magnetic and inertial measurement units: Methods, lessons learned, and future challenges
Chen et al. Distributed cubature information filtering based on weighted average consensus
CN109916407B (en) Indoor mobile robot combined positioning method based on adaptive Kalman filter
CN110702095B (en) Data-driven high-precision integrated navigation data fusion method
CN108896047B (en) Distributed sensor network collaborative fusion and sensor position correction method
CN103389506B (en) A kind of adaptive filtering method for a strapdown inertia/Beidou satellite integrated navigation system
AU2009289008B2 (en) Estimating a state of at least one target
Gao et al. Adaptive unscented Kalman filter based on maximum posterior and random weighting
CN116840777A (en) Adaptive square root unscented Kalman filter spatial deviation registration method and system
CN110567455B (en) Tightly-combined navigation method for quadrature updating volume Kalman filtering
CN103776449B (en) A kind of initial alignment on moving base method that improves robustness
CN112798021A (en) Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter
CN109508510A (en) A kind of rubidium atomic clock parameter estimation algorithm based on improved Kalman filtering
CN111076722A (en) Attitude estimation method and device based on self-adaptive quaternion
CN109151759B (en) Sensor network distributed information weighted consistency state filtering method
CN109520503A (en) Adaptive Kalman filtering SLAM method for square root volume ambiguity
CN108344409A (en) Method for improving satellite attitude determination precision
CN115981148A (en) Unmanned aerial vehicle ground moving target tracking method
CN114323007A (en) Carrier motion state estimation method and device
CN114501615A (en) Terminal positioning method, device, computer equipment, storage medium and program product
De Silva et al. Efficient distributed multi-robot localization: A target tracking inspired design
Ruoyu et al. Modeling of MEMS gyro drift based on wavelet threshold denoising and improved Elman neural network
Alkhatib et al. Further results on a robust multivariate time series analysis in nonlinear models with autoregressive and t-distributed errors
CN109343013B (en) Spatial registration method and system based on restarting mechanism

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination