A Multi-UAV B-Grid Relative Navigation Method Based on Factor Graph Optimization

Abstract

This invention discloses a multi-UAV dual-grid relative navigation method and device based on factor graph optimization, belonging to the field of multi-robot cooperative localization technology. Existing relative navigation technologies are affected by state modeling accuracy and measurement nonlinearity, making accurate relative position estimation difficult in challenging GNSS environments. This method first proposes a new relative position calculation method, based on which a dual absolute navigation state model is proposed, improving the accuracy of relative navigation state modeling. Then, IMU pre-integration is improved by considering gravity variations, and pre-integration of the error state is proposed. Based on this, a multi-UAV dual-grid relative navigation framework based on factor graph optimization is proposed, significantly reducing the impact of measurement nonlinearity on relative position estimation. The navigation method proposed in this invention can significantly reduce the impact of state modeling accuracy and measurement nonlinearity on relative position estimation, improving the accuracy of relative position estimation, and has practical engineering application significance.

Description

Technical Field

[0001] The invention relates to the field of multi-UAV relative navigation technology, and in particular to a multi-UAV dual-grid relative navigation method based on factor graph optimization. Background Technology

[0002] The rapid development of the low-altitude economy has provided a strong impetus for the unmanned aerial vehicle (UAV) industry, driving its widespread application in logistics, transportation, and many other fields. While UAVs excel in single-mission scenarios due to their high maneuverability, they still face significant limitations in complex tasks such as collaborative search and rescue or swarm operations. Therefore, multi-UAV collaborative systems have become a core research direction of common interest to both academia and industry. As a key technology for multi-UAV collaboration, high-precision relative positioning technology is crucial, directly affecting the positioning accuracy of the formation, collision avoidance safety, and overall mission reliability. Ideally, when GNSS signals are continuous and stable, relatively accurate relative position measurements can be achieved between UAVs via satellite navigation. However, in real-world operating environments, such as urban canyons, indoor spaces, or areas with strong electromagnetic interference, GNSS signals are often intermittent or even completely lost due to obstruction or interference. These problems can lead to the accumulation of relative positioning errors and formation position drift, posing a serious threat to the reliability and safety of multi-UAV collaborative operations. Therefore, how to achieve stable, reliable, and high-precision relative position estimation in environments with limited GNSS signals remains a key technical challenge that urgently needs to be overcome in the development of UAV systems.

[0003] Relative navigation technology aims to achieve accurate relative position estimation in environments where Global Navigation Satellite System (GNSS) signals are difficult to obtain. This is achieved by fusing inertial measurement unit (IMU) data, relative observation data between UAVs, and available external absolute measurement data, and by employing appropriate information fusion algorithms. Existing relative navigation methods are mainly based on the Extended Kalman Filter (EKF) and fall into two main categories: centralized and distributed methods. Centralized methods are equipped with a central fusion node that can acquire measurement data from all nodes in real time and broadcast the estimated state to all nodes immediately. However, this method places extremely high demands on the communication capabilities of the UAV swarm, limiting its feasibility in practical applications. In distributed methods, each node acts as a fusion center, estimating its own local or global state, thus offering greater flexibility. Based on the nature of the estimated state, distributed methods can be further subdivided into decentralized and multi-centralized methods. In multi-centralized methods, each node estimates the global state of the entire UAV swarm, achieving positioning accuracy comparable to centralized methods. However, as the swarm size increases, the computational and communication loads increase significantly. In decentralized methods, each node only estimates its own local state, requiring less computational and communication resources and making it easier to scale to large-scale relative navigation systems. Among these, the relative navigation algorithm based on bi-grid modeling introduces a relative grid coordinate system in which a virtual anchor point, the NC (Navigation controller), exists with infinitely precise relative position, regardless of whether it can obtain accurate GNSS signals. Through certain relative measurements and relative maneuvers, this method can achieve convergent relative position estimation in challenging GNSS environments. However, existing bi-grid modeling-based relative navigation methods struggle to further improve relative positioning accuracy due to state modeling accuracy and measurement nonlinearity issues. More seriously, measurement nonlinearity can lead to inconsistent state estimation results, resulting in divergent estimation errors.

[0004] Compared to EKF, Factor Graph Optimization (FGO) has inherent advantages in handling nonlinear measurements and has received increasing attention in recent years. Nevertheless, applying FGO to relative navigation to address the aforementioned problems still faces challenges in state modeling and pre-integrated measurement construction. Summary of the Invention

[0005] The purpose of this invention is to address the problem that existing relative navigation methods are susceptible to inaccurate relative position estimation due to the influence of state modeling accuracy and measurement nonlinearity. To this end, this invention proposes a distributed relative navigation method based on factor graph optimization: It proposes a novel relative position calculation method, based on which a new dual-absolute navigation state model is proposed. Each UAV node in the cluster models only its own direct state and the error state of the Navigation Controller (NC), improving the accuracy of relative navigation state modeling. An improved IMU pre-integration model is proposed to adapt to high-dynamic scenarios, and a pre-integration model for the NC error state is proposed for the first time. Based on this, a UAV dual-grid relative navigation framework based on factor graph optimization is proposed, significantly reducing the impact of measurement nonlinearity on relative navigation positioning accuracy.

[0006] This invention provides a multi-UAV dual-grid relative navigation method based on factor graph optimization, comprising:

[0007] Divide the N drone nodes in the drone swarm into N-1 ordinary drone nodes and 1 NC node; acquire the output data of the gyroscope and accelerometer in the drone IMU and the position estimation and velocity estimation data broadcast by the NC; calculate the pose estimation of any drone node and the estimation result of the NC error state modeled by drone node i; at the same time, calculate the IMU pre-integration measurement of any drone node using the IMU pre-integration method and the NC error state pre-integration measurement of the ordinary drone node using the NC error state pre-integration method.

[0008] Construct a factor graph; if the UAV receives an altimeter measurement, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and an altimeter measurement factor to the factor graph; if the UAV receives a TOA measurement, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and a TOA measurement factor to the factor graph; if the UAV receives a message from the UAV node NC, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and an equivalent altimeter measurement factor to the factor graph; when adding a new variable node to the factor graph, a priori factors also need to be added to the factor graph and connected to the oldest variable node within the sliding window; when adding a new variable node to the factor graph, the variable node also needs to be connected to the IMU constant bias factor.

[0009] Construct a factor graph optimization problem based on all variable nodes and factor nodes in the factor graph, calculate the cost function, and output the posterior estimate of the UAV state at the current time.

[0010] Based on the posterior estimation of the UAV's state, the relative position estimate of the UAV in the relative coordinate system is calculated.

[0011] Furthermore, the position, velocity, and attitude of any UAV node i with respect to itself are estimated as follows:

[0012]

[0013] Where i∈[0,N-1], and i=0 is an NC node; The quaternions represent the ECEF frame position coordinates, ECEF frame velocity, and attitude quaternions of the carrier frame (b frame) relative to the ECEF frame at time m; m-1 and m represent the start and end times of this inertial navigation solution, respectively; Δt m,m-1 The time interval is from time m-1 to time m. Let m be the rotation matrix corresponding to the attitude quaternion at time m-1; For Δt m,m-1 The speed increment within, This indicates the specific force output by the accelerometer; Let m be the gravitational acceleration in the ECEF frame at time m-1; Angular velocity of Earth's rotation in the ECEF system; quaternion The corresponding rotation vector is Δt m,m-1 Angular increment Δθ within m , Represents the angular velocity output by the gyroscope; quaternion and rotation matrix This is caused by the Earth's rotation, and their rotation vectors are:

[0014] The extrapolation of the NC error state for the modeling of the ordinary UAV node is as follows:

[0015]

[0016] Where i∈[1,N-1]; φ m,i,NC Δt represents the NC error state modeled at node i at time m; m,m-1 Indicates the discretization time; This represents the antisymmetric matrix corresponding to the projection vector of the gravitational acceleration at the position of NC at time k-1 in the ECEF frame. The optimal position estimate is obtained using the NC broadcast. Calculations show that... The antisymmetric matrix corresponding to the velocity vector of the ECEF system of NC at time k-1 can be obtained by using the optimal velocity estimate of NC broadcast; This represents the antisymmetric matrix corresponding to the Earth's rotational angular velocity vector in the ECEF system.

[0017] Furthermore, the calculation of IMU pre-integration measurements for any UAV node i, i∈[0,N-1] using the IMU pre-integration method is specifically as follows:

[0018] Step 1.1: Using the frequency of the inertial navigation calculation as a reference, calculate the IMU velocity pre-integration increment for any UAV node in each inertial navigation calculation;

[0019]

[0020] Where m-1 and m represent the start and end times of the inertial navigation calculation, respectively, and the time interval is from time k-1 to time k, where m-1≥k-1 and m≤k; Let be the attitude rotation matrix at time k-1; rotation matrix and This is caused by the Earth's rotation, and the corresponding rotation vector can be represented as... It is the rotation matrix corresponding to the attitude pre-integration from time k-1 to time m-1; This represents the velocity increment obtained from the accelerometer corresponding to this inertial navigation calculation;

[0021] Step 1.2: Update IMU location pre-integration:

[0022]

[0023] in, and The IMU position pre-integrations from time k-1 to time m and from time k-1 to time m-1 are respectively, and there are Pre-integrate the IMU velocity from time k-1 to time m-1, and have The IMU velocity pre-integration increment from time m-1 to time m; Δt m,m-1 The time interval is from time m-1 to time m.

[0024] Step 1.3: Update IMU velocity pre-integral:

[0025]

[0026] in, and The IMU velocity pre-integrations are from time k-1 to time m and from time k-1 to time m-1, respectively.

[0027] Step 1.4: Update IMU attitude pre-integration:

[0028]

[0029] in, and The IMU attitude pre-integrations from time k-1 to time m and from time k-1 to time m-1 are respectively, and there are The attitude quaternion is the angle increment output by the IMU from time m-1 to time m;

[0030] Step 1.5: Update the IMU pre-integrated measurement noise covariance matrix:

[0031]

[0032] in, Φ m Calculated using the following formula:

[0033] Φ m =exp(F(t) m )Δt m,m-1 )≈I+F(t m )Δt m,m-1

[0034]

[0035] Wherein F(t) m ) and Φ m These represent the continuous and discrete state transition matrices, respectively. This represents the accumulation from time k-1 to time t. m The rotation matrix corresponding to the attitude pre-integration quaternion at time step; For t m The specific force measurement at time t represents the projection of the specific force in the carrying system (frame b) relative to the inertial frame (frame i) into the carrying system (frame b). This is an estimate of the zero bias of the accelerometer constant at time k-1; For t m The angular velocity measurement value output by the gyroscope at any given time. This is an estimate of the gyroscope's constant zero bias at time k-1;

[0036] Q m Calculated using the following formula:

[0037]

[0038] Where, σ g ,σ a These are the standard deviations of the gyroscope random walk and the accelerometer random walk, respectively.

[0039] Step 1.6: Repeat steps 2.1 to 2.5 to calculate the IMU pre-integration measurements over the entire interval:

[0040]

[0041] in, These represent the position pre-integration measurement, velocity pre-integration measurement, and attitude pre-integration measurement accumulated from time k-1 to time k, respectively.

[0042] The IMU pre-integral measurement about its own direct state calculated by human node i is represented as:

[0043]

[0044] The measurement noise covariance matrix is ​​calculated by accumulating the measurement noise covariance matrix corresponding to the IMU pre-integration measurements throughout the entire pre-integration interval.

[0045] Furthermore, the calculation of the NC error state pre-integration measurement for the ordinary UAV node i, i∈[1,N] using the NC error state pre-integration is specifically as follows:

[0046] Step 2.1: The pre-integral measurement of the NC error state is the zero vector:

[0047]

[0048] Step 2.2: Calculate the measurement noise covariance matrix for each push position;

[0049]

[0050] in,

[0051] Φ i,NC,m The calculation is performed using the following formula:

[0052] Φ i,NC,m =exp(F i,NC (t m )Δt m,m-1 )≈I+F i,NC (t m )Δt m,m-1

[0053]

[0054] Q i,NC,m Calculated using the following formula:

[0055]

[0056] in, Let σ be the attitude rotation matrix corresponding to the attitude quaternion of NC at time k-1. g,NC ,σ a,NC These are the standard deviations of the NC's gyroscope random walk and accelerometer random walk, respectively.

[0057] Step 2.3: Accumulate and calculate the NC error state pre-integration measurement noise covariance matrix over the entire interval.

[0058] Furthermore, when any UAV node obtains an altitude measurement in the geographic coordinate system using its onboard altimeter, the corresponding measurement is:

[0059]

[0060] Where i∈[0,N-1]; The function representing the altitude measurement. h represents the position state vector. i,k This represents the true altitude of any node i in the northeast-northeast coordinate system at time k. σ ba The standard deviation is measured for height measurement.

[0061] Furthermore, when a regular drone node obtains a message broadcast from the NC node using its onboard data link, the corresponding TOA pseudorange measurement is:

[0062]

[0063] Where i∈[1,N-1]; j∈[0,N-1] / i; and These are the grid coordinates of a normal node i and the grid coordinates of an arbitrary node j, respectively. This represents the TOA pseudorange measurement function; σ toa The standard deviation of the TOA pseudorange measurement;

[0064]

[0065] in, This represents the true value of the ECEF coordinates of a normal node i at time k. This represents the projection of the true value of the inertial navigation error of NC at time k onto the ECEF frame.

[0066] Furthermore, when a regular drone node receives a message broadcast from the NC node using its onboard data link, the corresponding equivalent altitude measurement is as follows:

[0067]

[0068] Where i∈[1,N-1]; x represents the equivalent height measurement function; i,NC,k h represents the position error state of the NC modeled by UAV i; NC,k This represents the true altitude of NC in the geographic coordinate system at time k. σba The standard deviation is measured for height measurement.

[0069] Furthermore, the cost function of the factor graph optimization problem is:

[0070]

[0071] in, This indicates the IMU pre-integration measurement residual; The pre-integrated measurement residual represents the NC error state; This indicates the residual value measured at height. This indicates the residual of the TOA pseudorange measurement; This indicates the residual of the equivalent height measurement.

[0072] (1) The pre-integration measurement residual of any UAV node IMU is:

[0073]

[0074] Where i∈[0,N-1]; [·] v This indicates taking the imaginary part of a quaternion; These are the Coriolis correction terms in the pre-integration of position and velocity:

[0075]

[0076] (2) The pre-integral measurement residual of the NC error state of a typical UAV node is:

[0077]

[0078] Where i∈[1,N-1]; F i,NC (t k ) required and Calculated using the following formula:

[0079]

[0080] in, and These represent the projections of the velocity and position indicated by the NC's inertial navigation system at time k-1 into the ECEF frame, obtained through messages broadcast by the NC.

[0081] (3) The residual of the measurement factor for the altitude of any UAV node is:

[0082]

[0083] Where i∈[0,N-1];

[0084] (4) The residual of the TOA pseudorange measurement factor for ordinary UAV nodes is:

[0085]

[0086] Where i∈[1,N-1]; j∈[0,N-1] / i;

[0087] (5) The residual of the equivalent altitude measurement factor for ordinary UAV nodes is:

[0088]

[0089] Where i∈[1,N-1];

[0090] (6) The residual of the constant bias factor for any UAV node is:

[0091]

[0092] Where i∈[0,N-1].

[0093] The relative position of the UAV in the relative coordinate system is estimated as follows:

[0094]

[0095] in, This represents the estimated ECEF coordinates of ordinary node i at time k; This represents the projection of the inertial navigation error of the ordinary node i at time k onto the ECEF frame; Let k be the projection of the position indicated by the NC inertial navigation system in the ECEF frame.

[0096] The present invention also provides a computer device / equipment / system, including a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the multi-UAV dual-grid relative navigation method based on factor graph optimization described above.

[0097] The present invention also provides a computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the multi-UAV dual-grid relative navigation method based on factor graph optimization described above.

[0098] The present invention also provides a computer program product, including a computer program / instruction that, when executed by a processor, implements the steps of the factor graph-optimized multi-UAV dual-grid relative navigation method described above.

[0099] The beneficial effects of this invention are as follows:

[0100] This invention provides a distributed relative navigation method based on factor graph optimization for multi-UAV cooperative localization. This method proposes a novel dual-absolute navigation state model, significantly improving the accuracy of the state model. Based on this, a dual-grid UAV relative navigation framework based on factor graph optimization is proposed, effectively mitigating the impact of measurement nonlinearity and weak observability on positioning accuracy. In practical applications, ranging measurements are typically nonlinear, and measurement observability is affected by relative maneuvers between nodes. The proposed method can significantly solve these problems and has practical significance. Attached Figure Description

[0101] Figure 1 A flowchart of a multi-UAV dual-grid relative navigation method based on factor graph optimization is provided for the implementation of this invention;

[0102] Figure 2 This is the trajectory diagram used for testing in this invention;

[0103] Figure 3 This is the formation diagram used for testing in this invention;

[0104] Figure 4 The parameter values ​​used in the implementation of this invention;

[0105] Figure 5 The relative position estimation accuracy provided for the implementation of this invention in a GNSS completely denied environment;

[0106] Figure 6 The relative position estimation accuracy under different initial errors in a GNSS completely denied environment provided for the implementation of this invention;

[0107] Figure 7 This invention provides relative position estimation accuracy under different TOA ranging accuracies in a GNSS full rejection environment. Detailed Implementation

[0108] The present invention will now be further described with reference to the accompanying drawings.

[0109] This invention discloses a multi-UAV dual-grid relative navigation method based on factor graph optimization, comprising:

[0110] Step 1: Each node in the drone swarm establishes its designed state model;

[0111] Step 2: Each node in the drone swarm establishes its designed measurement model;

[0112] Step 3: Construct communication data packets under the time-division multiple access round-robin broadcast communication mechanism;

[0113] Step 4: When the UAV receives a measurement, add the variable node corresponding to the current state and the factor node corresponding to the measurement to the factor graph, construct the factor graph optimization problem and solve it, and output the maximum a posteriori estimate of the UAV state at the current time.

[0114] Step 5: Based on the optimized posterior state estimate, calculate the relative position estimate of all UAVs in the relative coordinate system.

[0115] In step one above, the state is first defined:

[0116] Assume there are N nodes i in the cluster, one of which is an NC (no node), i = 0, and the rest are ordinary nodes, i ∈ [1, N]. For the NC, its absolute state needs to be modeled, as shown below:

[0117]

[0118] in, Here are the ECEF coordinates of NC. The velocity of NC in the ECEF system. Let be the attitude quaternion of the NC carrier coordinate system (b system) relative to the ECEF system.

[0119] For a normal node i, in addition to modeling its own absolute state, it is also necessary to model the error state of NC, which takes the following form:

[0120]

[0121] Among them, the first 10 dimensions of absolute state are the same as the absolute state of NC modeling itself. The ECEF coordinate position error of NC modeled for node i The ECEF system velocity error of the NC modeled for node i. The attitude misalignment angle of the NC modeled for node i.

[0122] In addition, all nodes need to model their own gyroscope bias and accelerometer bias, specifically as follows:

[0123]

[0124] Where ε is the gyroscope bias. This refers to the accelerometer bias. It's important to note that in the sliding window of factor graph optimization, each variable node corresponds to only one set of IMU bias states. That is, for any node i, if there are L variable nodes in the sliding window, then all the states in the sliding window are... The total state dimension is 19L+6 (the state dimension of NC is 10L+6).

[0125] In step two above, the pre-integral measurement of the NC error state is defined as the zero vector:

[0126]

[0127] in, This represents the pre-integral measurement of the NC error state modeled from time k-1 to time k for ordinary node i.

[0128] In step three above, this invention uses a time-division multiple access (TDMA) round-robin broadcast communication mechanism. Each node in the cluster broadcasts in a fixed order and at a fixed broadcast period. The specific content of the broadcast message data packet is as follows:

[0129] (1) Message data packets broadcast by the NC node

[0130] Node ID; optimal estimates of its own position, velocity, and attitude; inertial navigation system (INS) calculations of its own position, velocity, and attitude; and the most recently received altitude measurement.

[0131] (2) Message data packets broadcast by ordinary nodes (non-NC)

[0132] Node ID; optimal estimates of its own position, velocity, and attitude.

[0133] Step four above specifically includes:

[0134] (1) Set the prior factors at the initial moment, and then use the state propagation model to push the position of its own navigation state and the modeled NC error state, and calculate the corresponding IMU pre-integration measurement and NC error state pre-integration measurement.

[0135] (2) When a normal node i in the cluster receives any measurement other than IMU measurement, such as barometric altimeter, TOA measurement or equivalent altimeter measurement, it will add an IMU pre-integration measurement factor and an NC error state pre-integration measurement factor node to the factor graph and connect them to the corresponding variable node.

[0136] (3) When a relative measurement is generated between a normal node i and a node j in the cluster, a relative measurement factor node will be added to the factor graph and connected to the corresponding variable node.

[0137] (3) When a normal node i in the cluster receives a message broadcast from NC, it will add an equivalent height measurement factor node to the factor graph and connect it to the corresponding variable node.

[0138] (4) When a normal node i in the cluster receives a measurement and adds a variable node to the factor graph, it will add an IMU constant bias factor to the factor graph and connect it to the latest variable node.

[0139] (5) When the number of IMU pre-integration measurements in the sliding window exceeds the set sliding window length, perform an edge-out operation to edge out the oldest variable node.

[0140] (6) Construct a factor graph optimization problem based on all variable nodes and factor nodes in the factor graph, calculate the residual function of all factor nodes and add it to the cost function. The cost function is composed of the Mahalanobis distance of the residuals of all measured factors, as shown below:

[0141]

[0142] By minimizing the cost function using the Ceres solver, the maximum a posteriori estimate of the node state can be obtained.

[0143] Step five above specifically includes:

[0144] Based on the posterior state estimates of all UAV nodes, their coordinate estimates in the relative coordinate system are calculated using the following formula:

[0145]

[0146] This outputs valid relative position information.

[0147] Example 1

[0148] A multi-UAV dual-grid relative navigation method based on factor graph optimization includes:

[0149] Step 1: Define the state model and measurement model of the UAV, and determine the relevant input parameters such as... Figure 4 As shown.

[0150] like Figure 2 The image shows the trajectory of the drone, where phases 1 and 2 use formation 1, and phase 3 uses formation 2. Figure 3 The drone formation is assumed to consist of N drone nodes i, one of which is an NC node i = 0, and the rest are ordinary nodes i ∈ [1, N-1]. Each drone is equipped with an IMU for pose estimation, an altimeter to obtain the altitude measurement in the geographic coordinate system of its location, and a data link for communication and ranging between drone nodes. The inertial navigation estimation formula for each drone is as follows:

[0151]

[0152] Where i∈[0,N-1], and i=0 is an NC node; The quaternions represent the ECEF frame position coordinates, ECEF frame velocity, and attitude quaternions of the carrier frame (b frame) relative to the ECEF frame at time m; m-1 and m represent the start and end times of this inertial navigation solution, respectively; Δt m,m-1 The time interval is from time m-1 to time m. Let m be the rotation matrix corresponding to the attitude quaternion at time m-1; For Δt m,m-1 The speed increment within, This indicates the specific force output by the accelerometer; Let m be the gravitational acceleration in the ECEF frame at time m-1; Angular velocity of Earth's rotation in the ECEF system; quaternion The corresponding rotation vector is Δt m,m-1 Angular increment Δθ within m , Represents the angular velocity output by the gyroscope; quaternion and rotation matrix This is caused by the Earth's rotation, and their rotation vectors can be represented as:

[0153]

[0154] Since ordinary nodes model the error state of the NC, they need to extrapolate the error state of the NC during inertial navigation system (INS) positioning. The extrapolation formula for the NC error state is as follows:

[0155]

[0156] in, φ m,i,NC Δt represents the NC error state modeled at node i at time m; m,m-1 Indicates the discretization time; This represents the antisymmetric matrix corresponding to the projection vector of the gravitational acceleration at the position of NC at time k-1 in the ECEF frame. The optimal position estimate is obtained using the NC broadcast. Calculations show that... The antisymmetric matrix corresponding to the velocity vector of the ECEF system of NC at time k-1 can be obtained by using the optimal velocity estimate of NC broadcast; This represents the antisymmetric matrix corresponding to the Earth's rotational angular velocity vector in the ECEF system.

[0157] When any UAV node obtains an altitude measurement in the northeast-northeast coordinate system using its onboard altimeter, the corresponding measurement is described as follows:

[0158]

[0159] in, The function representing the altitude measurement. h represents the position state vector. i,k This represents the true altitude of any node i in the northeast-northeast coordinate system at time k. σ ba The standard deviation is measured for height measurement.

[0160] When a regular drone node receives a message broadcast from an NC node using its onboard data link, the corresponding TOA pseudorange measurement is described as follows:

[0161]

[0162] Where i∈[1,N]; j∈[0,N] / i; and Let i be the grid coordinates of a normal node i and the grid coordinates of an arbitrary node j, respectively. This represents the TOA pseudorange measurement function. σ toa This is the standard deviation of the TOA pseudorange measurement. Grid coordinates are defined by the following formula:

[0163]

[0164] in, This represents the true value of the ECEF coordinates of a normal node i at time k. Let represent the projection of the true value of the inertial navigation error of NC at time k onto the ECEF frame. Correspondingly, the grid coordinate estimate is expressed by the following formula:

[0165]

[0166] in, This represents the projection of the inertial navigation error of the ordinary node i at time k onto the ECEF frame; Let k be the projection of the position indicated by the NC inertial navigation system in the ECEF frame.

[0167] When a regular drone node receives a message broadcast from the NC node via its onboard data link, the corresponding equivalent altitude measurement is described as follows:

[0168]

[0169] in, x represents the equivalent height measurement function. i,NC,k h represents the position error state of the NC modeled by UAV i. NC,k This represents the true altitude of NC in the northeast-northeast coordinate system at time k. σ ba The standard deviation is measured for height measurement.

[0170] Then, input the initial position, velocity, attitude and initial error of each UAV, input parameters such as sliding window length L and number of optimization iterations M, and initialize the IMU pre-integration and NC error state pre-integration.

[0171] Step 2: Each UAV uses the velocity increment and angular increment output by the IMU to perform its own pose estimation, and uses IMU pre-integration to calculate the IMU pre-integration measurement and the corresponding measurement noise covariance matrix.

[0172] Step 2.1: Using the frequency of the inertial navigation system (INS) calculation as a reference, calculate the IMU velocity pre-integration increment for each INS calculation:

[0173]

[0174] Where m-1 and m represent the start and end times of the inertial navigation calculation, respectively, and the time interval is from time k-1 to time k, where m-1≥k-1 and m≤k; Let be the attitude rotation matrix at time k-1; rotation matrix and This is caused by the Earth's rotation, and the corresponding rotation vector can be represented as... It is the rotation matrix corresponding to the attitude pre-integration from time k-1 to time m-1; This represents the velocity increment output by the IMU during this inertial navigation calculation.

[0175] Step 2.2: Update IMU location pre-integration:

[0176]

[0177] in, and The IMU position pre-integrations from time k-1 to time m and from time k-1 to time m-1 are respectively, and there are Pre-integrate the IMU velocity from time k-1 to time m-1, and have The IMU velocity pre-integration increment from time m-1 to time m; Δt m,m-1 The time interval is from time m-1 to time m.

[0178] Step 2.3: Update IMU velocity pre-integral:

[0179]

[0180] in, and The IMU velocity pre-integrations are from time k-1 to time m and from time k-1 to time m-1, respectively.

[0181] Step 2.4: Update IMU attitude pre-integration:

[0182]

[0183] in, and The IMU attitude pre-integrations from time k-1 to time m and from time k-1 to time m-1 are respectively, and there are The attitude quaternion is the angle increment output by the IMU from time m-1 to time m;

[0184] Step 2.5: Update the IMU pre-integrated measurement noise covariance matrix:

[0185]

[0186] in, Φ m Calculated using the following formula:

[0187] Φ m =exp(F(t) m )Δt m,m-1 )≈I+F(t m )Δt m,m-1

[0188]

[0189] Wherein F(t) m ) and φ m These represent the continuous and discrete state transition matrices, respectively. This represents the accumulation from time k-1 to time t. m The rotation matrix corresponding to the attitude pre-integration quaternion at time step; For t m The specific force measurement at time t represents the projection of the specific force in the carrying system (frame b) relative to the inertial frame (frame i) into the carrying system (frame b). This is an estimate of the zero bias of the accelerometer constant at time k-1; For t m The angular velocity measurement value output by the gyroscope at any given time. This is an estimate of the gyroscope's constant zero bias at time k-1;

[0190] Q m Calculated using the following formula:

[0191]

[0192] Where, σ g ,σ a These are the standard deviations of the gyroscope random walk and the accelerometer random walk, respectively.

[0193] Step 2.6: Repeat steps 2.1 to 2.5 to calculate the IMU pre-integration measurements over the entire interval:

[0194]

[0195] in, These represent the position pre-integration measurement, velocity pre-integration measurement, and attitude pre-integration measurement accumulated from time k-1 to time k, respectively.

[0196] Accumulate and calculate the measurement noise covariance matrix corresponding to the IMU pre-integration measurements over the entire interval.

[0197] Step 3: The ordinary UAV uses the NC broadcast message to estimate the NC error state, and uses the NC error state pre-integration to calculate the NC error state pre-integration measurement and the corresponding measurement noise covariance matrix. Similar to IMU pre-integration, the calculation process of the corresponding measurement noise covariance matrix needs to be synchronized with the NC error state propagation process.

[0198] Step 3.1: Calculate the measurement noise covariance matrix for each push-out:

[0199]

[0200] in, Φ i,NC,m The calculation is performed using the following formula:

[0201] Φ i,NC,m =exp(F i,NC (t m )Δt m,m-1 )≈I+F i,NC (t m )Δt m,m-1

[0202]

[0203] Q i,NC,m Calculated using the following formula:

[0204]

[0205] in, Let σ be the attitude rotation matrix corresponding to the attitude quaternion of NC at time k-1. g,NC ,σ a,NC These are the gyroscope random walk and accelerometer random walk of the NC, respectively.

[0206] Step 3.2: Iteratively calculate the NC error state pre-integration measurement noise covariance matrix over the entire interval.

[0207] Step 4: When the UAV receives external measurements (such as altimeter measurements or NC broadcast messages; the NC itself will not receive these broadcast messages), the methods provided in Steps 2 and 3 are used to calculate the IMU pre-integrated measurement, IMU pre-integrated measurement noise covariance matrix, NC error state pre-integrated measurement (defined as a zero vector), and NC error state pre-integrated measurement noise covariance matrix from the last time the external measurement was received to the current time. Corresponding variable nodes (i.e., the current state) and factor nodes (i.e., measurements) are then added to the factor graph. The residual calculation formulas for the measurement factor nodes and the Jacobian calculation formulas for the residuals with respect to the state are also provided.

[0208] The expression for the IMU pre-integral measurement residual is as follows:

[0209]

[0210] in, [·] v This indicates taking the imaginary part of the quaternion. These are the Coriolis correction terms in the pre-integration of position and velocity:

[0211]

[0212] Therefore, the IMU pre-integration measurement residual is related to the absolute state at the previous moment. The Jacobian matrix is:

[0213]

[0214] Where L[·] represents the left multiplication matrix in quaternion multiplication, R[·] represents the right multiplication matrix in quaternion multiplication, and only the bottom right 3x3 matrix of the result is taken, and we have:

[0215]

[0216] IMU pre-integration measurement residuals for the current state The Jacobian matrix is:

[0217]

[0218] The expression for the pre-integral measurement residual under NC error conditions is given below:

[0219]

[0220] Among them, F i,NC (t k ) required and Calculated using the following formula:

[0221]

[0222] in, and These represent the projections of the velocity and position indicated by the NC's inertial navigation system at time k-1 into the ECEF frame, obtained through messages broadcast by the NC.

[0223] Therefore, the Jacobian matrix of the pre-integral measurement residual of the NC error state with respect to the state at the previous time step can be obtained as follows:

[0224]

[0225] The Jacobian matrix of the pre-integrated measurement residual of the NC error state with respect to the current state is:

[0226]

[0227] Step 5: When adding a variable node to the factor graph, connect the IMU constant bias factor node to the latest variable node.

[0228] The IMU constant bias factor at any node i, its residuals, and the Jacobian matrix of the residuals with respect to the state are given below:

[0229]

[0230] Step 6: If the drone receives an altimeter measurement, add an altimeter factor node to the factor graph and connect it to the corresponding variable node.

[0231] The residual expression for the altimeter measurement factor is:

[0232]

[0233] in, This represents the estimated altitude in the geographic coordinate system obtained from the absolute state of node i at time k (i.e., solving the latitude, longitude, and altitude coordinates in the Northeast Sky coordinate system using ECEF coordinates, and only taking the altitude dimension).

[0234] The Jacobian matrix of the altitude measurement residual with respect to the state is:

[0235]

[0236] in, Let represent the three-axis coordinates of node i in the ECEF system at time k, and we have:

[0237]

[0238] Among them, L i,k R represents the latitude value of node i at time k; adenoted as the semi-major axis in the Earth's rotating ellipsoid model; e is the first eccentricity of the Earth's rotating ellipsoid model.

[0239] Step 7: After a relative measurement is generated between ordinary node i and node j in the cluster, add a relative measurement factor section to the factor graph and connect it to the corresponding variable node. The formulas for calculating the residuals of the corresponding factor nodes and the Jacobian matrix of the residuals with respect to the state are also provided.

[0240] The residual expression for the TOA pseudorange measurement factor is given below:

[0241]

[0242] Correspondingly, the Jacobian matrix of the TOA pseudorange measurement residuals with respect to the state is:

[0243]

[0244] Step 8: If the drone receives a message from the NC broadcast, add an equivalent altitude measurement factor node to the factor graph and connect it to the corresponding variable node.

[0245] The residual expression for the equivalent height measurement factor is given below:

[0246]

[0247] in, This indicates that the NC inertial navigation error estimated by node i is used to correct the NC inertial navigation calculation result, and the height estimate of NC in the northeast-northeast coordinate system is calculated using the corrected position.

[0248] Correspondingly, the Jacobian matrix of the equivalent height measurement residual with respect to the state is:

[0249]

[0250] in, The Jacobian matrix of the height measurement residuals with respect to the state is the same as that in step 6, we have:

[0251]

[0252] Step 9: Construct a factor graph optimization problem based on all variable nodes and factor nodes in the factor graph. Calculate the residual function for all factor nodes and add it to the cost function. The cost function is composed of the Mahalanobis distances of all factor residuals. Iterate and optimize using the Ceres solver, outputting the latest state estimate of the variable nodes at the current time. Iterative optimization using the Ceres solver: During the solution process, the Ceres solver automatically calls the residual calculation methods given in steps 4, 5, 6, and 7, as well as the Jacobian matrix calculation method for residuals versus states, to calculate the total cost of the cost function. The parameter values ​​corresponding to the variable nodes are adjusted using the Jacobian matrix to reduce the total cost. The solution process ends when the maximum number of iterations is reached or the optimization condition is met.

[0253]

[0254] Step 10: k = k + 1, jump to step 4.

[0255] The results show that the average relative position estimation accuracy under a completely denied GNSS environment is as follows: Figure 5 As shown. The parameters used in the simulation are listed below. Figure 4 The relative position estimation accuracy under different initial position error settings in a GNSS completely denied environment is as follows: Figure 6 As shown. The relative position estimation accuracy under different ranging accuracies in a GNSS completely denied environment is as follows: Figure 7 As shown.

[0256] It can be seen that the proposed method greatly improves the accuracy of state modeling and reduces its impact on relative position estimation. The proposed method can still achieve high-precision relative position estimation under different measurement nonlinearity scenarios, which proves that the method is effective.

[0257] In particular, in some preferred embodiments of the present invention, a computer device is also provided, including a memory and a processor and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the multi-UAV dual-grid relative navigation method based on factor graph optimization described in any of the above embodiments.

[0258] In some other preferred embodiments of the present invention, a computer-readable storage medium is also provided, on which a computer program / instruction is stored, wherein when the computer program is executed by a processor, the steps of the multi-UAV dual-grid relative navigation method based on factor graph optimization described in any of the above embodiments are implemented.

[0259] Those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the above embodiments of the multi-UAV dual-grid relative navigation method based on factor graph optimization, which will not be repeated here.

[0260] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., refer to specific features, structures, materials, or characteristics described in connection with that embodiment or example, which are included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0261] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of that feature. In the description of this invention, "N" means at least two, such as two, three, etc., unless otherwise explicitly specified.

[0262] Any process or method description in the flowchart or otherwise herein can be understood as representing a module, segment, or portion of code comprising one or more N executable instructions for implementing custom logic functions or processes, and the scope of preferred embodiments of the invention includes additional implementations in which functions may be performed not in the order shown or discussed, including substantially simultaneously or in reverse order depending on the functions involved, as should be understood by those skilled in the art to which embodiments of the invention pertain.

[0263] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a sequenced list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by, or in conjunction with, an instruction execution system, apparatus, or device (such as a computer-based system, a processor-included system, or other system that can fetch and execute instructions from, an instruction execution system, apparatus, or device). For the purposes of this specification, "computer-readable medium" can be any means that can contain, store, communicate, propagate, or transmit programs for use by, or in conjunction with, an instruction execution system, apparatus, or device. More specific examples (a non-exhaustive list) of computer-readable media include: an electrical connection having one or more wires (electronic device), a portable computer disk drive (magnetic device), random access memory (RAM), read-only memory (ROM), erasable and editable read-only memory (EPROM or flash memory), fiber optic devices, and portable optical disc read-only memory (CDROM). Alternatively, the computer-readable medium may be paper or other suitable media on which the program can be printed, since the program can be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, interpreting, or otherwise processing as necessary, and then stored in a computer memory.

[0264] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0265] Those skilled in the art will understand that all or part of the steps of the methods in the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, the program includes one or a combination of the steps of the method embodiments.

[0266] Furthermore, the functional units in the various embodiments of the present invention can be integrated into a processing module, or each unit can exist physically separately, or two or more units can be integrated into a module. The integrated module can be implemented in hardware or as a software functional module. If the integrated module is implemented as a software functional module and sold or used as an independent product, it can also be stored in a computer-readable storage medium.

[0267] The storage medium mentioned above can be a read-only memory, a disk, or an optical disk, etc. Although embodiments of the present invention have been shown and described above, it is to be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A multi-UAV double-grid relative navigation method based on factor graph optimization, characterized in that, include: drone swarm Each drone node is divided into One ordinary UAV node and one NC node; acquire the output data of the gyroscope and accelerometer in the UAV IMU and the position and velocity estimation data broadcast by the NC, calculate the pose estimation of any UAV node and the position of the UAV node. The modeling results of the NC error state estimation, the IMU pre-integration measurement calculated by any UAV node using the IMU pre-integration method, and the NC error state pre-integration measurement calculated by ordinary UAV node using the NC error state pre-integration. Construct a factor graph; if the UAV receives an altimeter measurement, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and an altimeter measurement factor to the factor graph; if the UAV receives a TOA measurement, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and a TOA measurement factor to the factor graph; if the UAV receives a message from the UAV node NC, add an IMU pre-integration measurement factor, an NC error state pre-integration measurement factor, and an equivalent altimeter measurement factor to the factor graph; when adding a new variable node to the factor graph, a priori factors also need to be added to the factor graph and connected to the oldest variable node within the sliding window; when adding a new variable node to the factor graph, the variable node also needs to be connected to the IMU constant bias factor. Construct a factor graph optimization problem based on all variable nodes and factor nodes in the factor graph, calculate the cost function, and output the posterior estimate of the UAV state at the current time. Based on the posterior estimation of the UAV's state, the relative position estimate of the UAV in the relative coordinate system is calculated.

2. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 1, characterized in that, any drone node The inferences regarding its own position, velocity, and attitude are as follows: in, , For NC nodes; express The ECEF system position coordinates, ECEF system velocity and the load system at that time The attitude quaternion relative to the ECEF system; and These represent the start and end times of the inertial navigation calculation, respectively. for Time's up The time interval between moments; In order to be in Rotation matrix corresponding to the attitude quaternion at time step; for The speed increment within, , This indicates the specific force output by the accelerometer; In order to be in Gravitational acceleration in the ECEF frame at any given time; Angular velocity of Earth's rotation in the ECEF system; quaternion The corresponding rotation vector is Inner angle increment , , Represents the angular velocity output by the gyroscope; quaternion and rotation matrix This is caused by the Earth's rotation, and their rotation vectors are: ; The ordinary drone node The extrapolation of the NC error state in the model is as follows: in, ; express Time Node Modeling the NC error state; Indicates the discretization time; express The antisymmetric matrix corresponding to the projection vector of the gravitational acceleration at the position of NC at time ____ in the ECEF frame is used to estimate the optimal position using the broadcast from NC. Calculations show that... express The antisymmetric matrix corresponding to the velocity vector of the ECEF system at time NC can be calculated using the optimal velocity estimate broadcast by NC. This represents the antisymmetric matrix corresponding to the Earth's rotational angular velocity vector in the ECEF system.

3. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 2, characterized in that, any drone node , The specific steps for calculating IMU pre-integration measurements using IMU pre-integration are as follows: Step 1.1: Using the frequency of the inertial navigation system (INS) calculation as a reference, calculate the IMU velocity pre-integration increment for each INS calculation; in, and These represent the start and end times of the inertial navigation calculation, respectively. Time's up Sub-time periods of time, , ; for Attention rotation matrix at time step; rotation matrix and This is caused by the Earth's rotation, and the corresponding rotation vector can be represented as... ; From Time's up The rotation matrix corresponding to the pre-integration of the attitude at time t; This represents the velocity increment output by the IMU during this inertial navigation calculation. Step 1.2: Update IMU location pre-integration: in, and They are respectively Time's up Time and Time's up The IMU position pre-integration at time t, and has ; for Time's up The IMU velocity pre-integration at time t, and has ; for Time's up IMU velocity pre-integral increment at time step; Step 1.3: Update IMU velocity pre-integral: in, and They are respectively Time's up Time and Time's up IMU velocity pre-integration at time t; Step 1.4: Update IMU attitude pre-integration: in, and They are respectively Time's up Time and Time's up IMU attitude pre-integration at time step, and has ; for Time's up The attitude quaternion corresponding to the angle increment output by the IMU at time t; Step 1.5: Update the IMU pre-integrated measurement noise covariance matrix: in, , Calculated using the following formula: in, and These represent the continuous and discrete state transition matrices, respectively. Indicates from Accumulated time The rotation matrix corresponding to the attitude pre-integration quaternion at time step; , for The specific force measurement value at a given time indicates the load system, i.e. The specific force in a frame relative to an inertial frame, i.e. The system is based on the carrier system. Projection in the system, for The estimated value of the zero bias of the accelerometer constant at any given time; , for The angular velocity measurement value output by the gyroscope at any given time. for The estimated value of the constant zero bias of the gyroscope at any given time; Calculated using the following formula: in, These are the standard deviations of the gyroscope random walk and the accelerometer random walk, respectively. Step 1.6: Repeat steps 1.1 to 1.5 to calculate the IMU pre-integration measurements over the entire interval: in, They represent from Accumulated time Position pre-integration measurement, velocity pre-integration measurement, and attitude pre-integration measurement at each moment; drone nodes The calculated IMU pre-integral measurements about its own direct state are expressed as: ； The measurement noise covariance matrix is ​​calculated by superimposing the measurement noise covariance matrix corresponding to the IMU pre-integration measurements throughout the entire interval.

4. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 3, characterized in that, The ordinary drone node , The specific calculation of NC error state pre-integration measurement using NC error state pre-integration is as follows: Step 2.1: The pre-integral measurement of the NC error state is the zero vector: ； Step 2.2: Calculate the measurement noise covariance matrix for each push position; in, ; The calculation is performed using the following formula: Calculated using the following formula: in, for The attitude rotation matrix corresponding to the attitude quaternion at time NC. These are the gyroscope random walk and accelerometer random walk of the NC, respectively. Step 2.3: Accumulate and calculate the NC error state pre-integration measurement noise covariance matrix over the entire interval. .

5. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 4, characterized in that, When any UAV node obtains an altitude measurement in the northeast-northeast coordinate system using its onboard altimeter, the corresponding measurement is: in, ; The function representing the altitude measurement. Represents the position state vector. express any node at any time The actual altitude in the northeast celestial coordinate system , The standard deviation is measured for height measurement.

6. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 5, characterized in that, When a regular drone node receives a message broadcast from the NC node using its onboard data link, the corresponding TOA pseudorange measurement is: in, ; ; and They are ordinary nodes. Grid coordinates and arbitrary nodes Grid coordinates; This represents the TOA pseudorange measurement function; , The standard deviation of the TOA pseudorange measurement; in, express ordinary nodes at any time The true values ​​of ECEF coordinates. express The projection of the true value of the inertial navigation error at time NC onto the ECEF frame.

7. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 6, characterized in that, When a regular drone node receives a message broadcast from the NC node via its onboard data link, the corresponding equivalent altitude measurement is as follows: in, ; Represents the equivalent height measurement function; Indicates drone The position error status of the modeled NC; express The actual altitude of NC in the geographic coordinate system at any given time. , The standard deviation is measured for height measurement.

8. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 7, characterized in that, The cost function for the factor graph optimization problem is: in, This indicates the IMU pre-integration measurement residual; The pre-integrated measurement residual represents the NC error state; This indicates the residual value measured at height. This indicates the residual of the TOA pseudorange measurement; This indicates the residual of the equivalent height measurement.

9. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 8, characterized in that, (1) The pre-integration measurement residual of any UAV node IMU is: in, ; , This indicates taking the imaginary part of a quaternion; These are the Coriolis correction terms in the pre-integration of position and velocity: (2) The pre-integral measurement residual of the NC error state of a typical UAV node is: in, ; What is needed in and Calculated using the following formula: in, and They represent The velocity and position indicated by the NC's inertial navigation system at any given time, projected onto the ECEF frame, are obtained through messages broadcast by the NC. (3) The residual of the measurement factor for the altitude of any UAV node is: in, ; (4) The residual of the TOA pseudorange measurement factor for ordinary UAV nodes is: in, ; ; (5) The residual of the equivalent altitude measurement factor for ordinary UAV nodes is: in, ; ; (6) The residual of the constant bias factor for any UAV node is: in, .

10. The multi-UAV dual-grid relative navigation method based on factor graph optimization according to claim 9, characterized in that, The relative position of the UAV in the relative coordinate system is estimated as follows: in, express ordinary nodes at any time The estimated values ​​of the ECEF coordinates; express ordinary nodes at any time The projection of the estimated NC inertial navigation error into the ECEF frame; for The projection of the position indicated by the NC inertial navigation system in the ECEF frame at any given time.

Images