CN113188566A - Airborne distributed POS data fusion method - Google Patents
Airborne distributed POS data fusion method Download PDFInfo
- Publication number
- CN113188566A CN113188566A CN202110309944.7A CN202110309944A CN113188566A CN 113188566 A CN113188566 A CN 113188566A CN 202110309944 A CN202110309944 A CN 202110309944A CN 113188566 A CN113188566 A CN 113188566A
- Authority
- CN
- China
- Prior art keywords
- measurement
- node
- sub
- matrix
- attitude
- 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.)
- Granted
Links
- 238000007500 overflow downdraw method Methods 0.000 title claims abstract description 18
- 238000005259 measurement Methods 0.000 claims abstract description 213
- 239000011159 matrix material Substances 0.000 claims abstract description 119
- 239000013598 vector Substances 0.000 claims abstract description 76
- 230000004927 fusion Effects 0.000 claims abstract description 69
- 238000000034 method Methods 0.000 claims abstract description 40
- 238000012937 correction Methods 0.000 claims abstract description 39
- 238000001914 filtration Methods 0.000 claims abstract description 30
- 238000005070 sampling Methods 0.000 claims abstract description 19
- 238000004364 calculation method Methods 0.000 claims description 48
- 238000012546 transfer Methods 0.000 claims description 17
- 230000008569 process Effects 0.000 claims description 10
- 230000009466 transformation Effects 0.000 claims description 10
- 238000006243 chemical reaction Methods 0.000 claims description 6
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 239000000835 fiber Substances 0.000 claims description 6
- 101100272279 Beauveria bassiana Beas gene Proteins 0.000 claims description 3
- 238000003491 array Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000009827 uniform distribution Methods 0.000 claims description 3
- 235000006629 Prosopis spicigera Nutrition 0.000 claims 1
- 240000000037 Prosopis spicigera Species 0.000 claims 1
- 230000002411 adverse Effects 0.000 abstract description 3
- 230000005540 biological transmission Effects 0.000 abstract description 3
- 230000000694 effects Effects 0.000 abstract description 3
- 238000003384 imaging method Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
- G01C23/005—Flight directors
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The invention provides an airborne distributed POS data fusion method, which specifically comprises the following steps: calculating redundant position and attitude information of the positions of the child nodes; calculating a measurement vector of the sub-node, and generating a bootstrap measurement set by using a measurement bootstrap strategy; sampling elements of the bootstrap measurement set by using a Metropolis-Hastings sampling criterion to obtain an effective measurement set; calculating the weight and the measurement noise matrix of each effective measurement vector in the effective measurement set; performing data fusion on the child nodes by using sequential filtering, and correcting the motion parameters of the child nodes by using a fusion result; and respectively repeating the steps by other child nodes which do not carry out data fusion. The method utilizes the motion information of all the sub-nodes, reduces the adverse effect of the uncertainty of the measurement noise on the transmission alignment estimation precision, and uses the data fusion result for the correction of the motion parameters, thereby improving the estimation precision of the motion parameters of each sub-node.
Description
Technical Field
The invention relates to the technical field of airborne multi-task remote sensing load node information fusion, in particular to a data fusion method of an airborne distributed Position and attitude measurement System (POS), which can be used for improving the estimation precision of airborne distributed POS sub-node motion parameters.
Background
The Position and Orientation System (POS) is the main means for acquiring Position, velocity and Orientation information of a vehicle. By utilizing the information, the remote sensing data can be corrected, and the imaging quality is improved. Therefore, POS is often applied to the fields of aerial remote sensing imaging, aerial photogrammetry, and the like. For example, remote sensing loads such as a POS assisted Synthetic Aperture Radar (SAR), a laser radar, a multispectral scanner, a digital camera, a large field-of-view infrared scanner, etc. are used to meet the requirement of high precision motion imaging.
With the development of the flight platform technology, one aircraft starts to be provided with a plurality of or a plurality of remote sensing devices to realize synchronous observation of multiple observation windows, and typically, the SAR, the visible light camera, the imaging spectrometer and the laser radar work simultaneously, and the array antenna SAR with a plurality of antennas working simultaneously is provided. Such a multitask remote sensing mode integrating multiple or multiple remote sensing loads has become an important development direction of the aerial remote sensing system. Since a plurality of observation loads are installed at different positions of the carrier, the requirement for measuring the position and posture information of multiple nodes cannot be satisfied by using the conventional single POS. Therefore, in practical use, a plurality of Inertial Measurement Units (IMUs) are installed on the aircraft to form a distributed position and attitude Measurement system, i.e. a distributed POS, so as to realize Measurement of multi-node motion parameters.
The distributed POS is generally composed of one or a small number of high-precision main POS and a plurality of medium/low-precision sub IMUs, wherein the two parts are respectively arranged near remote sensing loads on two sides of a machine body or a wing to form a distributed multi-sensor system. The position of the main POS is called a main node, and the position of each sub IMU is called a sub node.
In order to realize accurate measurement of motion information at each sub-node in the distributed POS, the sub-node needs to correct the result of self strapdown calculation according to the motion information such as high-precision position, speed, attitude and the like provided by the main POS, and the result is used as the final motion parameter of the point, and the process is called transfer alignment. Due to the difference of factors such as body deformation and lever arms at each sub-node, after one-to-one transmission alignment from the main node to each sub-node is completed, the precision of motion parameters at each sub-node is different and has different heights. Particularly, the flexural deformation of the sub-nodes far away from the center of the body is the most complicated, and the transfer alignment accuracy is low. If each subnode can use the motion information of other nodes to perform data fusion, the motion parameter precision of each subnode is further improved.
The current data fusion algorithm for multi-sensor systems can be divided into two types: the first method is centralized fusion, which completes the measurement data of all sensors by one-time fusion calculation, and is proved to be the globally optimal system with the minimum information loss, but for a multi-sensor system such as an airborne distributed POS, multiplication and inversion of a high-dimensional matrix are involved at the time of fusion, and if the fusion method is adopted in the airborne distributed POS, the fusion method has the disadvantages of large calculation load, low real-time performance and low fault tolerance. The second method is distributed fusion, in which the raw data information of multiple sensors is filtered by multiple filters, and then processed by a fusion center. For example, patent No. CN201810153913.5 adopts a distributed fusion method, and the inverse of the covariance matrix obtained by transferring and aligning the sub-IMU is used as the weight matrix for data fusion, and a specific method for fusing position, velocity, and attitude information is provided. However, in the method, the weight matrix with the total number of the subnodes is required to be calculated for data fusion, the calculation of the weight matrix is complex, inversion of the matrix is involved, the problem that the matrix is singular exists, and the total calculation amount is large. Meanwhile, the method does not consider the dependence of flexural motion between the sub-nodes, which also has adverse effect on the precision of the data fusion result.
Common structures used in distributed fusion are federal filtering and sequential filtering. Both distributed fusion architectures are based on kalman filtering. The federal filtering belongs to one of decentralized filtering methods, the structure of the federal filtering comprises a plurality of sub-filters and a main filter, and information distribution between the sub-filters and the main filter is realized by applying an information distribution principle. The federal filtering has the advantages of flexible design and good fault tolerance, and is widely applied to a combined navigation system. However, the federal filtering aims at the fusion of sensor data with complementarity of a plurality of measurement functions installed at a certain point, and each subnode in the airborne distributed POS is only provided with one subimu, so that a common reference system and a plurality of subsystems required by the use of the federal filtering cannot be formed for any subnode, namely, the federal filtering structure cannot be formed, and thus the federal filtering is not suitable for the data fusion of the airborne distributed POS. And the sequential filtering is to sequentially perform Kalman filtering on multiple groups of measurement data, and take the final result as a data fusion result. For airborne distributed POS, the motion parameter information of each sub-node can be converted to any other sub-node by using the deformation data measured by the fiber bragg grating sensor, namely, any sub-node can obtain the redundant motion parameter information converted from the rest sub-nodes to the sub-node, so that a plurality of measurement vectors are formed, the possibility of filtering and estimating the plurality of measurement vectors of each sub-node by adopting sequential filtering is provided, and the sequential filtering has the advantages of smaller calculated amount and simple structure.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the defects of the prior art are overcome, the airborne distributed POS data fusion method is provided, and the estimation precision of the motion parameters of each sub-node of the airborne distributed POS is improved.
The technical solution of the invention is as follows: an airborne distributed POS data fusion method. The method comprises the following specific steps:
(1) calculating tkRedundant position and attitude information at the time child node;
(2) calculating tkMeasuring vectors of the sub-nodes at the moment, and generating a bootstrap measurement set by using a measurement bootstrap strategy;
(3) sampling elements in the bootstrap measurement set by using a Metropolis-Hastings sampling criterion to obtain an effective measurement set of the child nodes;
(4) calculating the weight and the measurement noise matrix of each effective measurement vector in the effective measurement set;
(5) using sequential filtering to perform data fusion on the child node, and applying the fusion result to the child node tkCorrecting the motion parameters at the moment;
(6) repeating the steps (1) to (5) for other child nodes which are not subjected to data fusion and motion parameter correction until all child nodes finish tkData fusion and correction of motion parameters at the moment;
(7)tk=tk+1and (5) executing the steps (1) to (6) until the data fusion and the correction of the motion parameters of all the child nodes at all the time are completed.
Obtaining t in the step (1)kThe specific steps of the redundant position and attitude information at the time child node are as follows:
1) sub-node strapdown resolving
The definition of the relevant reference coordinate system includes: e is a terrestrial coordinate system; the navigation coordinate systems of the main node and the sub-nodes are northeast geographic coordinate systems, the navigation coordinate system of the main node is represented by n, and the navigation coordinate system and the calculation navigation coordinate system of the J-th sub-node are respectively represented by nJ and n′JJ is 1,2, …, N is the number of child nodes; the origin of the carrier coordinate system is the center of gravity of the carrier, the x-axis is right along the transverse axis of the carrier, the y-axis is forward along the longitudinal axis of the carrier, the z-axis is upward along the vertical axis of the carrier, and the coordinate system is fixed on the carrier and is called as a right-front upper carrier coordinate system, and b is usedm、bJAnd the carrier coordinate systems respectively represent a main node and a J-th sub-node, wherein J is 1,2, … and N.
Each child node is solved through strapdown to obtain tkThe position [ L ] of each timeJ λJ HJ]TAnd attitude [ psiJθJ γJ]TAnd velocity wherein ,LJ、λJ and HJRespectively representing the latitude, longitude and altitude of the J-th sub-node; psiJ、θJ and γJRespectively representing a course angle, a pitch angle and a roll angle of the J-th sub-node;andrespectively represent the J-th sub-node at nJEast speed, north speed, and sky speed under the tie.
2) Acquisition of redundant position and attitude information of child nodes
a) Acquisition of child node redundant location information
By mounting on a machineFiber grating sensor on wing, can obtain tkStrain information at any sub-node at any time, and further obtain the displacement of the sub-node from the initial position and relative position information Δ P between any two sub-nodes J, J '(J, J ≠ 1,2, …, N; J ≠ J')J←J′,ΔPJ←J′Representing the difference in latitude, longitude, and altitude between any two child nodes J, J'.
At tkAt the moment, the J-th child node is subjected to strapdown solution to obtain a position [ LJ λJ HJ]TJ-1, 2, …, N, and the other N-1 child nodes are designated as [ LJ′ λJ′ HJ′]T(J '═ 1,2, …, N, J' ≠ J). The redundant location information at the jth child node may be expressed as:
[LJ←J′ λJ←J′ HJ←J′]T=[LJ′ λJ′ HJ′]T+ΔPJ←J′(J,J′=1,2,…,N;J≠J′)
therefore, the J-th sub-node has N calculated values of latitude, longitude and altitude, namely [ L ] in total, in addition to the position information obtained by self strapdown calculationJ λJ HJ]T and [LJ←J′ λJ←J′ HJ←J′]T(J, J '═ 1,2, …, N, J' ≠ J). Position information [ L ] obtained by directly resolving the current child node J in a strapdown mannerJ λJ HJ]TIs newly recorded asRemaining N-1 location information [ LJ←J′ λJ←J′ HJ←J′]T(J, J '═ 1,2, …, N; J ≠ J') is reiterated
b) Acquisition of child node redundant attitude information
T measured by fiber grating sensorkThe angular displacement information of each sub-node at the moment can construct a conversion matrix between any two sub-nodes J, J '(J, J ≠ 1,2, …, N; J ≠ J') carrier coordinate systemsCarrier coordinate system at jth child node (b)JSystem) and the point navigation coordinate system (n)JSystem) as a transformation matrix (attitude matrix) betweenThe attitude matrix at the other N-1 child nodes is represented asThe redundant attitude matrix at the jth child node can be represented as:
wherein ,
at tkAt time, there are N attitude matrices at the J-th child node. Using the N attitude matrices, N attitude angles can be calculated. The specific calculation method of the attitude angle is as follows:
wherein ,TJxyIs a matrixThe x-th row and y-th column of the elementElement, x ═ 1,2,3, y ═ 1,2, 3; the attitude angle of the J-th sub-node, including the heading angle psiJAngle of pitch thetaJAnd roll angle γJPrincipal value of, i.e.Andrespectively as follows:
course angle psiJAngle of pitch thetaJAnd roll angle γJAre respectively defined as [0, 2 pi ]]、 [-π,+π](ii) a Then, ψJ、θJ and γJThe true value of (c) can be determined by:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the J-th sub-node can be calculatedCorresponding N-1 redundant attitude angles. The attitude matrix of the child node J itselfThe calculated attitude angle is recorded againFrom the rest N-1 attitude matricesThe attitude angle calculated is recorded as
Calculating t in the above step (2)kThe method comprises the following specific steps of measuring vectors of the sub-nodes at the moment and generating a bootstrap measurement set by using a measurement bootstrap strategy:
1) calculating a measurement vector for a child node
tkThe latitude, longitude and altitude of the time master node m after lever arm compensation are respectively recorded as Lm、λm、HmThe heading angle, pitch angle and roll angle of the master node m are respectively recorded as psim、θm、γm. T can be obtained for any sub-node J (J ═ 1,2, …, N)kN measured vector of timeThe expression is as follows:
wherein ,respectively represents tkThe ith latitude, longitude and altitude of the time sub-node J are different from the latitude, longitude and altitude of the master node m after lever arm compensation;respectively represents tkAnd the difference value of the ith course angle, the pitch angle and the roll angle of the sub-node J at the moment and the course angle, the pitch angle and the roll angle corresponding to the main node m.
2) Generating a bootstrap measurement set using a measurement bootstrap policy
In the measurement vectorBased on the measurement result, the bootstrap measurement is generated by increasing the disturbanceThe method comprises the following specific steps:
wherein ,representing the original measurement vectorThe c-th bootstrap measurement generated on the basis, c ═ 1,2, …, l, l denote bootstrap measurementsL is 5; hJIs a measurement matrix of the system, xJIs the state vector of the system, vJIs measurement noise, and vJWhite Gaussian noise satisfying zero mean with a covariance of and vJHaving the same statistical properties, i.e.White Gaussian noise satisfying zero mean, its covariance
By the method, NxL bootstrap measurement data can be obtainedDefining a bootstrap measurement set: wherein Representing the original measurement vector, i.e.
In the step (3), the Metropolis-Hastings sampling criterion is used to sample the elements in the bootstrap measurement set to obtain the effective measurement set of the child node, and the specific steps are as follows:
1) calculating confidence and acceptance probability
From N bootstrap measurements, a set was collected according to Metropolis-Hastings sampling principleIn the random selection of two setsThen randomly extracting an element from the two setsAndand calculating corresponding credibilityAndthe calculation formula of the reliability is as follows:
wherein Is a measure of the mean value of the prediction,representative of the measurement noise vJCovariance matrixDeterminant (c).
In obtaining confidence levelAndon the basis of (1), calculating the acceptance probability according to the following formula:
2) Constructing valid metrology collections
Firstly, a random number χ satisfying uniform distribution U (0,1) is generated, and an effective measurement set ΩJThe determination method of (2) is as follows:
in the formula, if the elements are randomly extractedAndif the corresponding acceptance probability is greater than or equal to the generated random number χ, then it will beAs an effective measurement set omegaJThe elements of (1); otherwise, willAs an effective measurement set omegaJOf (1). The above process is a sampling process for determining the effective measurement vector.
Repeating the sampling for L times, wherein L is 2N, and L effective measurement vectors in the effective measurement set are respectively defined as zJ(1),zJ(2),…zJ(L) these effective measurement vectors constitute an effective measurement set omegaJ。
The specific steps of calculating the weight of each effective measurement vector in the effective measurement set and measuring the noise matrix in the step (4) are as follows:
1) computing a coherence distance and a coherence matrix
Defining an effective metrology set omegaJAny two of the measurement vectors zJ(ξ) and zJThe confidence distance between (ζ) isThe calculation method is as follows:
calculating a consistency distance based thereonAnd the consistency matrix ΨJThe calculation method is as follows:
wherein ,represents omegaJThe maximum value of the confidence distance between any two measurement vectors.
2) Calculating weights of effective measurement vectors and measurement noise matrix
Obtain the consistency matrix ΨJThen, the maximum module eigenvalue lambda and the corresponding eigenvector beta of the matrix are calculated, and the beta is unitized to obtainOrder weight vectorAt this time, the process of the present invention,can be used to represent the corresponding valid measurement vector is valid measurement set omegaJThe overall degree of support of all elements in.
According to the basic principle of calculating the variance in the mathematical statistics, the measurement noise matrix corresponding to each element in the effective measurement set is calculatedThe calculation method is as follows:
in the step (5), the sequential filtering is used for carrying out data fusion on the child nodes, and the fusion result is used for the child nodes tkThe correction of the moment motion parameters comprises the following specific steps:
1) establishing airborne distributed POS transfer alignment model
The transfer alignment model adopts a matching mode of 'position + attitude', and the system state vector is a 15-dimensional state vector and comprises a platform misalignment angle of a child node JError in velocityPosition error (δ L)J、δλJ、δHJ) Gyro constant driftAnd adding a constant offsetAnd N is the number of subsystems. Wherein, east, north and sky misalignment angles of the J-th child node respectively;respectively, J-th sub-node is at nJEast, north and sky speed errors under the system; delta LJ、δλJ、δHJRespectively a latitude error, a longitude error and an altitude error of the J-th child node;andthe sub-IMU at the J-th sub-node is under its carrier coordinate system (b)JSystem) gyro constant drift and add a constant bias;at the J-th child node, respectively, the child IMU is at bJThe gyroscope on the x axis, the y axis and the z axis is subjected to random constant drift;at the J-th child node, respectively, the child IMU is at bJThe accelerometer is constantly biased in the x, y and z axes. The system state vector thus has the form:
a) establishment of equation of state
wherein the system noiseIs zero mean white Gaussian noise, its variance matrixDetermined by the noise levels of the gyroscopes and accelerometers in the IMU. FJThe state transition matrix is expressed in the following specific form:
wherein ,ωieRepresenting the rotational angular velocity of the earth;andrespectively, the sub IMU at the sub-node J in the navigation coordinate system (n)JSystem), northbound, and skyward specific force components;andrespectively, the child nodes J in the navigation coordinate system (n)JSystem) east-wise, north-wise and sky-wise speeds;andthe main curvature radius of the child node J along the meridian circle and the prime circle is respectively; t is a filtering period; gJThe system noise matrix of the child node J is expressed in the following specific form:
b) establishment of measurement equation
L valid measurement vectors z of child node JJA specific expression of (τ) (τ ═ 1,2, …, L) is:
wherein ,respectively representing the difference values of the tau-th latitude, longitude and altitude calculation value of the child node J and the latitude, longitude and altitude of the master node m after lever arm compensation;andrespectively representing differences of the tau-th course angle, the pitch angle and the roll angle calculated value of the child node J and the course angle, the pitch angle and the roll angle corresponding to the master node m;
Taas a master node's attitude matrix, i.e.And orderRepresentation matrix TaThe elements in the s-th row and T-th column, i.e. TaCan be expressed as:
2) data fusion using sequential filtering
Firstly, time updating is carried out, and for the J-th child node, the time is updated according to the last time tk-1Filtered estimate ofAnd transferring the alignment model to obtain the current tkOne-step predictive estimate of time of dayThe calculation formula is as follows:
wherein ,ΓJIs to transfer the state of the continuous system to the matrix FJAnd discretizing to obtain a transfer matrix of the discrete system.
In the same way, according to the estimated mean square error array P of the last momentJ(k-1| k-1) and the transfer alignment model may be derived for the current tkThe mean square error matrix is predicted by one step at a moment, and the calculation formula is as follows:
wherein ,ΞJIs to drive the noise of the continuous system to the array GJAnd discretizing to obtain a noise driving array of the discrete system.
Then, the effective measurement vector obtained in the step (3) is used for measurement updating, and the J-th sub-node is located at the current tkThe L measured direction at that time are respectively recorded asThe data fusion formula of the J-th child node is as follows:
wherein ,andare respectively asThe corresponding measurement matrix and the measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofCalculating to obtain a state estimation value during measurement updating;is composed ofA corresponding estimated mean square error matrix; i is andunit arrays with the same dimension.
wherein ,andare respectively asA corresponding measurement matrix and a measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofCalculating to obtain a state estimation value during measurement updating;is composed ofAnd (4) corresponding estimation mean square error matrix.
Executing the data fusion formula, and when the flow runs to the condition that tau is equal to L, obtaining the resultAs the current tkThe final data fusion result at the J-th child node at time instant, i.e. wherein ,containing tkLatitude error delta L after data fusion of J-th sub-node at momentJ(k) Longitude error δ λJ(k) And height error deltaHJ(k) And also includes n after data fusionJEast-down velocity error under tieError in north direction velocityAnd speed error in the skyAnd east misalignment angle after data fusionAngle of north misalignmentAnd angle of vertical misalignment
3) Motion parameter correction
Using the above fusion result to tkAnd correcting the result of the J-th sub-node strapdown calculation at the moment. The calibration procedure is as follows:
a) position correction
LJ(k)|new=LJ(k)-δLJ(k),λJ(k)|new=λJ(k)-δλJ(k),HJ(k)|new=HJ(k)-δHJ(k)
wherein ,LJ(k)、λJ(k)、HJ(k) Respectively represents tkAt the moment, the J-th child node is subjected to strapdown resolving to obtain latitude, longitude and altitude; l isJ(k)|new、λJ(k)|new、HJ(k)|newRespectively represents tkThe corrected latitude, longitude, and altitude of the jth child at time instant.
b) Velocity correction
wherein ,respectively represents tkAt the moment, the J-th child node is subjected to strapdown calculation to obtain an east-direction speed, a north-direction speed and a sky-direction speed;respectively represents tkAnd the east-direction speed, the north-direction speed and the sky-direction speed corrected by the J-th sub-node at the moment.
c) Attitude correction
Calculating tkNavigation coordinate system n of J-th sub-node at momentJAnd calculating a navigation coordinate system nJ' conversion matrix between
Modified transformation matrixComprises the following steps: wherein ,is tkObtaining an attitude matrix after performing strapdown calculation on J sub-nodes at the moment;
using modified transformation matricesCalculating tkCourse angle psi of sub-node J at timeJ(k)|newAngle of pitch thetaJ(k)|newAnd roll angle γJ(k)|new。
Repeating the steps (1) to (5) for other child nodes which are not subjected to data fusion and motion parameter correction in the step (6) until all child nodes finish tkThe method comprises the following specific steps of time data fusion and motion parameter correction:
1) let tkThe child node number of which is k at the moment when the steps (1) to (5) are completedN,kNThe initial value is 1;
2) if k isNN is the number of the child nodes, which indicates that the child nodes still do not finish data fusion and correction of motion parameters, kN=kN+1, pair number kNThe child node repeats steps (1) to (5);
3) if k isNIf N, stop step (6), meaning all child nodes have completed tkAnd (4) data fusion of time and correction of motion parameters.
Compared with the prior art, the invention has the advantages that:
the measurement bootstrap strategy and the sequential filtering are combined around the goal of improving the overall accuracy of airborne distributed POS multi-node motion parameter measurement. In the former method, the motion parameter information of all the child nodes is obtained by obtaining the measurement vector of each child node which is closer to the true value, then the measurement vectors are used for carrying out sequential filtering, and the obtained data fusion result is used for correcting the motion parameters of the child nodes. The method fully utilizes the motion parameter information of all the child nodes, overcomes the adverse effect of the uncertainty of the measurement noise on the transmission alignment estimation precision, and finally improves the estimation precision of the motion parameters of the child nodes.
Drawings
FIG. 1 is a flow chart of the present invention;
fig. 2 is a structural diagram of a data fusion method based on measurement bootstrapping and sequential filtering according to the present invention.
Detailed Description
As shown in FIG. 1, the specific method of the present invention is implemented as follows:
1. calculating tkThe method comprises the following specific steps of calculating redundant position and attitude information at a time sub-node:
(1) sub-node strapdown resolving
The definition of the relevant reference coordinate system includes: e is a terrestrial coordinate system; the navigation coordinate systems of the main node and the sub-nodes are northeast geographic coordinate systems, the navigation coordinate system of the main node is represented by n, and the navigation coordinate system and the calculation navigation coordinate system of the J-th sub-node are respectively represented by nJ and n′JIs represented by the formula J ═1,2, …, N, N is the number of child nodes; the origin of the carrier coordinate system is the center of gravity of the carrier, the x-axis is right along the transverse axis of the carrier, the y-axis is forward along the longitudinal axis of the carrier, the z-axis is upward along the vertical axis of the carrier, and the coordinate system is fixed on the carrier and is called as a right-front upper carrier coordinate system, and b is usedm、bJAnd the carrier coordinate systems respectively represent a main node and a J-th sub-node, wherein J is 1,2, … and N.
Each child node is solved through strapdown to obtain tkThe position [ L ] of each timeJ λJ HJ]TAnd attitude [ psiJθJ γJ]TAnd velocity wherein ,LJ、λJ and HJRespectively representing the latitude, longitude and altitude of the J-th sub-node; psiJ、θJ and γJRespectively representing a course angle, a pitch angle and a roll angle of the J-th sub-node;andrespectively represent the J-th sub-node at nJEast speed, north speed, and sky speed under the tie.
(2) Acquisition of redundant position and attitude information of child nodes
a) Acquisition of child node redundant location information
T can be obtained by a fiber grating sensor arranged on the wingkStrain information at any sub-node at any time, and further obtain the displacement of the sub-node from the initial position and relative position information Δ P between any two sub-nodes J, J '(J, J ≠ 1,2, …, N; J ≠ J')J←J′,ΔPJ←J′Representing the difference in latitude, longitude, and altitude between any two child nodes J, J'.
At tkAt the moment, the J-th child node is subjected to strapdown solution to obtain a position [ LJ λJ HJ]TJ-1, 2, …, N, and the other N-1 child nodes are designated as [ LJ′ λJ′ HJ′]T(J '═ 1,2, …, N, J' ≠ J). The redundant location information at the jth child node may be expressed as:
[LJ←J′ λJ←J′ HJ←J′]T=[LJ′ λJ′ HJ′]T+ΔPJ←J′(J,J′=1,2,…,N;J≠J′)
therefore, the J-th sub-node has N calculated values of latitude, longitude and altitude, namely [ L ] in total, in addition to the position information obtained by self strapdown calculationJ λJ HJ]T and [LJ←J′ λJ←J′ HJ←J′]T(J, J '═ 1,2, …, N, J' ≠ J). Position information [ L ] obtained by directly resolving the current child node J in a strapdown mannerJ λJ HJ]TIs newly recorded asRemaining N-1 location information [ LJ←J′ λJ←J′ HJ←J′]T(J, J '═ 1,2, …, N; J ≠ J') is reiterated
b) Acquisition of child node redundant attitude information
T measured by fiber grating sensorkThe angular displacement information of each sub-node at the moment can construct a conversion matrix between any two sub-nodes J, J '(J, J ≠ 1,2, …, N; J ≠ J') carrier coordinate systemsCarrier coordinate system at jth child node (b)JSystem) and the point navigation coordinate system (n)JSystem) as a transformation matrix (attitude matrix) betweenThe attitude matrix at the other N-1 child nodes is represented asThe redundant attitude matrix at the jth child node can be represented as:
wherein ,
at tkAt time, there are N attitude matrices at the J-th child node. By using the N attitude matrices, N attitude information, i.e., N attitude angles, can be calculated. The specific calculation method of the attitude angle is as follows:
wherein ,TJxyIs a matrixThe x-th row and the y-th column in the formula (I), wherein x is 1,2,3, and y is 1,2, 3; the attitude angle of the J-th sub-node, including the heading angle psiJAngle of pitch thetaJAnd roll angle γJPrincipal value of, i.e.Andrespectively as follows:
course angle psiJAngle of pitch thetaJAnd roll angle γJAre respectively defined as [0, 2 pi ]]、 [-π,+π](ii) a Then, ψJ、θJ and γJThe true value of (c) can be determined by:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the J-th sub-node can be calculatedCorresponding N-1 redundant attitude angles. The attitude matrix of the child node J itselfThe calculated attitude angle is recorded againFrom the rest N-1 attitude matricesThe attitude angle calculated is recorded as
2. Calculating tkMeasuring vectors of the sub-nodes at the moment, and generating a bootstrap measurement set by using a measurement bootstrap strategy, wherein the method specifically comprises the following steps:
(1) calculating a measurement vector for a child node
tkThe latitude, longitude and altitude of the time master node m after lever arm compensation are respectively recorded as Lm、λm、HmThe heading angle, pitch angle and roll angle of the master node m are respectively recorded as psim、θm、γm. T can be obtained for any sub-node J (J ═ 1,2, …, N)kN measured vector of timeThe expression is as follows:
wherein ,respectively represents tkThe ith latitude, longitude and altitude of the time sub-node J are different from the latitude, longitude and altitude of the master node m after lever arm compensation;respectively represents tkAnd the difference value of the ith course angle, the pitch angle and the roll angle of the sub-node J at the moment and the course angle, the pitch angle and the roll angle corresponding to the main node m.
(2) Generating a bootstrap measurement set using a measurement bootstrap policy
In the measurement vectorBased on the measurement result, the bootstrap measurement is generated by increasing the disturbanceThe method comprises the following specific steps:
wherein ,representing the original measurement vectorThe c-th bootstrap measurement generated on the basis, c ═ 1,2, …, l, l denote bootstrap measurementsL is 5; hJIs a measurement matrix of the system, xJIs the state vector of the system, vJIs measurement noise, and vJWhite Gaussian noise satisfying zero mean with a covariance of and vJHaving the same statistical properties, i.e.White Gaussian noise satisfying zero mean, its covariance
By the method, NxL bootstrap measurement data can be obtainedDefining a bootstrap measurement set: wherein Representing the original measurement vector, i.e.
3. And (3) sampling the elements in the bootstrap measurement set obtained in the step (2) by using a Metropolis-Hastings sampling criterion to obtain an effective measurement set of the child nodes, wherein the specific steps are as follows:
(1) calculating confidence and acceptance probability
From N bootstrap measurements, a set was collected according to Metropolis-Hastings sampling principleIn the random selection of two setsThen randomly extracting an element from the two setsAndand calculating corresponding credibilityAndthe calculation formula of the reliability is as follows:
wherein Is a measure of the mean value of the prediction,representative of the measurement noise vJCovariance matrixDeterminant (c).
In obtaining confidence levelAndon the basis of (1), calculating the acceptance probability according to the following formula:
(2) Constructing valid metrology collections
Firstly, a random number χ satisfying uniform distribution U (0,1) is generated, and an effective measurement set ΩJThe determination method of (2) is as follows:
in the formula, if the elements are randomly extractedAndif the corresponding acceptance probability is greater than or equal to the generated random number χ, then it will beAs an effective measurement set omegaJThe elements of (1); otherwise, willAs an effective measurement set omegaJOf (1). The above process is a sampling process for determining the effective measurement vector.
Repeating the sampling for L times, wherein L is 2N, and L effective measurement vectors in the effective measurement set are respectively defined as zJ(1),zJ(2),…zJ(L) these effective measurement vectors constitute an effective measurement set omegaJ。
4. Aiming at the effective measurement set obtained in the step 3, calculating the weight of each effective measurement vector and a measurement noise matrix, and the specific steps are as follows:
(1) computing a coherence distance and a coherence matrix
Defining an effective metrology set omegaJAny two of the measurement vectors zJ(ξ) and zJThe confidence distance between (ζ) isThe calculation method is as follows:
calculating a consistency distance based thereonAnd the consistency matrix ΨJThe calculation method is as follows:
wherein ,represents omegaJThe maximum value of the confidence distance between any two measurement vectors.
(2) Calculating weights of effective measurement vectors and measurement noise matrix
Obtain the consistency matrix ΨJThen, the maximum module eigenvalue lambda and the corresponding eigenvector beta of the matrix are calculated, and the eigenvector beta is unitized to obtainOrder weight vectorAt this time, the process of the present invention,the effective measurement set omega can be used to represent the corresponding effective measurement vectorJThe overall degree of support of all elements in.
According to the basic principle of calculating the variance in the mathematical statistics, the measurement noise matrix corresponding to each element in the effective measurement set is calculatedThe calculation method is as follows:
5. using sequential filtering to perform data fusion on the child node, and applying the fusion result to the child node tkThe correction of the moment motion parameters comprises the following specific steps:
(1) establishing airborne distributed POS transfer alignment model
The transfer alignment model adopts a matching mode of 'position + attitude', and the system state vector is a 15-dimensional state vector and comprises a platform misalignment angle of a child node JError in velocityPosition error (δ L)J、δλJ、δHJ) Gyro constant driftAnd adding a constant offsetAnd N is the number of subsystems. Wherein, east, north and sky misalignment angles of the J-th child node respectively;respectively, J-th sub-node is at nJEast, north and sky speed errors under the system; delta LJ、δλJ、δHJRespectively a latitude error, a longitude error and an altitude error of the J-th child node;andthe sub-IMU at the J-th sub-node is under its carrier coordinate system (b)JSystem) gyro constant drift and add a constant bias;at the J-th child node, respectively, the child IMU is at bJThe gyroscope on the x axis, the y axis and the z axis is subjected to random constant drift;at the J-th child node, respectively, the child IMU is at bJThe accelerometer is constantly biased in the x, y and z axes. The system state vector thus has the form:
a) establishment of equation of state
The system state equation has the form:
wherein the system noiseIs zero mean white Gaussian noise, its variance matrixDetermined by the noise levels of the gyroscopes and accelerometers in the IMU.
in the formula ,FJThe state transition matrix is expressed in the following specific form:
wherein ,
wherein ,ωieRepresenting the rotational angular velocity of the earth;andrespectively, the sub IMU at the sub-node J in the navigation coordinate system (n)JSystem), northbound, and skyward specific force components;andrespectively, the child nodes J in the navigation coordinate system (n)JSystem) east-wise, north-wise and sky-wise speeds;andthe main curvature radius of the child node J along the meridian circle and the prime circle is respectively; t is the filter period. GJThe system noise matrix of the child node J is expressed in the following specific form:
b) establishment of measurement equation
L valid measurement vectors z of child node JJA specific expression of (τ) (τ ═ 1,2, …, L) is:
wherein ,respectively representing the difference values of the tau-th latitude, longitude and altitude calculation value of the child node J and the latitude, longitude and altitude of the master node m after lever arm compensation;andrespectively representing differences of the tau-th course angle, the pitch angle and the roll angle calculated value of the child node J and the course angle, the pitch angle and the roll angle corresponding to the master node m;
the measurement equation has the following expression form:
wherein
TaAs a master node's attitude matrix, i.e.And orderRepresentation matrix TaThe elements in the s-th row and T-th column, i.e. TaCan be expressed as:
because any child node of each child node in the airborne distributed POS can obtain the redundant motion parameter information converted to the position of the other child nodes, the redundant measurement vector can be obtained according to the method in the step 2, and therefore, for the child node J, the measurement vector can be expressed as:
(2) Data fusion using sequential filtering
Firstly, time updating is carried out, and for the J-th child node, the time is updated according to the last time tk-1Filtered estimate ofAnd transferring the alignment model to obtain the current tkOne-step predictive estimate of time of dayThe calculation formula is as follows:
wherein ,ΓJIs to transfer the state of the continuous system to the matrix FJAnd discretizing to obtain a transfer matrix of the discrete system.
In the same way, according to the estimated mean square error array P of the last momentJ(k-1| k-1) and the transfer alignment model may be derived for the current tkThe mean square error matrix is predicted by one step at a moment, and the calculation formula is as follows:
wherein ,ΞJIs to drive the array G with continuous system noiseJAnd discretizing to obtain a noise driving array of the discrete system.
Then, the effective measurement vector obtained in the step (3) is used for measurement updating, and the J-th sub-node is located at the current tkThe L measured direction at that time are respectively recorded asThe data fusion formula of the J-th child node is as follows:
wherein ,andare respectively asThe corresponding measurement matrix and the measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofCalculating to obtain a state estimation value during measurement updating;is composed ofA corresponding estimated mean square error matrix; i is andunit arrays with the same dimension.
wherein ,andare respectively asA corresponding measurement matrix and a measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofAmount of progressMeasuring a state estimation value obtained by calculation during updating;is composed ofAnd (4) corresponding estimation mean square error matrix.
Executing the data fusion formula, and when the flow runs to the condition that tau is equal to L, obtaining the resultAs the current tkThe final data fusion result at the J-th child node at time instant, i.e. wherein ,containing tkLatitude error delta L after data fusion of J-th sub-node at momentJ(k) Longitude error δ λJ(k) And height error deltaHJ(k) And also includes n after data fusionJEast-down velocity error under tieError in north direction velocityAnd speed error in the skyAnd east misalignment angle after data fusionAngle of north misalignmentAnd angle of vertical misalignment
(3) Motion parameter correction
Using the above fusion result to tkAnd correcting the result of the J-th sub-node strapdown calculation at the moment. The calibration procedure is as follows:
a) position correction
wherein ,LJ(k)、λJ(k)、HJ(k) Respectively represents tkAt the moment, the J-th child node is subjected to strapdown resolving to obtain latitude, longitude and altitude; l isJ(k)|new、λJ(k)|new、HJ(k)|newRespectively represents tkThe corrected latitude, longitude, and altitude of the jth child at time instant.
b) Velocity correction
wherein ,respectively represents tkAt the moment, the J-th child node is subjected to strapdown calculation to obtain an east-direction speed, a north-direction speed and a sky-direction speed;respectively represents tkAnd the east-direction speed, the north-direction speed and the sky-direction speed corrected by the J-th sub-node at the moment.
c) Attitude correction
Calculating tkNavigation coordinate system n of J-th sub-node at momentJAnd calculating a navigation coordinate system nJ' conversion matrix between
wherein ,is tkObtaining an attitude matrix after performing strapdown calculation on J sub-nodes at the moment;
using modified transformation matricesCalculating tkCourse angle psi of sub-node J at timeJ(k)|newAngle of pitch thetaJ(k)|newAnd roll angle γJ(k)|new。
6. Repeating the steps 1 to 5 for other sub-nodes which are not subjected to data fusion and motion parameter correction until all the sub-nodes finish tkThe method comprises the following specific steps of time data fusion and motion parameter correction:
(1) let tkThe child node number k at which steps 1 to 5 have been completedN,kNIs 1;
(2) if k isNN is the number of the child nodes, which indicates that the child nodes still do not finish data fusion and correction of motion parameters, kN=kN+1, pair number kNThe child node repeats the first 5 steps;
(3) if k isNN, then step 6 is stopped, indicating that all child nodes have completed tkAnd (4) data fusion of time and correction of motion parameters.
7、tk=tk+1Execution of the step1 to 6, until the data fusion and the correction of the motion parameters of all the child nodes at all the time are completed.
Those skilled in the art will appreciate that the invention may be practiced without these specific details. For those skilled in the art, variations can be made in the specific embodiments and applications without departing from the spirit of the invention. In view of the above, the present disclosure should not be construed as limiting the invention.
Claims (8)
1. A data fusion method for an airborne distributed position and attitude measurement system comprises the following specific steps:
1.1 calculating tkRedundant position and attitude information at the time child node;
1.2 calculating tkMeasuring vectors of the sub-nodes at the moment, and generating a bootstrap measurement set by using a measurement bootstrap strategy;
1.3 sampling elements in the bootstrap measurement set by using a Metropolis-Hastings sampling criterion to obtain an effective measurement set of the child nodes;
1.4 calculating the weight and the measurement noise matrix of each effective measurement vector in the effective measurement set;
1.5 data fusion for the child node using sequential filtering, applying the fusion result to the child node tkCorrecting the motion parameters at the moment;
1.6 repeating steps 1.1 to 1.5 for other sub-nodes without data fusion and motion parameter correction until all sub-nodes finish tkData fusion and correction of motion parameters at the moment;
1.7 tk=tk+1and executing the steps 1.1 to 1.6 until the data fusion and the correction of the motion parameters of all the child nodes at all the time are completed.
2. The data fusion method of the airborne distributed position and attitude measurement system according to claim 1, characterized in that: in the step 1.1, t is calculatedkThe specific steps of the redundant position and attitude information at the time child node are as follows:
2.1 child node strapdown resolution
The definition of the relevant reference coordinate system includes: e is a terrestrial coordinate system; the navigation coordinate systems of the main node and the sub-nodes are northeast geographic coordinate systems, the navigation coordinate system of the main node is represented by n, and the navigation coordinate system and the calculation navigation coordinate system of the J-th sub-node are respectively represented by nJ and n′JJ is 1,2, …, N is the number of child nodes; the origin of the carrier coordinate system is the center of gravity of the carrier, the x-axis is right along the transverse axis of the carrier, the y-axis is forward along the longitudinal axis of the carrier, the z-axis is upward along the vertical axis of the carrier, and the coordinate system is fixed on the carrier and is called as a right-front upper carrier coordinate system, and b is usedm、bJA carrier coordinate system representing the main POS and the jth sub-node, J ═ 1,2, …, N;
each child node is solved through strapdown to obtain tkThe position [ L ] of each timeJ λJ HJ]TAttitude [ psiJ θJγJ]TAnd velocity wherein ,LJ、λJ and HJRespectively representing the latitude, longitude and altitude of the J-th sub-node; psiJ、θJ and γJRespectively representing a course angle, a pitch angle and a roll angle of the J-th sub-node;andrespectively representing the east speed, the north speed and the sky speed of the J-th child node;
2.2 acquisition of redundant position and attitude information of child nodes
a) Acquisition of child node redundant location information
T can be obtained by a fiber grating sensor arranged on the wingkStrain information of any sub-node at any moment, and further displacement of the strain information relative to the initial position of the strain information and any two sub-nodes JAnd J' relative position information DeltaPJ←J′,ΔPJ←J′Represents the difference in latitude, longitude, and altitude between any two child nodes J, J ', J, J' is 1,2, …, N; j is not equal to J';
at tkAt the moment, the J-th child node is subjected to strapdown solution to obtain a position [ LJ λJ HJ]TJ-1, 2, …, N, and the other N-1 child nodes are designated as [ LJ′ λJ′ HJ′]TJ '═ 1,2, …, N, J' ≠ J; the redundant location information at the jth child node may be expressed as:
[LJ←J′ λJ←J′ HJ←J′]T=[LJ′ λJ′ HJ′]T+ΔPJ←J′(J,J′=1,2,…,N;J≠J′)
therefore, the J-th sub-node has N calculated values of latitude, longitude and altitude, namely [ L ] in total, in addition to the position information obtained by self strapdown calculationJ λJ HJ]T and [LJ←J′ λJ←J′ HJ←J′]TJ, J '═ 1,2, …, N, J' ≠ J; position information [ L ] obtained by directly resolving the current child node J in a strapdown mannerJ λJ HJ]TIs newly recorded asRemaining N-1 location information [ LJ←J′λJ←J′ HJ←J′]TIs newly recorded as
b) Acquisition of child node redundant attitude information
T measured by fiber grating sensorkThe angular displacement information of each child node at the moment can construct a conversion matrix between carrier coordinate systems of any two child nodes J, JCarrier coordinate system b at jth sub-nodeJIs calculated with the point and navigates the coordinate system nJThe transformation matrix between the systems is expressed asThe transformation matrix is also called an attitude matrix, and the attitude matrices at the other N-1 sub-nodes are expressed asThe redundant attitude matrix at the jth child node can be represented as:
wherein ,
at tkAt the moment, N attitude matrixes exist at the J-th sub-node; by utilizing the N attitude matrixes, N attitude information, namely N attitude angles, can be calculated; the specific calculation method of the attitude angle is as follows:
wherein ,TJxyIs a matrixThe x-th row and the y-th column in the formula (I), wherein x is 1,2,3, and y is 1,2, 3; the attitude angle of the J-th sub-node, including the heading angle psiJAngle of pitch thetaJAnd roll angle γJPrincipal value of, i.e.Andrespectively as follows:
course angle psiJAngle of pitch thetaJAnd roll angle γJAre respectively defined as [0, 2 pi ]]、 [-π,+π](ii) a Then, ψJ、θJ and γJThe true value of (c) can be determined by:
according to the calculation method of the attitude angle, N-1 redundant attitude matrixes at the J-th sub-node can be calculatedCorresponding N-1 redundant pose information, J ═ 1,2, …, N; j is not equal to J'; the attitude matrix of the child node J itselfThe calculated attitude angle is recorded againFrom the rest N-1 attitude matricesThe attitude angle calculated is recorded as
3. The data fusion method of the airborne distributed position and attitude measurement system according to claim 2, characterized in that: in the step 1.2, t is calculatedkThe method comprises the following specific steps of measuring vectors of the sub-nodes at the moment and generating a bootstrap measurement set by using a measurement bootstrap strategy:
3.1 calculating the measurement vector of the child node
tkThe latitude, longitude and altitude of the time master node m after lever arm compensation are respectively recorded as Lm、λm、HmThe heading angle, pitch angle and roll angle of the master node m are respectively recorded as psim、θm、γm(ii) a For any sub-node J, J ═ 1,2, …, N, t can be obtainedkN measured vector at timeThe expression is as follows:
wherein ,respectively represents tkThe ith latitude, longitude and altitude of the time sub-node J are different from the latitude, longitude and altitude of the master node m after lever arm compensation;respectively represents tkThe difference value of the ith course angle, the pitch angle and the roll angle of the sub-node J at the moment and the course angle, the pitch angle and the roll angle corresponding to the main node m;
3.2 generating a bootstrap measurement set by using a measurement bootstrap strategy
In the measurement vectorBased on the measurement result, the bootstrap measurement is generated by increasing the disturbanceThe method comprises the following specific steps:
wherein ,representing the original measurement vectorThe c-th bootstrap measurement generated on the basis, c ═ 1,2, …, l, l denote bootstrap measurementsL is 5; hJIs a measurement matrix of the system, xJIs the state vector of the system, vJIs measurement noise, and vJWhite Gaussian noise satisfying zero mean with a covariance of and vJHaving the same statistical properties, i.e.White Gaussian noise satisfying zero mean, its covariance
4. The data fusion method of the airborne distributed position and attitude measurement system according to claim 3, characterized in that: in step 1.3, the Metropolis-Hastings sampling criterion is used to sample elements in the bootstrap measurement set to obtain an effective measurement set of child nodes, and the specific steps are as follows:
4.1 calculate confidence and acceptance probability
From N bootstrap measurements, a set was collected according to Metropolis-Hastings sampling principleIn the random selection of two setsThen randomly extracting an element from the two setsAndand calculating corresponding credibilityAndthe calculation formula of the reliability is as follows:
wherein Is a measure of the mean value of the prediction,representative of the measurement noise vJCovariance matrixDeterminant of (4);
in obtaining confidence levelAndon the basis of (1), calculating the acceptance probability according to the following formula:
4.2 building efficient metrology collections
Firstly, a random number χ satisfying uniform distribution U (0,1) is generated, and an effective measurement set ΩJThe determination method of (2) is as follows:
in the formula, if the elements are randomly extractedAndif the corresponding acceptance probability is greater than or equal to the generated random number χ, then it will beAs an effective measurement set omegaJThe elements of (1); otherwise, willAs an effective measurement set omegaJThe elements of (1); the above process is a sampling process for determining an effective measurement vector;
repeating the sampling for L times, wherein L is 2N, and L effective measurement vectors in the effective measurement set are respectively defined as zJ(1),zJ(2),…zJ(L) these effective measurement vectors constitute an effective measurement set omegaJ。
5. The data fusion method of the airborne distributed position and attitude measurement system according to claim 4, characterized in that: the specific steps of calculating the weight of each effective measurement vector in the effective measurement set and measuring the noise matrix in step 1.4 are as follows:
5.1 calculate the consistency distance and consistency matrix
Defining an effective metrology set omegaJAny two of the measurement vectors zJ(ξ) and zJThe confidence distance between (ζ) isThe calculation method is as follows:
calculating a consistency distance based thereonAnd the consistency matrix ΨJThe calculation method is as follows:
wherein ,represents omegaJThe maximum value of the confidence distance between any two measurement vectors;
5.2 calculating weights of effective measurement vectors and measurement noise matrix
Obtain the consistency matrix ΨJThen, the maximum module eigenvalue lambda and the corresponding eigenvector beta of the matrix are calculated, and the eigenvector beta is unitized to obtainOrder weight vectorAt this time, the process of the present invention,can be used to represent the corresponding valid measurement vector is valid measurement set omegaJThe overall degree of support of all elements in;
according to the basic principle of calculating the variance in the mathematical statistics, the measurement noise matrix corresponding to each element in the effective measurement set is calculatedThe calculation method is as follows:
6. the data fusion method of the airborne distributed position and attitude measurement system according to claims 1 and 4, characterized in that: in the step 1.5, sequential filtering is used for carrying out data fusion on the child nodes, and the fusion result is used for the child nodes tkThe correction of the moment motion parameters comprises the following specific steps:
6.1 building airborne distributed POS transfer alignment model
The transfer alignment model adopts a matching mode of 'position + attitude', and the system state vector is a 15-dimensional state vector and comprises a platform misalignment angle of a child node JError in velocityPosition error δ LJ、δλJ、δHJConstant drift of gyroAnd adding a constant offsetN is the number of subsystems; wherein,east, north and sky misalignment angles of the J-th child node respectively;respectively, J-th sub-node is at nJEast, north and sky speed errors under the system; delta LJ、δλJ、δHJRespectively a latitude error, a longitude error and an altitude error of the J-th child node;andrespectively, the sub IMU at the J-th sub-node is in the carrier coordinate system bJThe gyro constant drift and the addition constant bias are tied down;at the J-th child node, respectively, the child IMU is at bJThe gyroscope on the x axis, the y axis and the z axis is subjected to random constant drift;at the J-th child node, respectively, the child IMU is at bJAccelerometer constant offsets in the x, y and z axes; the system state vector thus has the form:
a) establishment of equation of state
The system state equation has the form:
wherein the system noiseIs zero mean white Gaussian noise, its variance matrixDetermined by the noise levels of the gyroscopes and accelerometers in the IMU;
in the formula ,FJThe state transition matrix is expressed in the following specific form:
wherein ,ωieRepresenting the rotational angular velocity of the earth;andrespectively, the sub IMU at the sub node J in the navigation coordinate system nJEast, north and sky specific force components in the system;andrespectively a child node J in a navigation coordinate system nJAn east-direction speed, a north-direction speed and a sky-direction speed under the system;andthe main curvature radius of the child node J along the meridian circle and the prime circle is respectively; t is a filtering period; gJThe system noise matrix of the child node J is expressed in the following specific form:
b) establishment of measurement equation
L valid measurement vectors z of child node JJ(τ), τ ═ 1,2, …, and the specific expression for L is:
wherein ,respectively representing the difference values of the tau-th latitude, longitude and altitude calculation value of the child node J and the latitude, longitude and altitude of the master node m after lever arm compensation;andrespectively representing differences of the tau-th course angle, the pitch angle and the roll angle calculated value of the child node J and the course angle, the pitch angle and the roll angle corresponding to the master node m;
the measurement equation has the following expression form:
wherein ,
Taas a master node's attitude matrix, i.e.And orderRepresentation matrix TaThe elements in the s-th row and T-th column, i.e. TaCan be expressed as:
6.2 data fusion Using sequential Filtering
Firstly, time updating is carried out, and for the J-th child node, the time is updated according to the last time tk-1Filtered estimate ofAnd transferring the alignment model to obtain the current tkOne-step predictive estimate of time of dayThe calculation formula is as follows:
wherein ,ΓJIs to transfer the state of the continuous system to the matrix FJObtaining a transfer matrix of a discrete system after discretization;
in the same way, according to the estimated mean square error array P of the last momentJ(k-1| k-1) and the transfer alignment model may be derived for the current tkOne step predictive mean square of timeError matrix, the calculation formula is as follows:
wherein ,ΞJIs to drive the noise of the continuous system to the array GJDiscretizing to obtain a noise driving array of a discrete system;
then, the effective measurement vector obtained in the step 1.3 is used for measurement updating, and the J-th sub-node is located at the current tkThe L measured direction at that time are respectively recorded asThe data fusion formula of the J-th child node is as follows:
wherein ,andare respectively asThe corresponding measurement matrix and the measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofCalculating to obtain a state estimation value during measurement updating; p1 J(k | k) isA corresponding estimated mean square error matrix; i is andunit arrays with the same dimension;
wherein ,andare respectively asThe corresponding measurement matrix and the measurement noise matrix;to useCalculating a filter gain matrix during measurement updating;indicating use ofCalculating to obtain a state estimation value during measurement updating;is composed ofA corresponding estimated mean square error matrix;
executing the data fusion formula, and when the flow runs to the condition that tau is equal to L, obtaining the resultAs the current tkThe final data fusion result at the J-th child node at time instant, i.e. wherein ,containing tkLatitude error delta L after data fusion of J-th sub-node at momentJ(k) Longitude error δ λJ(k) And height error deltaHJ(k) And also includes n after data fusionJEast-down velocity error under tieError in north direction velocityAnd speed error in the skyAnd east misalignment angle after data fusionAngle of north misalignmentAnd angle of vertical misalignment
6.3 motion parameter correction
Using the above fusion result to tkAnd correcting the result of the J-th sub-node strapdown calculation at the moment, wherein the correction steps are as follows:
a) position correction
wherein ,LJ(k)、λJ(k)、HJ(k) Respectively represents tkThe J-th sub-node at the moment is obtained by the quick-link solutionLatitude, longitude and altitude to; l isJ(k)|new、λJ(k)|new、HJ(k)|newRespectively represents tkLatitude, longitude and altitude after J-th child node correction at the moment;
b) velocity correction
wherein ,respectively represents tkAt the moment, the J-th child node is subjected to strapdown calculation to obtain an east-direction speed, a north-direction speed and a sky-direction speed;respectively represents tkThe east speed, the north speed and the sky speed after the J-th sub-node correction at the moment;
c) attitude correction
Calculating tkNavigation coordinate system n of J-th sub-node at momentJAnd calculating a navigation coordinate system nJ' conversion matrix between
wherein ,is tkPerforming strapdown calculation on the J-th sub-node at the moment to obtain an attitude matrix;
7. The data fusion method of the airborne distributed position and attitude measurement system according to claim 1, characterized in that: in the step 1.6, the steps 1.1 to 1.5 are repeated for other child nodes which are not subjected to data fusion and motion parameter correction until all the child nodes finish tkThe method comprises the following steps of time data fusion and motion parameter correction:
7.1 hypothesis tkThe child node number k at which steps 1.1 to 1.5 have been completedN,kNIs 1;
7.2 if kNN is the number of the child nodes, which indicates that the child nodes still do not finish data fusion and correction of motion parameters, kN=kN+1, pair number kNThe child node of (1) repeats steps 1.1 to 1.5;
7.3 if kNN, then step 1.6 is stopped, indicating that all child nodes have completed tkAnd (4) data fusion of time and correction of motion parameters.
8. The data fusion method of the airborne distributed position and attitude measurement system according to claim 1, characterized in that: in the step 1.7, t is enabledk=tk+1And executing the steps 1.1 to 1.6 until the data fusion and the correction of the motion parameters of all the child nodes at all the time are completed.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110309944.7A CN113188566B (en) | 2021-03-23 | 2021-03-23 | Airborne distributed POS data fusion method |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110309944.7A CN113188566B (en) | 2021-03-23 | 2021-03-23 | Airborne distributed POS data fusion method |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113188566A true CN113188566A (en) | 2021-07-30 |
CN113188566B CN113188566B (en) | 2023-09-29 |
Family
ID=76973665
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110309944.7A Active CN113188566B (en) | 2021-03-23 | 2021-03-23 | Airborne distributed POS data fusion method |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113188566B (en) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104655152A (en) * | 2015-02-11 | 2015-05-27 | 北京航空航天大学 | Onboard distributed type POS real-time transmission alignment method based on federal filtering |
CN107728182A (en) * | 2017-09-18 | 2018-02-23 | 北京航空航天大学 | Flexible more base line measurement method and apparatus based on camera auxiliary |
CN108387227A (en) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | The multinode information fusion method and system of airborne distribution POS |
CN108458709A (en) * | 2018-02-22 | 2018-08-28 | 北京航空航天大学 | The airborne distributed POS data fusion method and device of view-based access control model subsidiary |
WO2020233290A1 (en) * | 2019-05-17 | 2020-11-26 | 东南大学 | Dual-filter-based transfer alignment method under dynamic deformation |
CN112525191A (en) * | 2021-02-08 | 2021-03-19 | 北京航空航天大学 | Airborne distributed POS transfer alignment method based on relative strapdown calculation |
-
2021
- 2021-03-23 CN CN202110309944.7A patent/CN113188566B/en active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104655152A (en) * | 2015-02-11 | 2015-05-27 | 北京航空航天大学 | Onboard distributed type POS real-time transmission alignment method based on federal filtering |
CN107728182A (en) * | 2017-09-18 | 2018-02-23 | 北京航空航天大学 | Flexible more base line measurement method and apparatus based on camera auxiliary |
CN108387227A (en) * | 2018-02-22 | 2018-08-10 | 北京航空航天大学 | The multinode information fusion method and system of airborne distribution POS |
CN108458709A (en) * | 2018-02-22 | 2018-08-28 | 北京航空航天大学 | The airborne distributed POS data fusion method and device of view-based access control model subsidiary |
WO2020233290A1 (en) * | 2019-05-17 | 2020-11-26 | 东南大学 | Dual-filter-based transfer alignment method under dynamic deformation |
CN112525191A (en) * | 2021-02-08 | 2021-03-19 | 北京航空航天大学 | Airborne distributed POS transfer alignment method based on relative strapdown calculation |
Non-Patent Citations (5)
Title |
---|
GONG XIAOLIN ET AL.: "Multi-Node Transfer Alignment Based on Mechanics Modeling for Airborne DPOS", 《IEEE SENSORS JOURNAL》, vol. 18, no. 2, pages 669 - 679 * |
LI JIANLI ET AL.: "Multisensor Time Synchronization Error Modeling and Compensation Method for Distributed POS", 《IEEE TRANSACTIONS ON INSTRUMENTATION AND MEASUREMENT》, vol. 65, no. 11, pages 2637 - 2645, XP011625064, DOI: 10.1109/TIM.2016.2598020 * |
宋丽君;秦永元;: "联邦模糊自适应卡尔曼滤波在速度+姿态传递对准中的应用", 测控技术, no. 05, pages 135 - 138 * |
房建成等: "机载分布式POS传递对准建模与仿真", 《中国惯性技术学报》, vol. 20, no. 4, pages 379 - 385 * |
胡振涛等: "基于Metropolis-Hastings采样的多传感器集合卡尔曼滤波算法", 《电子学报》, vol. 45, no. 4, pages 868 - 873 * |
Also Published As
Publication number | Publication date |
---|---|
CN113188566B (en) | 2023-09-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110501024B (en) | Measurement error compensation method for vehicle-mounted INS/laser radar integrated navigation system | |
CN112629538B (en) | Ship horizontal attitude measurement method based on fusion complementary filtering and Kalman filtering | |
CN111426318B (en) | Low-cost AHRS course angle compensation method based on quaternion-extended Kalman filtering | |
CN101949703B (en) | Strapdown inertial/satellite combined navigation filtering method | |
CN108413887B (en) | Wing-shaped deformation measuring method, device and platform of fiber bragg grating assisted distributed POS | |
CN101788296B (en) | SINS/CNS deep integrated navigation system and realization method thereof | |
CN108387227B (en) | Multi-node information fusion method and system of airborne distributed POS | |
CN107728182B (en) | Flexible multi-baseline measurement method and device based on camera assistance | |
US20040133346A1 (en) | Attitude change kalman filter measurement apparatus and method | |
CN110954102B (en) | Magnetometer-assisted inertial navigation system and method for robot positioning | |
CN110243377B (en) | Cluster aircraft collaborative navigation method based on hierarchical structure | |
CN108458709B (en) | Airborne distributed POS data fusion method and device based on vision-aided measurement | |
CN111189442B (en) | CEPF-based unmanned aerial vehicle multi-source navigation information state prediction method | |
CN107014376A (en) | A kind of posture inclination angle method of estimation suitable for the accurate operation of agricultural machinery | |
CN104698486A (en) | Real-time navigation method of data processing computer system for distributed POS | |
CN110285834B (en) | Quick and autonomous readjustment method for double-inertial navigation system based on one-point position information | |
CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
CN108303120B (en) | Real-time transfer alignment method and device for airborne distributed POS | |
CN112325886A (en) | Spacecraft autonomous attitude determination system based on combination of gravity gradiometer and gyroscope | |
CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
CN116007620A (en) | Combined navigation filtering method, system, electronic equipment and storage medium | |
CN110388942B (en) | Vehicle-mounted posture fine alignment system based on angle and speed increment | |
CN116222551A (en) | Underwater navigation method and device integrating multiple data | |
CN107764268B (en) | Method and device for transfer alignment of airborne distributed POS (point of sale) | |
CN116105730A (en) | Angle measurement-only optical combination navigation method based on cooperative target satellite very short arc observation |
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 | ||
GR01 | Patent grant | ||
GR01 | Patent grant |