Trustworthiness-driven integrated navigation method and system based on NV color center quantum magnetometer

Abstract

A reliability-driven integrated navigation method and system based on an NV center quantum magnetometer is disclosed. The reliability-driven integrated navigation method based on the NV center quantum magnetometer includes the following steps: defining and initializing a platform navigation vector and its covariance matrix, wherein the platform navigation vector includes navigation state parameters and platform magnetic interference parameters; predicting the platform navigation vector at a later time based on the platform navigation vector at a previous time based on IMU measurement data; and updating the platform navigation vector at the later time based on GNSS observation data and NV center quantum magnetometer observation data.

Description

Technical Field

[0001] This disclosure relates to the field of navigation and positioning technology, specifically to a reliability-driven integrated navigation method and system based on an NV color center quantum magnetometer, namely, an integrated navigation method and system that integrates an NV color center quantum magnetometer, an inertial navigation system (INS), and a satellite navigation system (GNSS). Background Technology

[0002] Existing integrated navigation systems typically fuse a satellite navigation system (GNSS) and an inertial navigation system (INS) to calculate position, velocity, and attitude. The INS provides continuous state propagation, while the satellite navigation system provides absolute observation constraints; the two work together to perform recursive state estimation. In real-world operating environments, GNSS signal quality can fluctuate due to obstruction, multipath effects, electromagnetic interference, or changes in propagation conditions. When observation quality changes, using a fixed or empirically defined observation covariance model for fusion often fails to accurately reflect changes in observation reliability. Summary of the Invention

[0003] This disclosure provides a reliability-driven integrated navigation method based on an NV color center quantum magnetometer, comprising the following steps:

[0004] Define and initialize the platform navigation vector and its covariance matrix, wherein the platform navigation vector includes navigation state parameters and platform magnetic disturbance parameters;

[0005] Predict the platform navigation vector in the next time step based on the platform navigation vector in the previous time step using IMU measurement data;

[0006] The platform navigation vector at the later time step is updated based on GNSS observation data and NV color center quantum magnetometer observation data.

[0007] In some embodiments, the platform navigation vector x is represented by the following formula:

[0008]

[0009] The navigation state parameters include p, v, q, and b. g and b a p represents the 3D position, v represents the 3D velocity, q represents the attitude quaternion, and b represents the position. g For zero bias of the gyroscope, b a For accelerometer zero bias, the platform's magnetic interference parameters include b. m With S m b m S is the hard ferromagnetic interference vector. m This is the soft ferromagnetic distortion matrix.

[0010] In some embodiments, predicting the platform navigation vector at a later time based on the platform navigation vector at a previous time step using IMU measurement data includes:

[0011] The platform navigation vector at later time steps is predicted by performing navigation state propagation, magnetic interference parameter propagation, and covariance matrix propagation corresponding to the platform navigation vector.

[0012] In some embodiments, updating the platform navigation vector at a later time step based on GNSS observation data and NV color center quantum magnetometer observation data includes the following steps:

[0013] Acquire observation data and mass parameters from GNSS and NV color center quantum magnetometers;

[0014] Perform confidence calculations for GNSS and NV color center quantum magnetometers;

[0015] The platform navigation vector update is driven by the aforementioned credibility.

[0016] In some embodiments, acquiring observation data and mass parameters from GNSS and NV color center quantum magnetometers includes the following steps:

[0017] Acquire GNSS observations and GNSS quality parameters, the GNSS observations including position observations. and velocity observations The GNSS quality parameters include the satellite's carrier-to-noise ratio C / N0 and the number of visible satellites N. sat Geometric precision factor (DOP);

[0018] Acquire NV magnetic observations and quantum physics measurement quality parameters, wherein the NV magnetic observations include triaxial magnetic vector observations z. B The quantum physics measurement quality parameters include the ODMR spectral linewidth. Fluorescence contrast ratio (C), optical signal-to-noise ratio (SNR), and frequency locking error signal amplitude (e) lock .

[0019] In some embodiments, performing a confidence calculation for GNSS and NV color center quantum magnetometers includes the following steps:

[0020] The reliability of GNSS observations, c, is calculated using the following formula. gnss

[0021]

[0022] in, For signal quality components; This is a component representing the residual consistency. For cross-sensor consistency components; , , These are weighting coefficients, and ;

[0023] The confidence level c of NV magnetic observation is calculated using the following formula. nv

[0024] in, ODMR spectral linewidth, C is fluorescence contrast ratio, SNR is optical signal-to-noise ratio, e lock This represents the amplitude of the frequency locking error signal.

[0025] The reliability of GNSS observations was assessed using a first-order low-pass filter. gnss and the credibility of NV magnetic observations c nv Execution time is smooth.

[0026] In some embodiments, driving the platform navigation vector update at a later time step based on the confidence level includes the following steps:

[0027] Perform residual calculation and decomposition;

[0028] Calculate the component-level update coefficients; and

[0029] Update the platform navigation vector in the later time step.

[0030] In some embodiments, performing residual calculation and decomposition includes the following steps:

[0031] For each available observation, the residual is calculated using the following formula.

[0032]

[0033] in, For residuals, For the observed values, For the corresponding observation function, The platform navigation vector at time k is predicted from the platform navigation vector at time k-1.

[0034] The NV magnetic observation residuals are linearized and decomposed using the following formula.

[0035]

[0036] in, For NV magnetic observation residuals, Let Jacobian matrix be the attitude error matrix; For hard ferromagnetic interference parameters, Jacobian matrix, The Jacobian matrix represents the soft ferromagnetic interference parameters. To minimize attitude error; For hard ferromagnetic interference parameter error, This is due to the error in the soft ferromagnetic interference parameters; This is magnetic observation noise;

[0037] Positioning attitude error energy and magnetic interference error energy To determine the main sources of NV magnetic observation residuals:

[0038]

[0039] in, , The projection matrix can be generated from the basis vectors of the subspace constructed from the Jacobian matrix.

[0040] In some embodiments, calculating the component-level update coefficients includes:

[0041] Based on residual decomposition results and the reliability of NV magnetic observations Calculate the update coefficients of the attitude state components. and the update coefficients of the magnetic interference state components

[0042]

[0043]

[0044] in, To prevent small positive numbers from being divided by zero.

[0045] In some embodiments, updating the platform navigation vector at a later time step includes the following steps:

[0046] Calculate the standard Kalman gain;

[0047] Based on the update coefficients of the attitude state components and the update coefficients of the magnetic interference state components Adjust the gain;

[0048] Update the platform navigation vector at the later time step using the adjusted gain;

[0049] The covariance matrix corresponding to the platform navigation vector is updated based on the standard Kalman gain.

[0050] Some embodiments of this disclosure also provide a reliability-driven integrated navigation system based on an NV color center quantum magnetometer, the reliability-driven integrated navigation system based on an NV color center quantum magnetometer comprising:

[0051] Inertial measurement module, configured to acquire IMU measurement data;

[0052] The satellite navigation module is configured to acquire GNSS observation data;

[0053] The NV color center quantum magnetometer is configured to acquire NV magnetic observation data; and

[0054] The processor is configured to execute the credibility-driven integrated navigation method described in any of the foregoing embodiments.

[0055] This disclosure introduces an NV color center quantum magnetometer into a navigation system, and incorporates platform magnetic interference parameters as dynamically evolving state variables into a unified estimation framework. During normal operation, these parameters are continuously recursively estimated without relying on offline calibration, thereby improving the long-term stability of magnetic observations, reducing the risk of continuous contamination of attitude calculations by magnetic interference, and achieving high precision, high reliability, and high adaptability of the integrated navigation system under high-sensitivity magnetic measurement conditions. Attached Figure Description

[0056] Figure 1 This is a schematic diagram illustrating a scenario of reliability-driven integrated navigation based on an NV color center quantum magnetometer, provided for some embodiments of this disclosure.

[0057] Figure 2 A flowchart of a credibility-driven integrated navigation method based on an NV color center quantum magnetometer provided for some embodiments of this disclosure.

[0058] Figure 3 for Figure 2 The detailed flowchart of step S300.

[0059] Figure 4 for Figure 3 The detailed flowchart of step S310.

[0060] Figure 5 for Figure 3 The detailed flowchart of step S320.

[0061] Figure 6 for Figure 3 The detailed flowchart of step S330.

[0062] Figure 7 for Figure 6 The detailed flowchart of step S331.

[0063] Figure 8 for Figure 6 The detailed flowchart of step S333.

[0064] Figure 9 This is a schematic diagram of the structure of a reliability-driven integrated navigation system based on an NV color center quantum magnetometer, provided for some embodiments of this disclosure. Detailed Implementation

[0065] To make the objectives, technical solutions, and advantages of this disclosure clearer, the disclosure will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this disclosure, and not all of them. Based on the embodiments of this disclosure, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this disclosure.

[0066] The terminology used in the embodiments of this disclosure is for the purpose of describing particular embodiments only and is not intended to be limiting of this disclosure. The singular forms “a,” “the,” and “the” as used in the embodiments of this disclosure and the appended claims are also intended to include the plural forms, and “multiple” generally includes at least two unless the context clearly indicates otherwise.

[0067] It should be understood that the term "and / or" used in this article is merely a description of the relationship between related objects, indicating that three relationships can exist. For example, A and / or B can represent: A existing alone, A and B existing simultaneously, and B existing alone. Additionally, the character " / " in this article generally indicates that the preceding and following related objects have an "or" relationship.

[0068] It should be understood that although the terms first, second, third, etc. may be used to describe the embodiments in this disclosure, it should not be limited to these terms.

[0069] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that an article or device that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such an article or device. Without further limitation, an element defined by the phrase "comprising one" does not exclude the presence of other identical elements in the article or device that includes said element.

[0070] The platform described in this disclosure refers to a carrier platform that carries a navigation system, such as a moving vehicle carrying navigation equipment, including vehicles, ships, aircraft, robots, airborne / shipborne / vehicle-mounted platforms, etc. The platform will experience attitude movements and position changes, and will have its own magnetic interference (current, motors, ferromagnetic structures), making it the measurement object and operating environment of the system.

[0071] In related technologies, magnetic-assisted navigation, as a physical constraint source independent of external wireless signals, has been introduced into integrated navigation systems to provide supplementary constraints when GNSS observation quality deteriorates. Through geomagnetic field vector constraints, additional observational information can be provided for attitude calculation. However, traditional magnetoresistive or fluxgate sensors suffer from limited sensitivity and significant drift in engineering applications, and their output signals typically lack physical quality parameters that directly reflect measurement accuracy. Furthermore, magnetic interference from the platform itself is often handled through offline calibration or fixed compensation models, usually assuming that the magnetic interference parameters are constant.

[0072] With the development of NV color center high-precision quantum magnetometer technology, the sensitivity of magnetic observation has been significantly improved, enabling high-precision measurement of minute magnetic field changes. Under high-sensitivity conditions, the impact of platform magnetic interference on magnetic observation becomes more significant. During operation, changes in platform load, current distribution, or actuator status can all cause dynamic changes in magnetic interference parameters. If a fixed compensation model or simple covariance adjustment method is still used, it may lead to a deviation between the magnetic observation model and the actual operating conditions.

[0073] Meanwhile, in the process of multi-source fusion, the quality of different observation sources usually exhibits dynamic changes. Traditional methods often use fixed weights or simple gating mechanisms to handle this, lacking a unified modeling structure for the continuous changes in observation quality and their impact on the convergence process of weakly observable states.

[0074] This disclosure provides a reliability-driven integrated navigation method and system based on an NV center quantum magnetometer. The reliability-driven integrated navigation method includes the following steps: defining and initializing a platform navigation vector and its covariance matrix, wherein the platform navigation vector includes navigation state parameters and platform magnetic interference parameters; predicting the platform navigation vector at a later time based on the platform navigation vector at a previous time based on IMU measurement data; and updating the platform navigation vector at the later time based on GNSS observation data and NV center quantum magnetometer observation data. This addresses the technical problems in related technologies, such as the difficulty in online dynamic compensation of platform magnetic interference, the lack of a continuous characterization mechanism for multi-source observation quality fluctuations, and the susceptibility of weakly observable states to contamination by anomalous observations during the fusion process. It achieves high precision, high reliability, and high adaptability of the integrated navigation system under high-sensitivity magnetic measurement conditions.

[0075] The optional embodiments of this disclosure are described in detail below with reference to the accompanying drawings.

[0076] Figure 1 This is a schematic diagram of a scenario for a reliability-driven integrated navigation system based on an NV color center quantum magnetometer, provided by some embodiments of this disclosure. It integrates an NV color center quantum magnetometer, an inertial navigation system (INS), and a satellite navigation system (GNSS), and is suitable for navigation scenarios where there are fluctuations in observation quality or changes in operating conditions.

[0077] Figure 2 A flowchart illustrating a reliability-driven integrated navigation method based on an NV color center quantum magnetometer, provided for some embodiments of this disclosure. Figure 2 As shown, the reliability-driven integrated navigation method based on the NV color center quantum magnetometer includes the following steps:

[0078] S100: Define and initialize the platform navigation vector and its covariance matrix, wherein the platform navigation vector reflects the platform magnetic interference parameters;

[0079] S200: Predicts the platform navigation vector in the next time step based on the platform navigation vector in the previous time step using IMU measurement data;

[0080] S300: Update the platform navigation vector at the later time step based on GNSS observation data and NV color center quantum magnetometer observation data.

[0081] In this embodiment, the platform magnetic interference parameters are introduced into the platform navigation vector as dynamically evolving state variables, realizing online recursive estimation and dynamic compensation of magnetic interference within a unified state estimation framework. The magnetic interference parameters are continuously updated during navigation operation based on the difference between magnetic observation residuals and state predictions, without relying on offline calibration results, thereby adapting to the dynamic evolution of magnetic disturbances caused by changes in operating conditions.

[0082] In step S100, the platform magnetic interference parameters are introduced into the platform navigation vector as dynamically evolving state variables. These parameters include a hard ferromagnetic interference vector and a soft ferromagnetic distortion matrix. The hard ferromagnetic interference vector represents the fixed additional magnetic field generated by permanent magnets, current loops, etc., on the platform, manifested as a constant bias in the magnetic observations. The soft ferromagnetic distortion matrix represents the scaling, coupling, and distortion effects of the platform's soft magnetic materials on the Earth's magnetic field, manifested as a linear transformation of the Earth's magnetic field vector. In some embodiments, the platform navigation vector x is expressed by the following formula:

[0083]

[0084] The navigation state parameters include p, v, q, and b. g and b a p represents the 3D position, v represents the 3D velocity, q represents the attitude quaternion, and b represents the position. g For zero bias of the gyroscope, b a For accelerometer zero bias, the platform's magnetic interference parameters include b. m With S m b m S is the hard ferromagnetic interference vector. m It is a soft ferromagnetic distortion matrix, for example, a three-dimensional square matrix.

[0085] The covariance matrix is ​​a square matrix with the same dimension as the state vector. The diagonal elements represent the variance of the estimation error of each state, and the larger the value, the greater the uncertainty. The off-diagonal elements represent the error correlation between states.

[0086] During the initialization phase, the platform navigation vector and its covariance matrix need to be set synchronously. The platform navigation vector represents the optimal state point estimate of the system, including all estimated states such as position, velocity, attitude, sensor zero bias, and magnetic interference parameters. The covariance matrix represents the variance and correlation of the state estimation errors, reflecting the degree of uncertainty in the state estimation. The two constitute the complete prior information for filtering, which is synchronously recursively derived and corrected during state propagation and measurement updates. The covariance matrix adaptively adjusts the intensity of the state vector update by controlling the Kalman gain, jointly ensuring the consistency and stability of the integrated navigation estimation.

[0087] During initialization, position and velocity can be obtained from GNSS or manually given, attitude can be obtained through initial alignment, zero bias can be set to the factory nominal value or zero, magnetic interference parameters can be set to zero or offline calibration value, and covariance is set according to prior uncertainty.

[0088] In some embodiments, step S200: predicting the platform navigation vector at a later time based on the platform navigation vector at a previous time based on IMU measurement data includes:

[0089] By performing navigation state propagation, magnetic interference parameter propagation, and propagation of the covariance matrix corresponding to the platform navigation vector, the platform navigation vector and its covariance matrix at later time steps are predicted.

[0090] At each IMU sampling time (prediction, e.g., 100Hz), navigation state prediction is performed based on IMU measurement data, i.e., state propagation.

[0091] Specifically, the navigation state propagation is performed using the strapdown inertial navigation system's mechanical equations, which are as follows:

[0092]

[0093]

[0094]

[0095] Where p is the platform's three-dimensional position vector in the navigation coordinate system; v is the platform's three-dimensional velocity vector in the navigation coordinate system; and q is a unit quaternion describing the platform's attitude, satisfying... ; For accelerometer zero bias, in the equation This indicates that the effect of zero bias has been subtracted; With zero bias for the gyroscope, the true angular velocity is ; , These are the measurements from the accelerometer and gyroscope, respectively; R(q) is the rotation matrix corresponding to the attitude quaternion; It is the acceleration due to gravity. For the angular velocity vector The constructed quaternion right-multiplication matrix has the following form:

[0096]

[0097] In practice, discrete-time forms are used, such as fourth-order Runge-Kutta integrals or simple Euler integrals, to obtain the predicted navigation state parameters.

[0098] For the propagation of magnetic interference parameters, an NV magnetic observation model incorporating platform magnetic interference parameters is constructed:

[0099]

[0100] in, The magnetic observation value at time k; This is the attitude rotation matrix from the world coordinate system to the platform coordinate system; This is the ambient magnetic field vector; Let be the platform's three-dimensional position vector at time k; This is the hard ferromagnetic interference vector; This is the soft ferromagnetic distortion matrix; For measuring noise.

[0101] The dynamic evolution model of magnetic interference parameters is as follows:

[0102]

[0103]

[0104] in, , These are the hard ferromagnetic interference vector and the soft ferromagnetic distortion matrix at time k, respectively. , The process noise is used to describe the slow changes in magnetic interference parameters or the changes in the correlation ratio of operating conditions.

[0105] This dynamic evolution model does not assume that magnetic disturbances are strictly constant, but allows them to be gradually modified during navigation operation, thereby obtaining the predicted platform magnetic disturbance parameters.

[0106] The predicted platform navigation vector is obtained by combining the predicted navigation state parameters and the predicted platform magnetic disturbance parameters. ,in, Let represent the predicted platform navigation vector at time k, where the navigation state parameters are obtained by integrating the updated state at time k-1 using IMU data. The system uses the IMU sampling frequency as the main prediction period, and updates are triggered at the corresponding time by aligning the timestamps of the GNSS and NV color center quantum magnetometer observations. Here, k represents the current time index, k-1 represents the previous time index, k is a positive integer, and k≥1.

[0107] The covariance matrix corresponding to the platform navigation vector is propagated according to the standard extended Kalman filter formula:

[0108]

[0109] in, Let be the navigation state covariance matrix at the previous time step, i.e., time step k-1; The state transition Jacobian matrix; This is the noise driving matrix; The system noise covariance matrix contains IMU measurement noise and magnetic interference random walk noise.

[0110] Figure 3 for Figure 2 A detailed flowchart of step S300 is provided in some embodiments, such as... Figure 3 As shown, step S300: updating the platform navigation vector at the later time step based on GNSS observation data and NV color center quantum magnetometer observation data includes the following steps:

[0111] S310: Acquire observation data and mass parameters from GNSS and NV color center quantum magnetometer;

[0112] S320: Performs reliability calculations for GNSS and NV color center quantum magnetometers;

[0113] S330: Drive the platform navigation vector update at a later time based on the credibility.

[0114] In steps S310 and S320, at each observation time (e.g., GNSS 10Hz, NV color center quantum magnetometer 50Hz), the processing unit 140 acquires the observation data and related quality parameters of each sensor.

[0115] Figure 4 for Figure 3 A detailed flowchart of step S310 is provided in some embodiments, such as... Figure 4 As shown, step S310: Obtaining observation data and mass parameters from GNSS and NV color center quantum magnetometers includes the following steps:

[0116] S311: Acquire GNSS observations and GNSS quality parameters, the GNSS observations including position observations. and velocity observations The GNSS quality parameters include the satellite's carrier-to-noise ratio C / N0 and the number of visible satellites N. sat Geometric precision factor (DOP);

[0117] Specifically, GNSS observations are performed to obtain location observation values. and velocity observations Simultaneously record the carrier-to-noise ratio C / N0 and the number of visible satellites N for each satellite. sat Quality parameters such as geometric precision factor (DOP).

[0118] S312: Obtain NV magnetic observations and quantum physics measurement mass parameters, wherein the NV magnetic observations include triaxial magnetic vector observations z. B The quantum physics measurement quality parameters include the ODMR spectral linewidth. Fluorescence contrast ratio (C), optical signal-to-noise ratio (SNR), and frequency locking error signal amplitude (e) lock .

[0119] Specifically, observations were performed using the NV color center quantum magnetometer to obtain the triaxial magnetic vector observation value z. B Simultaneously record the ODMR spectral linewidth Fluorescence contrast ratio (C), optical signal-to-noise ratio (SNR), and frequency locking error signal amplitude (e) lock Quantum physics measurement mass parameters.

[0120] Figure 5 for Figure 3 A detailed flowchart of step S320 is provided in some embodiments, such as... Figure 5 As shown, step S320: Performing a confidence calculation for the GNSS and NV color center quantum magnetometer includes the following steps:

[0121] S321: The reliability of GNSS observations is calculated using the following formula. gnss

[0122]

[0123] in, The signal quality component is calculated based on the average carrier-to-noise ratio, the number of satellites, and the geometric precision factor DOP. For the residual consistency component, the platform navigation vector predicted based on the current epoch is used. The normalized information and its covariance were obtained by chi-square test. This calculation was completed before the state update and does not constitute a time-series dependency. As a cross-sensor consistency component, the rate of change of magnetic observation residual amplitude calculated using the predicted state before and after GNSS observation triggering is evaluated to quantify the consistency of multi-source observations in physical space. , , These are weighting coefficients, and ;

[0124] S322: The reliability of NV magnetic observations is calculated using the following formula. nv

[0125] in, ODMR spectral linewidth, C is fluorescence contrast ratio, SNR is optical signal-to-noise ratio, e lock This represents the amplitude of the frequency locking error signal.

[0126] In a specific embodiment, the NV magnetic observation confidence level c is calculated using a weighted product method. nv :

[0127]

[0128] in, , These represent the typical minimum and maximum linewidths of ODMR spectra; The minimum acceptable signal-to-noise ratio; The maximum permissible frequency locking error; , , , It is a weighted index, which can be set through experimental calibration or experience.

[0129] S323: GNSS observation reliability c through first-order low-pass filtering gnss and the credibility of NV magnetic observations c nv Execution time is smooth.

[0130] Specifically, to avoid system oscillations caused by sudden changes in credibility, a first-order low-pass filter is introduced for time smoothing:

[0131]

[0132] in, The confidence level after smoothing at time k is... The instantaneous reliability calculated directly from S321 or S322 for the current epoch (i.e., or (current value) The smoothing coefficient is typically set to 0.6–0.9. 'i' represents the observation source dimension, distinguishing different sensors / observation channels, such as NV color center quantum magnetometers and GNSS. 'k' represents the time epoch dimension, distinguishing different sampling times.

[0133] Construct continuous confidence variables for various observation sources It is used to describe the overall reliability of the observed source at the current moment.

[0134] Figure 6 for Figure 3 A detailed flowchart of step S330 is provided in some embodiments, such as... Figure 6 As shown, step S330: driving the platform navigation vector update at a later time based on the credibility includes the following steps:

[0135] S331: Perform residual calculation and decomposition;

[0136] S332: Calculate the component-level update coefficients; and

[0137] S333: Update the platform navigation vector in a later time step.

[0138] Within the unified state estimation framework, the confidence variable directly participates in the observation weight adjustment and state update process, realizing the structural coupling between the change in observation quality and the state estimation process; the fusion process does not use discrete mode switching, but drives the natural evolution of the fusion mode through continuous changes in confidence.

[0139] Figure 7 for Figure 6 The detailed flowchart of step S331 is shown in some embodiments, such as... Figure 6 As shown, step S331: Calculating and decomposing the observation execution residuals includes the following steps:

[0140] S3311: For each available observation, calculate the residual using the following formula.

[0141]

[0142] in, For residuals, For the observed values, For the corresponding observation function, The platform navigation vector at time k is predicted from the platform navigation vector at time k-1.

[0143] For NV magnetic observations, the observation function is:

[0144]

[0145] Among them, R wb To predict attitude A defined rotation matrix from the world coordinate system to the platform coordinate system; To determine the predicted location Environmental magnetic field vectors obtained from geomagnetic field models (such as IGRF and WMM); , These are the predicted hard ferromagnetic interference vector and soft ferromagnetic distortion matrix, respectively.

[0146] For GNSS observations, the position and velocity residuals are calculated simultaneously using the following formula.

[0147]

[0148] It should be noted that the residual structure decomposition and energy projection in this embodiment are only performed on the NV magnetic observation channel. The GNSS residual is mainly used for the confidence component calculation and does not participate in the Jacobian subspace decomposition in this step.

[0149] S3312: The NV magnetic observation residuals are linearized and decomposed using the following formula.

[0150]

[0151] in, For NV magnetic observation residuals, Let Jacobian matrix be the attitude error matrix; For hard ferromagnetic interference parameters, Jacobian matrix, The Jacobian matrix represents the soft ferromagnetic interference parameters. To minimize attitude error; For hard ferromagnetic interference parameter error, This is due to the error in the soft ferromagnetic interference parameters; This is magnetic observation noise;

[0152] Positioning attitude error energy and magnetic interference error energy To determine the residuals of NV magnetic observations Main source:

[0153]

[0154] in, , The projection matrix can be generated from the basis vectors of the subspace constructed from the Jacobian matrix.

[0155] In some embodiments, step S332: calculating the component-level update coefficients includes:

[0156] For the NV magnetic observation channel, based on the residual decomposition results and the reliability of NV magnetic observations... Calculate the update coefficients of the attitude state components. and the update coefficients of the magnetic interference state components

[0157]

[0158]

[0159] in, To prevent small positive numbers from being divided by zero.

[0160] It should be noted that the calculation of the above component-level update coefficients is performed only for the NV magnetic observation channel to decouple attitude error from magnetic interference error. GNSS observations update the position / velocity state using standard Kalman gain and do not participate in this magnetic residual component-level decoupling.

[0161] , The update coefficient is jointly determined by the residual components and the confidence level. When the confidence level of a certain observation source decreases, the influence of that observation source on the update of navigation status and platform magnetic interference parameters is automatically reduced.

[0162] Update coefficients of attitude state components and the update coefficients of the magnetic interference state components The following adaptive adjustment logic is achieved by coupling the residual energy ratio with the reliability product:

[0163] (1) When the reliability of NV magnetic observation At higher levels ( →1), the confidence weight term approaches 1, and the update coefficient is mainly determined by the residual energy distribution. At this time, if the magnetic interference error energy Significantly greater than attitude error energy ,but Approaching 1 allows for sufficient updates of magnetic interference parameters to compensate for platform magnetic interference online;

[0164] (2) When the reliability of NV magnetic observation At lower ( →0), Product term in the formula Overall attenuation automatically suppresses the update intensity of magnetic interference parameters, preventing low-quality magnetic observations from causing erroneous convergence of magnetic interference parameters;

[0165] (3) When the attitude error energy Significantly greater than the magnetic interference error energy hour( ≫ Energy ratio term Approaching 1, The dominant approach is to use magnetic vector geometric constraints to correct attitude drift first, while avoiding incorrect estimation of magnetic interference parameters due to attitude error divergence.

[0166] The above mechanism achieves decoupling adjustment of attitude error and magnetic interference error by updating coefficients at the component level, preventing cross-contamination between the two and improving the convergence stability of joint estimation.

[0167] Figure 8 for Figure 6 The detailed flowchart of step S333 is shown in some embodiments, such as... Figure 8 As shown, step S333: updating the platform navigation vector at a later time step includes the following steps:

[0168] S3331: Calculate the standard Kalman gain

[0169] The standard Kalman gain K is calculated using the following formula, without considering component adjustment:

[0170]

[0171] Where H is the observation Jacobian matrix and R is the observation noise covariance matrix, which is a diagonal matrix, and the diagonal elements are the statistical prior values ​​of the observation variance of each sensor channel.

[0172] Divide the gain matrix into blocks according to the state: ,in Corresponding navigation status (position, velocity, attitude, zero bias). Corresponding magnetic interference parameters.

[0173] S3332: Update coefficients based on the attitude state components and the update coefficients of the magnetic interference state components Adjust the gain by dividing the standard Kalman gain matrix K into state blocks. ,in The gain components corresponding to the navigation states (position, velocity, attitude, zero bias), The gain components corresponding to the magnetic interference parameters. The adjusted gain matrix is ​​calculated using the following formula. :

[0174]

[0175] Where K' is the adjusted Kalman gain matrix; The update coefficients for the attitude state components (0≤ ≤1); The update coefficients for the magnetic interference state components (0≤ ≤1). When the confidence level decreases, the corresponding update coefficient decreases, thereby reducing the update intensity of the observation source for the corresponding state component.

[0176] S3333: Update the platform navigation vector at the later time step using the adjusted gain.

[0177] Use the adjusted gain to update the state:

[0178]

[0179] Where r is the observation residual vector.

[0180] S3334: The covariance matrix corresponding to the platform navigation vector is updated based on the standard Kalman gain.

[0181] Specifically, the covariance matrix corresponding to the platform navigation vector is updated based on the standard Kalman gain. The update uses the standard formula (using the unadjusted gain K to ensure consistency):

[0182]

[0183] In some embodiments, as an alternative to gain adjustment, the observation covariance can also be directly adjusted. Specifically, weight adjustment is achieved by scaling the observation noise covariance with confidence level.

[0184] For example, for NV magnetic observations:

[0185]

[0186] For GNSS observations:

[0187]

[0188] in, This is the original observation noise covariance matrix of the NV magnetic observations (a diagonal matrix, with diagonal elements representing the three-axis magnetic measurement variance). This is the raw observation noise covariance matrix of GNSS observations (a diagonal matrix, with diagonal elements representing the position / velocity observation variances). , The confidence levels of smoothed NV magnetic observations and GNSS observations are respectively. To prevent small positive numbers from being divided by zero; , This is the adjusted observation covariance matrix.

[0189] Adjusted K is calculated by substituting it into the standard Kalman gain formula, followed by updating the platform navigation vector and its covariance. When the confidence level decreases... Increasing the Kalman gain decreases the impact of the observation source on state updates. This method is mathematically equivalent to gain adjustment and has the same implementation logic, thus enabling continuous adjustment of update weights based on confidence.

[0190] In some embodiments, such as Figure 2 As shown, the reliability-driven integrated navigation method based on the NV color center quantum magnetometer further includes S400: outputting the updated navigation state at a later time step, i.e., the platform navigation vector at the current time step. Then return to step S200, wait for the IMU data or observation data at the next moment, and continue the recursive estimation.

[0191] Through the above steps, this disclosure achieves a unified closed-loop processing for online modeling of platform magnetic interference, multi-source observation credibility assessment, and component-level adaptive fusion. This method does not rely on discrete mode switching; the fusion mode evolves naturally from the credibility, maintaining high accuracy and stability even in complex environments.

[0192] Those skilled in the art will understand that the specific formulas and parameters used in the above steps are merely examples and can be adjusted as needed in practical applications. For example:

[0193] The state estimation framework can be replaced by unscented Kalman filtering (UKF), particle filtering (PF), or factor graph optimization instead of extended Kalman filtering;

[0194] The credibility mapping function can employ nonlinear models such as neural networks and fuzzy logic;

[0195] It can be expanded to include more observation sources such as odometry and visual SLAM, and corresponding credibility models can be built;

[0196] The evolution model of magnetic interference can be replaced by a first-order Markov process instead of a random walk model.

[0197] These variations all fall within the protection scope of this disclosure.

[0198] Figure 9 This is a schematic diagram illustrating the structure of a reliability-driven integrated navigation system based on an NV color center quantum magnetometer, provided in some embodiments of this disclosure. Figure 9 As shown, the reliability-driven integrated navigation system 100 based on the NV color center quantum magnetometer includes: an inertial measurement module 10, a satellite navigation module 20, an NV color center quantum magnetometer 30, and a processor 40.

[0199] The inertial measurement module 10 is configured to acquire IMU measurement data; the satellite navigation module 20 is configured to acquire GNSS observation data; the NV color center quantum magnetometer 30 is configured to acquire NV magnetic observation data; and the processor 40 is configured to execute the reliability-driven integrated navigation method described in the foregoing embodiments.

[0200] The embodiments disclosed herein provide a reliability-driven integrated navigation method and system based on an NV color center quantum magnetometer, which has the following technical effects:

[0201] The platform's online dynamic compensation capability for magnetic interference: Magnetic interference parameters are introduced into a unified estimation framework as dynamically evolving state variables, and are continuously recursively estimated during normal operation without relying on offline calibration, thereby improving the long-term stability of magnetic observation and reducing the risk of continuous contamination of attitude calculation by magnetic interference.

[0202] Prospective assessment of observation quality based on quantum physics measurement characteristics: Introducing quantum physics measurement quality indicators into the credibility model enables the adjustment of observation weights to have a forward-looking quality assessment capability. When the observation quality shows a downward trend, its update impact can be reduced in advance to avoid premature contamination of the state by abnormal observations.

[0203] Residual source differentiation and weak observability protection: By structurally decomposing the magnetic observation residuals and combining them with a component-level update adjustment mechanism, the structural differentiation and independent update adjustment of attitude error and magnetic interference error are achieved, preventing cross-contamination between attitude error and magnetic interference parameters and improving the convergence stability of joint estimation.

[0204] Continuous credibility-driven fusion natural evolution: Instead of using discrete rule mode switching, the fusion mode is driven to evolve naturally through continuous credibility variables, avoiding mode oscillation and estimation abrupt changes, ensuring the continuity of state updates, and improving the system's ability to smoothly transition in complex environments.

[0205] A structural closed-loop collaborative system is formed: observation physical quality assessment → credibility generation → component level update and adjustment → weak observable state protection forms a structural closed loop, realizing the synergistic enhancement of the stability of observation quality assessment and state estimation.

[0206] The device embodiments described above are merely illustrative. The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the modules can be selected to achieve the purpose of this embodiment according to actual needs. Those skilled in the art can understand and implement this without any creative effort.

[0207] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of this disclosure, and are not intended to limit them. Although this disclosure has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this disclosure.

Claims

1. A reliability-driven integrated navigation method based on an NV color center quantum magnetometer, characterized in that, The reliability-driven integrated navigation method based on the NV color center quantum magnetometer includes the following steps: Define and initialize the platform navigation vector and its covariance matrix, wherein the platform navigation vector includes navigation state parameters and platform magnetic disturbance parameters; Predict the platform navigation vector in the next time step based on the platform navigation vector in the previous time step using IMU measurement data; The platform navigation vector at the later time step is updated based on GNSS observation data and NV color center quantum magnetometer observation data.

2. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 1, characterized in that, The platform navigation vector x is represented by the following formula: The navigation state parameters include p, v, q, b g and b a , p is a three-dimensional position, v is a three-dimensional velocity, q is an attitude quaternion, b g is a gyro zero offset, b a is an accelerometer zero offset, the platform magnetic interference parameters include b m and S m , b m is a hard iron magnetic interference vector, and S m is a soft iron magnetic distortion matrix.

3. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 1, characterized in that, The prediction of the platform navigation vector in the later time step based on the platform navigation vector at the previous time step using IMU measurement data includes: The platform navigation vector at later time steps is predicted by performing navigation state propagation, magnetic interference parameter propagation, and covariance matrix propagation corresponding to the platform navigation vector.

4. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 1, characterized in that, The process of updating the platform navigation vector at a later time step based on GNSS observation data and NV color center quantum magnetometer observation data includes the following steps: Acquire observation data and mass parameters from GNSS and NV color center quantum magnetometers; Perform confidence calculations for GNSS and NV color center quantum magnetometers; The platform navigation vector update is driven by the aforementioned credibility.

5. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 4, characterized in that, Acquiring observational data and mass parameters from GNSS and NV color center quantum magnetometers includes the following steps: Acquire GNSS observations and GNSS quality parameters, the GNSS observations including position observations. and velocity observations The GNSS quality parameters include the satellite's carrier-to-noise ratio C / N0 and the number of visible satellites N. sat Geometric precision factor (DOP); Acquire NV magnetic observations and quantum physics measurement quality parameters, wherein the NV magnetic observations include triaxial magnetic vector observations z. B The quantum physics measurement quality parameters include the ODMR spectral linewidth. Fluorescence contrast ratio (C), optical signal-to-noise ratio (SNR), and frequency locking error signal amplitude (e) lock .

6. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 4, characterized in that, Performing confidence calculations for GNSS and NV color center quantum magnetometers includes the following steps: The reliability of GNSS observations, c, is calculated using the following formula. gnss in, For signal quality components; This is a component representing the residual consistency. For cross-sensor consistency components; , , These are weighting coefficients, and ; The confidence level c of NV magnetic observation is calculated using the following formula. nv in, ODMR spectral linewidth, C is fluorescence contrast ratio, SNR is optical signal-to-noise ratio, e lock This represents the amplitude of the frequency locking error signal. The reliability of GNSS observations was assessed using a first-order low-pass filter. gnss and the credibility of NV magnetic observations c nv Execution time is smooth.

7. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 4, characterized in that, The following steps are included in driving the platform navigation vector update at a later time step based on the aforementioned credibility: Perform residual calculation and decomposition; Calculate the component-level update coefficients; and Update the platform navigation vector in the later time step.

8. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 7, characterized in that, Performing residual calculation and decomposition includes the following steps: For each available observation, the residual is calculated using the following formula. in, For residuals, For the observed values, For the corresponding observation function, The platform navigation vector at time k is predicted from the platform navigation vector at time k-1. The NV magnetic observation residuals are linearized and decomposed using the following formula. in, For NV magnetic observation residuals, Let Jacobian matrix be the attitude error matrix; For hard ferromagnetic interference parameters, Jacobian matrix, The Jacobian matrix represents the soft ferromagnetic interference parameters. To minimize attitude error; For hard ferromagnetic interference parameter error, This is due to the error in the soft ferromagnetic interference parameters; This is magnetic observation noise; Positioning attitude error energy and magnetic interference error energy To determine the main sources of NV magnetic observation residuals: in, , The projection matrix can be generated from the basis vectors of the subspace constructed from the Jacobian matrix.

9. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 7, characterized in that, The calculation of component-level update coefficients includes: Based on residual decomposition results and the reliability of NV magnetic observations Calculate the update coefficients of the attitude state components. and the update coefficients of the magnetic interference state components in, To prevent small positive numbers from being divided by zero.

10. The reliability-driven integrated navigation method based on an NV color center quantum magnetometer according to claim 9, characterized in that... Updating the platform navigation vector in a later time step involves the following steps: Calculate the standard Kalman gain; Based on the update coefficients of the attitude state components and the update coefficients of the magnetic interference state components Adjust the gain; Update the platform navigation vector at the later time step using the adjusted gain; The covariance matrix corresponding to the platform navigation vector is updated based on the standard Kalman gain.

11. A reliability-driven integrated navigation system based on an NV color center quantum magnetometer, characterized in that, The reliability-driven integrated navigation system based on the NV color center quantum magnetometer includes: Inertial measurement module, configured to acquire IMU measurement data; The satellite navigation module is configured to acquire GNSS observation data; The NV color center quantum magnetometer is configured to acquire NV magnetic observation data; and A processor configured to execute the credibility-driven integrated navigation method as described in any one of claims 1 to 10.

Images