CN115014325A - Combined navigation data fusion method based on robust volume point updating framework - Google Patents

Combined navigation data fusion method based on robust volume point updating framework Download PDF

Info

Publication number
CN115014325A
CN115014325A CN202210615134.9A CN202210615134A CN115014325A CN 115014325 A CN115014325 A CN 115014325A CN 202210615134 A CN202210615134 A CN 202210615134A CN 115014325 A CN115014325 A CN 115014325A
Authority
CN
China
Prior art keywords
matrix
equation
representing
covariance matrix
error
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
CN202210615134.9A
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
Original Assignee
Qilu University of Technology
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 filed Critical Qilu University of Technology
Priority to CN202210615134.9A priority Critical patent/CN115014325A/en
Publication of CN115014325A publication Critical patent/CN115014325A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • 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
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis

Abstract

The invention relates to the field of integrated navigation and information fusion, and particularly discloses an integrated navigation data fusion method based on a robust volume point updating frame. And the integration navigation data fusion precision under the conditions of process uncertainty of the system model and measurement information loss is improved.

Description

Combined navigation data fusion method based on robust volume point updating framework
Technical Field
The invention relates to the field of integrated navigation and information fusion, in particular to an integrated navigation data fusion method based on a robust volume point updating framework.
Background
In integrated navigation systems, Extended Kalman Filters (EKFs), Unscented Kalman Filters (UKF), volumetric kalman filters (CKF) are often used for data fusion. The EKF utilizes Taylor series expansion to carry out simple local linearization on a nonlinear system equation, and the linearized system has serious model description errors; the UKF utilizes a group of selected probability distribution of sigma point approximate states to overcome errors caused by local linearization of an EKF algorithm, however, the numerical instability of the UKF algorithm is caused because the weight value of the central point of unscented transformation is possibly negative; compared with UKF, CKF has better numerical stability, while the CKF algorithm is derived based on the third-order volume criterion, and can only ensure the third-order approximation precision, thus being not suitable for application scenes with higher precision requirements; the high-order CKF algorithm has higher estimation precision compared with the traditional CKF algorithm. In practical applications, the integrated navigation system model is usually a theoretical approximation of the real system model, and especially in some integrated navigation systems with high real-time requirements, the theoretical approximation model has inevitable process uncertainty. In addition, the combined navigation system usually suffers from loss of measurement information due to the influence of external environment. The uncertainty and the measurement information loss of the integrated navigation system process can cause the serious performance reduction of the integrated navigation data fusion algorithm, thereby causing the reduction of the navigation precision of the integrated navigation system and even the unavailability of the navigation information. Therefore, how to realize high-precision fusion of the combined navigation system data under the above situation is a problem which needs to be solved urgently.
Disclosure of Invention
The purpose of the invention is as follows: in order to overcome the defects of the prior art, the invention provides a combined navigation data fusion method based on a robust volumetric point updating frame, which solves the problem that the navigation precision of a combined navigation system is reduced under the conditions that the combined navigation system has uncertainty and measurement information is lost.
The technical scheme is as follows: the invention discloses a combined navigation data fusion method based on a robust volume point updating frame, which comprises the following steps:
(1) constructing the measured conditional probability density by utilizing the statistical information of the innovation vector, and then introducing an estimation window with fixed length memory into a covariance matrix of the estimation process noise in a maximum likelihood criterion;
(2) approximating the likelihood function by using a predicted volume point error matrix of the likelihood function and a posterior volume point error matrix obtained by linear transformation of a model predicted residual error, and directly updating posterior volume points, thereby constructing a novel volume point updating frame;
(3) and (3) integrating the covariance matrix of the process noise estimated in the step (1) and the volume point updating strategy described in the step (2) into a high-order volume Kalman filtering framework to obtain the combined navigation data fusion method with higher robustness and precision.
Preferably, the specific operation method of step (1) is as follows:
first, a quantity z is measured k The conditional probability density function of (a) is expressed as:
Figure BDA0003673960420000021
where k denotes the time of day, Q denotes the process noise covariance matrix, z denotes the quantity measurement,
Figure BDA0003673960420000022
a predicted value representing the amount of measurement at time k,
Figure BDA0003673960420000023
an innovation vector representing the time instant k,
Figure BDA0003673960420000024
the measurement-representative prediction error covariance matrix is measured, and m represents the dimension of the measurement.
Based on algebraic operation, a measurement vector (z) is calculated by equation (44) k-N+1 ,z k-N+2 ,…,z k ) The likelihood function of (2).
Figure BDA0003673960420000025
Where N represents the size of the fixed length memory estimation window.
Finally, according to the maximum likelihood criterion, the covariance matrix Q of the process noise can be obtained by solving the following optimization problem
Figure BDA0003673960420000026
Preferably, the solution process of the optimization problem of equation (45) is:
by taking the logarithm of both sides of equation (44) and neglecting the constant term, equation (45) can be further rewritten as:
Figure BDA0003673960420000027
to facilitate solving the optimization problem shown in equation (46), the following cost function is defined.
Figure BDA0003673960420000028
The partial derivatives for each element of Q are calculated for the cost function and made equal to 0, resulting in the following maximum likelihood equation:
Figure BDA0003673960420000029
where i, l ═ 1,2, …, n, n denotes the dimension of the state quantity, tr (·) denotes the trace operation of the matrix, Q il Representing the ith row and jth column element in Q.
By combining the estimated values of state quantities of navigation system for k-1 time
Figure BDA0003673960420000031
With taylor expansion, the prediction error of the state quantity at the time k can be expressed as:
Figure BDA0003673960420000032
wherein the content of the first and second substances,
Figure BDA0003673960420000033
representing the second and higher order terms in the taylor expansion coefficients,
Figure BDA0003673960420000034
representing the state quantity estimation error of the combined navigation system at the moment k-1, w k Representing process noise of the integrated navigation system.
By introducing a diagonal matrix beta k ＝diag(β 1,k ,β 2,k ,…,β n,k ) To represent the first order linearity error in equation (49), equation (49) can be rewritten as:
Figure BDA0003673960420000035
further, the prediction covariance matrix of the state quantity error can be expressed as:
Figure BDA0003673960420000036
pair of equation (51) and the measured prediction error covariance matrix
Figure BDA0003673960420000037
Solving for Q il Partial derivatives of (a). When the filtering process within the estimation window reaches a steady state,
Figure BDA0003673960420000038
the first order term of the result can then be ignored, and one can then get:
Figure BDA0003673960420000039
wherein H k Representing an observation matrix.
Bringing formula (52) into formula (48) yields:
Figure BDA00036739604200000310
wherein l, i is 1,2, …, n.
According to filter gain formula
Figure BDA00036739604200000311
Formula (53) can be further rewritten as:
Figure BDA0003673960420000041
wherein l, i is 1,2, …, n.
It is assumed that the filtering processes within the estimation window are all in a steady state such that
Figure BDA0003673960420000042
May be approximated as a constant for all j (k-N +1, k-N +2, …, k). Therefore, equation (54) can be rewritten as:
Figure BDA0003673960420000043
where l, i is 1,2, …, n, and when equation (56) is satisfied, equation (55) is always true.
Figure BDA0003673960420000044
From the kalman filtering process, there are:
Figure BDA0003673960420000045
Figure BDA0003673960420000046
Figure BDA0003673960420000047
by taking equations (57), (58) and (59) into equation (56), the maximum likelihood estimation Q of the process noise covariance matrix Q can be obtained ML 。
Figure BDA0003673960420000048
Preferably, the specific operation method of step (2) is as follows:
defining a system prior volume point error matrix
Figure BDA0003673960420000049
Error matrix of posterior volume point
Figure BDA00036739604200000410
And the corresponding weight matrix W is:
Figure BDA00036739604200000411
Figure BDA00036739604200000412
Figure BDA00036739604200000413
wherein the content of the first and second substances,
Figure BDA00036739604200000414
and
Figure BDA00036739604200000415
respectively representing a predicted value and an estimated value of the state quantity at the time k, X i,kk-1 ＝f(ξ i,k-1 ) F (-) denotes a combined navigation system function, ξ i,k-1 Representing volume points, ω i Represents a volume point weight, i is 1,2, …,2n 2 +1, diag (·) represents the diagonal operation of the matrix, and n represents the dimension of the state quantity.
Suppose that
Figure BDA0003673960420000051
Can pass through
Figure BDA0003673960420000052
Expressed, the following constraint equation holds.
Figure BDA0003673960420000053
Figure BDA0003673960420000054
Figure BDA0003673960420000055
Wherein the content of the first and second substances,
Figure BDA0003673960420000056
representing the state quantity prediction error covariance matrix at time k,
Figure BDA0003673960420000057
a covariance matrix representing the state quantity estimation error at the time k,
Figure BDA0003673960420000058
ΔR k a matrix of uncertainty caused by the noise is represented,
Figure BDA0003673960420000059
Λ k representing a scale matrix, K k Representing the filter gain and R the metric noise covariance matrix.
Under the constraints of equations (65) and (66), we can obtain:
Figure BDA00036739604200000510
wherein the content of the first and second substances,
Figure BDA00036739604200000511
chol (·) denotes georges decomposition.
Finally, according to equations (62) and (67), the new volume point generated can be expressed as:
Figure BDA00036739604200000512
preferably, the specific operation method of step (3) is as follows:
1) initializing a volume point
Initializing a process noise covariance matrix Q and a measured noise covariance matrix R, and let Q ML Q, the volume point and corresponding weight are then calculated according to equations (69) and (70).
Figure BDA00036739604200000513
Figure BDA00036739604200000514
Wherein, 0 n Representing a zero-column vector, I, of size n n The unit matrix is represented by a matrix of units,
Figure BDA00036739604200000515
e k and e l Respectively represent unit matrices I n The k-th column and the l-th column,
Figure BDA0003673960420000061
Figure BDA0003673960420000062
represent
Figure BDA0003673960420000063
Row 1 column matrix.
2) Time updating
Calculating the predicted value of the state quantity of the integrated navigation system
Figure BDA0003673960420000064
And corresponding error covariance matrix
Figure BDA0003673960420000065
Figure BDA0003673960420000066
Figure BDA0003673960420000067
Then, a prior volumetric point error matrix is calculated
Figure BDA0003673960420000068
X i,kk-1 ＝f(ξ i,k-1 ),i＝1,2,…,2n 2 +1 (73)
Figure BDA0003673960420000069
According to formula (75) pair
Figure BDA00036739604200000610
And (6) updating.
Figure BDA00036739604200000611
Propagation volume points for approximating the likelihood function are generated according to equation (76).
Figure BDA00036739604200000612
Wherein the content of the first and second substances,
Figure BDA00036739604200000613
to represent
Figure BDA00036739604200000614
Column i.
3) Measurement update
Predicted value of calculated quantity measurement
Figure BDA00036739604200000615
Measuring prediction error covariance matrix
Figure BDA00036739604200000616
And a cross-covariance matrix between the state quantity predicted value and the quantity measurement predicted value
Figure BDA00036739604200000617
Figure BDA00036739604200000618
Figure BDA00036739604200000619
Figure BDA00036739604200000620
Wherein H k Which represents the observation matrix, is shown,
then, the estimated value of the state quantity
Figure BDA0003673960420000071
And corresponding covariance matrix
Figure BDA0003673960420000072
And (6) updating.
Figure BDA0003673960420000073
Figure BDA0003673960420000074
Figure BDA0003673960420000075
4) Volumetric point update
Calculating a posteriori volumetric point error matrix according to equation (83)
Figure BDA0003673960420000076
Figure BDA0003673960420000077
Then, volume points for k +1 times filtering are generated according to equation (84).
Figure BDA0003673960420000078
Wherein the content of the first and second substances,
Figure BDA0003673960420000079
to represent
Figure BDA00036739604200000710
Column i.
Finally, let k be k +1, and return to step 2) to execute the next time, i.e. the next filtering cycle.
Has the advantages that: compared with the prior art, the invention has the following remarkable advantages: 1. the invention discloses an integrated navigation data fusion method based on a robust volume point updating frame, which solves the problem that the navigation precision of an integrated navigation system is reduced under the conditions of process uncertainty and measurement information loss of the integrated navigation system; 2. on the premise of not changing the hardware structure of the integrated navigation system, the stability and the navigation precision of the integrated navigation system are improved in a software mode.
Drawings
Fig. 1 is a schematic diagram of the operation of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are only one embodiment of the present invention, and not all embodiments. All other embodiments that can be derived by a person skilled in the art from the detailed description of the invention without inventive step are within the scope of the invention.
As shown in fig. 1, the invention discloses a multi-frequency INS/CNS integrated navigation method based on artificial intelligence, comprising the steps of:
(1) the measured conditional probability density is constructed using the statistics of the innovation vector, and then an estimation window of fixed length memory is introduced into the maximum likelihood criterion to estimate the covariance matrix of the process noise. First, a quantity z is measured k Is expressed as:
Figure BDA0003673960420000081
where k denotes the time of day, Q denotes the process noise covariance matrix, z denotes the quantity measurement,
Figure BDA0003673960420000082
a predicted value representing the amount of measurement at time k,
Figure BDA0003673960420000083
an innovation vector representing the time instant k,
Figure BDA0003673960420000084
the measurement-representative prediction error covariance matrix is measured, and m represents the dimension of the measurement.
Based on algebraic operation, a measurement vector (z) is calculated by equation (86) k-N+1 ,z k-N+2 ,…,z k ) The likelihood function of (2).
Figure BDA0003673960420000085
Where N represents the size of the fixed length memory estimation window.
Finally, the solution of the covariance matrix Q of the process noise can be transformed into the solution of the following optimization problem according to the maximum likelihood criterion,
Figure BDA0003673960420000086
by taking the logarithm of both sides of equation (86) and neglecting the constant term, equation (87) can be further rewritten as:
Figure BDA0003673960420000087
to facilitate solving the optimization problem shown in equation (88), the following cost function is defined.
Figure BDA0003673960420000088
The partial derivatives for each element of Q are calculated for the cost function and made equal to 0, resulting in the following maximum likelihood equation:
Figure BDA0003673960420000089
where i, l is 1,2, …, n, n represents the dimension of the state quantity, and tr (·) represents the trace operation of the matrix，Q il Representing the ith row and jth column element in Q.
By combining the estimated values of state quantities of navigation system for k-1 time
Figure BDA00036739604200000810
With taylor expansion, the prediction error of the state quantity at the time k can be expressed as:
Figure BDA0003673960420000091
wherein the content of the first and second substances,
Figure BDA0003673960420000092
representing the second and higher order terms in the taylor expansion coefficients,
Figure BDA0003673960420000093
representing the state quantity estimation error of the combined navigation system at the moment k-1, w k Representing process noise of the integrated navigation system.
By introducing a diagonal matrix beta k ＝diag(β 1,k ,β 2,k ,…,β n,k ) To represent the first order linearity error in equation (91), equation (91) can be rewritten as:
Figure BDA0003673960420000094
further, the prediction covariance matrix of the state quantity error can be expressed as:
Figure BDA0003673960420000095
pair formula (93) and quantity measurement prediction error covariance matrix
Figure BDA0003673960420000096
Solving for Q il Partial derivatives of (a). When the filtering process within the estimation window reaches a steady state,
Figure BDA0003673960420000097
the first order term of the result can then be ignored, and one can then get:
Figure BDA0003673960420000098
wherein H k Representing an observation matrix.
Bringing formula (94) into formula (90) yields:
Figure BDA0003673960420000099
where l, i is 1,2, …, n.
According to filter gain formula
Figure BDA00036739604200000910
Formula (95) can be further rewritten as:
Figure BDA00036739604200000911
wherein l, i is 1,2, …, n.
It is assumed that the filtering processes within the estimation window are all in a steady state such that
Figure BDA0003673960420000101
It may be approximated as a constant for all j (k-N +1, k-N +2, …, k). Therefore, equation (96) can be rewritten as:
Figure BDA0003673960420000102
where l, i is 1,2, …, n, and when expression (98) is satisfied, expression (97) is always true.
Figure BDA0003673960420000103
From the kalman filtering process, there are:
Figure BDA0003673960420000104
Figure BDA0003673960420000105
Figure BDA0003673960420000106
the maximum likelihood estimation Q of the process noise covariance matrix Q can be obtained by taking equations (99), (100) and (101) into equation (98) ML ，
Figure BDA0003673960420000107
(2) The likelihood function is approximated by the predicted volume point error matrix of the likelihood function and the posterior volume point error matrix obtained by the linear transformation of the model predicted residual error, and the likelihood function is directly used for updating the posterior volume point, thereby constructing a novel volume point updating frame,
defining a system prior volume point error matrix
Figure BDA0003673960420000108
Posterior volume point error matrix
Figure BDA0003673960420000109
And the corresponding weight matrix W is:
Figure BDA00036739604200001010
Figure BDA00036739604200001011
Figure BDA00036739604200001012
wherein the content of the first and second substances,
Figure BDA00036739604200001013
a predicted value X representing the state quantity at time k i,kk-1 ＝f(ξ i,k-1 ) F (-) denotes a combined navigation system function, ξ i,k-1 Representing volume points, ω i Represents a volume point weight, i is 1,2, …,2n 2 +1, n represents the dimension of the state quantity, and diag (·) represents the diagonal operation of the matrix.
Suppose that
Figure BDA0003673960420000111
Can pass through
Figure BDA0003673960420000112
Expressed, the following constraint equation holds.
Figure BDA0003673960420000113
Figure BDA0003673960420000114
Figure BDA0003673960420000115
Wherein the content of the first and second substances,
Figure BDA0003673960420000116
representing the state quantity prediction error covariance matrix at time k,
Figure BDA0003673960420000117
a covariance matrix representing the state quantity estimation error at time k,
Figure BDA0003673960420000118
ΔR k representing a matrix of uncertainty caused by the noise,
Figure BDA0003673960420000119
Λ k representing a scale matrix and R representing a measurement noise covariance matrix.
Under the constraints of equations (107) and (108), we can obtain:
Figure BDA00036739604200001110
wherein the content of the first and second substances,
Figure BDA00036739604200001111
chol (·) denotes georges decomposition.
Finally, according to equations (104) and (109), the new volume point generated can be expressed as:
Figure BDA00036739604200001112
(3) and (3) integrating the covariance matrix of the process noise estimated in the step (1) and the volume point updating strategy described in the step (2) into a high-order volume Kalman filtering framework to obtain the combined navigation data fusion method with higher robustness and precision.
1) Initializing a volume point
Initializing a process noise covariance matrix Q and a measured noise covariance matrix R, and let Q ML Then, the volume point and the weight corresponding to the volume point are calculated according to equations (111) and (112).
Figure BDA00036739604200001113
Figure BDA00036739604200001114
Wherein, 0 n Representing a zero-column vector, I, of size n n The matrix of the unit is expressed by,
Figure BDA0003673960420000121
e k and e l Respectively represent unit matrices I n The k-th column and the l-th column,
Figure BDA0003673960420000122
Figure BDA0003673960420000123
to represent
Figure BDA0003673960420000124
Row 1 column matrix.
2) Time updating
Calculating the predicted value of the state quantity of the integrated navigation system
Figure BDA0003673960420000125
And corresponding error covariance matrix
Figure BDA0003673960420000126
Figure BDA0003673960420000127
Figure BDA0003673960420000128
Then, a prior volumetric point error matrix is calculated
Figure BDA0003673960420000129
X i,kk-1 ＝f(ξ i,k-1 ),i＝1,2,…,2n 2 +1 (115)
Figure BDA00036739604200001210
According to formula (117) pair
Figure BDA00036739604200001211
And (6) updating.
Figure BDA00036739604200001212
Propagation volume points for approximating the likelihood function are generated according to equation (118).
Figure BDA00036739604200001213
Wherein the content of the first and second substances,
Figure BDA00036739604200001214
represent
Figure BDA00036739604200001215
Column i.
3) Measurement update
Predicted value of calculated quantity measurement
Figure BDA00036739604200001216
Measuring prediction error covariance matrix
Figure BDA00036739604200001217
And a cross-covariance matrix between the state quantity predicted value and the quantity measurement predicted value
Figure BDA00036739604200001218
Figure BDA00036739604200001219
Figure BDA00036739604200001220
Figure BDA0003673960420000131
H k A representation of an observation matrix is shown,
then, an estimation value of the state quantity is obtained
Figure BDA0003673960420000132
And corresponding covariance matrix
Figure BDA0003673960420000133
And (6) updating.
Figure BDA0003673960420000134
Figure BDA0003673960420000135
Figure BDA0003673960420000136
Wherein, K k Representing the filter gain, z k The measured values are indicated.
4) Volumetric point update
Calculating a posteriori volumetric point error matrix according to equation (125)
Figure BDA0003673960420000137
Figure BDA0003673960420000138
Then, volume points for k +1 times filtering are generated according to equation (126).
Figure BDA0003673960420000139
Wherein the content of the first and second substances,
Figure BDA00036739604200001310
represent
Figure BDA00036739604200001311
Column i.
And finally, k is made to be k +1, and the step 2) is returned to execute the next filtering period.
The above-mentioned embodiments, objects, technical solutions and advantages of the present invention are further described in detail, it should be understood that the above-mentioned embodiments are only illustrative of the present invention and are not intended to limit the present invention, and any modifications, equivalents, improvements and the like made within the spirit and principle of the present invention should be included in the protection scope of the present invention.

Claims (7)

1. A combined navigation data fusion method based on a robust volume point updating frame is characterized by comprising the following steps:
(1) constructing the measured conditional probability density by utilizing the statistical information of the innovation vector, and then introducing an estimation window with fixed length memory into a covariance matrix of the estimation process noise in a maximum likelihood criterion;
(2) approximating the likelihood function by using a predicted volume point error matrix of the likelihood function and a posterior volume point error matrix obtained by linear transformation of a model predicted residual error, and directly updating posterior volume points, thereby constructing a novel volume point updating frame;
(3) and (3) integrating the process noise covariance matrix estimated in the step (1) and the volume point updating strategy described in the step (2) into a high-order volume Kalman filtering framework to obtain an updated integrated navigation data fusion method.
2. The robust volumetric point update framework based integrated navigation data fusion method of claim 1, wherein the step (1) comprises the steps of:
first, a quantity z is measured k The conditional probability density function of (a) is expressed as:
Figure FDA0003673960410000011
where k represents time of day, Q represents a process noise covariance matrix, z represents a quantity measurement,
Figure FDA0003673960410000012
a predicted value representing the amount of measurement at time k,
Figure FDA0003673960410000013
an innovation vector representing the time instant k,
Figure FDA0003673960410000014
representing the measured prediction error covariance matrix, m representing the measured dimension,
according to algebraic operation, obtaining measurement vector (z) by formula (2) k-N+1 ,z k-N+2 ,…,z k ) The likelihood function of (a) is,
Figure FDA0003673960410000015
wherein N represents the size of the fixed length memory estimation window,
finally, the covariance matrix Q of the process noise is obtained by equation (3) according to the maximum likelihood criterion,
Figure FDA0003673960410000016
3. the robust volumetric point update framework based integrated navigation data fusion method of claim 2, wherein the step (2) comprises the steps of:
defining a system prior volume point error matrix
Figure FDA0003673960410000017
Error matrix of posterior volume point
Figure FDA0003673960410000018
And the corresponding weight matrix W is:
Figure FDA0003673960410000021
Figure FDA0003673960410000022
Figure FDA0003673960410000023
wherein the content of the first and second substances,
Figure FDA0003673960410000024
and
Figure FDA0003673960410000025
respectively representing a predicted value and an estimated value of the state quantity at the time k, X i,k|k-1 ＝f(ξ i,k-1 ) F (-) denotes a combined navigation system function, ξ i,k-1 Representing volume points, ω i Represents a volume point weight, i is 1,2, …,2n 2 +1, n represents the dimension of the state quantity, and diag (·) represents the diagonal operation of the matrix.
4. The robust volumetric point update framework based integrated navigation data fusion method of claim 3,
Figure FDA0003673960410000026
by passing
Figure FDA0003673960410000027
Indicating that the following constraint equation holds true,
Figure FDA0003673960410000028
Figure FDA0003673960410000029
Figure FDA00036739604100000210
wherein the content of the first and second substances,
Figure FDA00036739604100000211
representing the state quantity prediction error covariance matrix at time k,
Figure FDA00036739604100000212
a covariance matrix representing the state quantity estimation error at time k,
Figure FDA00036739604100000213
ΔR k a matrix of uncertainty caused by the noise is represented,
Figure FDA00036739604100000214
Λ k representing a scale matrix, R representing a measurement noise covariance matrix,
under the constraints of equations (8) and (9), we obtain:
Figure FDA00036739604100000215
wherein the content of the first and second substances,
Figure FDA00036739604100000216
chol (·) denotes georges decomposition,
finally, according to equations (5) and (10), the new volume point is generated as:
Figure FDA00036739604100000217
5. the robust volumetric point update framework based integrated navigation data fusion method of claim 4, wherein the step (3) comprises the steps of:
1) initializing a volume point
Initializing a process noise covariance matrix Q and a measured noise covariance matrix R, and let Q ML Then, the volume points and the corresponding weights of the volume points are calculated according to the equations (12) and (13),
Figure FDA0003673960410000031
Figure FDA0003673960410000032
wherein, 0 n Representing a zero-column vector, I, of size n n The unit matrix is represented by a matrix of units,
Figure FDA0003673960410000033
e k and e l Respectively represent unit matrices I n The k-th column and the l-th column,
Figure FDA0003673960410000034
Figure FDA0003673960410000035
to represent
Figure FDA00036739604100000318
A matrix of rows and columns;
2) time updating
Calculating the predicted value of the state quantity of the integrated navigation system
Figure FDA0003673960410000036
And corresponding error covariance matrix
Figure FDA0003673960410000037
Figure FDA0003673960410000038
Figure FDA0003673960410000039
Then, a prior volumetric point error matrix is calculated
Figure FDA00036739604100000310
Figure FDA00036739604100000311
Figure FDA00036739604100000312
According to formula (18) pair
Figure FDA00036739604100000313
The updating is carried out, and the updating is carried out,
Figure FDA00036739604100000314
propagation volume points for approximating the likelihood function are generated according to equation (19),
Figure FDA00036739604100000315
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA00036739604100000316
to represent
Figure FDA00036739604100000317
Column i.
6. The method for integrating navigation data based on robust volume point update framework according to claim 5, wherein after performing initialization volume point and time update, measurement update and volume point update are required:
3) measurement update
Predicted value of calculated quantity measurement
Figure FDA0003673960410000041
Measuring prediction error covariance matrix
Figure FDA0003673960410000042
And a cross-covariance matrix between the state quantity predicted value and the quantity measurement predicted value
Figure FDA0003673960410000043
Figure FDA0003673960410000044
Figure FDA0003673960410000045
Figure FDA0003673960410000046
Wherein H k A representation of an observation matrix is shown,
then, an estimation value of the state quantity is obtained
Figure FDA0003673960410000047
And corresponding covariance matrix
Figure FDA0003673960410000048
The updating is carried out, and the updating is carried out,
Figure FDA0003673960410000049
Figure FDA00036739604100000410
Figure FDA00036739604100000411
wherein, K k Representing the filter gain, z k The measured value is expressed as a value of measurement,
4) volume point update
Calculating a posteriori volumetric point error matrix according to equation (26)
Figure FDA00036739604100000412
Figure FDA00036739604100000413
Then, a volume point for k +1 times filtering is generated according to equation (27),
Figure FDA00036739604100000414
wherein the content of the first and second substances,
Figure FDA00036739604100000415
to represent
Figure FDA00036739604100000416
The (ii) th column element of (ii),
finally, let k be k +1, and return to step 2) to execute the next filtering cycle.
7. The method for integrating navigation data based on robust volumetric point update framework according to claim 2, wherein when solving equation (3):
by taking the logarithm operation on both sides of equation (2) and neglecting the constant term, equation (3) is further rewritten as:
Figure FDA0003673960410000051
to facilitate the solution of the optimization problem shown in equation (28), a cost function is defined,
Figure FDA0003673960410000052
the partial derivatives for each element of Q are calculated for the cost function and made equal to 0, resulting in the following maximum likelihood equation:
Figure FDA0003673960410000053
wherein i, l ═ 1,2, …, n, tr (·) denotes the trace operation of the matrix, Q il Representing the ith row and jth column element in Q,
by combining the estimated values of state quantities of navigation system for k-1 time
Figure FDA0003673960410000054
Taylor expansion is performed, and the prediction error of the state quantity at the time k is represented as:
Figure FDA0003673960410000055
wherein, the first and the second end of the pipe are connected with each other,
Figure FDA0003673960410000056
Figure FDA0003673960410000057
representing the second and higher order terms in the taylor expansion coefficients,
Figure FDA0003673960410000058
representing the state quantity estimation error of the integrated navigation system at the moment k-1, w k Representing the process noise of the integrated navigation system,
by introducing a diagonal matrix beta k ＝diag(β 1,k ,β 2,k ,…,β n,k ) To represent the first order linearity error in equation (31), equation (31) is expressed as:
Figure FDA0003673960410000059
the prediction covariance matrix of the state quantity error is expressed as:
Figure FDA00036739604100000510
measure the prediction error covariance matrix for equation (33) and quantity
Figure FDA00036739604100000511
Solving for Q il The partial derivative of (c), when the filtering process within the estimation window reaches a steady state,
Figure FDA0003673960410000061
the first order term of the result is ignored, and then:
Figure FDA0003673960410000062
wherein H k A representation of an observation matrix is shown,
bringing equation (34) into equation (30) yields:
Figure FDA0003673960410000063
wherein l, i is 1,2, …, n,
according to filter gain formula
Figure FDA0003673960410000064
Formula (35) is rewritten as:
Figure FDA0003673960410000065
wherein l, i is 1,2, …, n,
the filtering processes in the estimation window are all in a stable state, so that
Figure FDA0003673960410000066
For all j (k-N +1, k-N +2, …, k) being approximately constant, equation (36) is rewritten as:
Figure FDA0003673960410000067
wherein l, i is 1,2, …, n, and when equation (38) is satisfied, equation (37) is constantly true,
Figure FDA0003673960410000068
from the kalman filtering process, there are:
Figure FDA0003673960410000069
Figure FDA00036739604100000610
Figure FDA00036739604100000611
by taking equations (39), (40) and (41) into equation (38), the maximum likelihood estimation value Q of the process noise covariance matrix Q can be obtained ML ，
Figure FDA0003673960410000071
CN202210615134.9A 2022-06-01 2022-06-01 Combined navigation data fusion method based on robust volume point updating framework Pending CN115014325A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210615134.9A CN115014325A (en) 2022-06-01 2022-06-01 Combined navigation data fusion method based on robust volume point updating framework

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210615134.9A CN115014325A (en) 2022-06-01 2022-06-01 Combined navigation data fusion method based on robust volume point updating framework

Publications (1)

Publication Number Publication Date
CN115014325A true CN115014325A (en) 2022-09-06

Family

ID=83071014

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210615134.9A Pending CN115014325A (en) 2022-06-01 2022-06-01 Combined navigation data fusion method based on robust volume point updating framework

Country Status (1)

Country Link
CN (1) CN115014325A (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608863A (en) * 2023-07-17 2023-08-18 齐鲁工业大学(山东省科学院) Combined navigation data fusion method based on Huber filtering update framework
CN117451043A (en) * 2023-12-25 2024-01-26 武汉大学 Multi-source fusion positioning method and system for digital-analog hybrid estimation

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608863A (en) * 2023-07-17 2023-08-18 齐鲁工业大学(山东省科学院) Combined navigation data fusion method based on Huber filtering update framework
CN116608863B (en) * 2023-07-17 2023-09-22 齐鲁工业大学(山东省科学院) Combined navigation data fusion method based on Huber filtering update framework
CN117451043A (en) * 2023-12-25 2024-01-26 武汉大学 Multi-source fusion positioning method and system for digital-analog hybrid estimation
CN117451043B (en) * 2023-12-25 2024-03-15 武汉大学 Multi-source fusion positioning method and system for digital-analog hybrid estimation

Similar Documents

Publication Publication Date Title
CN115014325A (en) Combined navigation data fusion method based on robust volume point updating framework
CN109612738B (en) Distributed filtering estimation method for improving gas path performance of turbofan engine
CN108225337A (en) Star sensor and Gyro method for determining posture based on SR-UKF filtering
CN103927436A (en) Self-adaptive high-order volume Kalman filtering method
CN108197725B (en) Water demand prior information-based water supply network node water demand checking method
CN107123265B (en) Highway traffic state estimation method based on parallel computation
CN110225454B (en) Confidence transfer distributed type volume Kalman filtering cooperative positioning method
CN110987068B (en) Data fusion method for multi-sensor integrated control system
CN109341690B (en) Robust and efficient combined navigation self-adaptive data fusion method
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN111623764B (en) Micro-nano satellite attitude estimation method
CN116734864B (en) Autonomous relative navigation method for spacecraft under constant observed deviation condition
CN112782732B (en) Navigation signal analysis method based on particle swarm algorithm and computer readable medium
CN110186482B (en) Method for improving drop point precision of inertial guidance spacecraft
CN107421543B (en) Implicit function measurement model filtering method based on state dimension expansion
CN112632454A (en) MEMS gyro filtering method based on adaptive Kalman filtering algorithm
CN111578931B (en) High-dynamic aircraft autonomous attitude estimation method based on online rolling time domain estimation
CN113074753A (en) Star sensor and gyroscope combined attitude determination method, combined attitude determination system and application
CN113432608A (en) Generalized high-order CKF algorithm based on maximum correlation entropy and suitable for INS/CNS integrated navigation system
CN112987054B (en) Method and device for calibrating SINS/DVL combined navigation system error
Slika et al. A practical polynomial chaos Kalman filter implementation using nonlinear error projection on a reduced polynomial chaos expansion
CN113608442A (en) State estimation method of nonlinear state model system based on characteristic function
CN113702838A (en) Lithium ion battery state of charge estimation method based on disturbance observer
CN112698368B (en) Navigation signal analysis method of navigation receiver and computer readable medium
CN115377977B (en) High-precision state estimation system and method for active power distribution network containing zero injection node

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