CN105716610A - Carrier attitude and heading calculation method assisted by geomagnetic field model and system - Google Patents

Abstract

The invention discloses a carrier attitude and heading calculation method assisted by a geomagnetic field model and a system. The method comprises the following steps that an ideal magnetic inclination angle is calculated according to position information of a carrier and the geomagnetic field model; an estimated magnetic inclination angle is calculated according to an output value of an accelerometer and an output value of a magnetic sensor; whether environmental magnetic interference exists or not is judged according to the ideal magnetic inclination angle and the estimated magnetic inclination angle; if the environmental magnetic interference exists, an equivalent weight matrix of magnetic sensor measurement values is constructed according to self-adaptive weights of the magnetic sensor measurement values; carrier attitude information and heading information are calculated according to an accelerometer measurement value, the magnetic sensor measurement values and the equivalent weight matrix of the magnetic sensor measurement values. According to the carrier attitude and heading calculation method assisted by the geomagnetic field model and the system, the environmental magnetic interference is detected according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, when the environmental magnetic interference is detected, the equivalent weight matrix of the magnetic sensor measurement values is constructed, self-adaptive adjustment of contribution of the magnetic sensor measurement values to a state estimation value is achieved, and the calculated attitude information and heading information have robustness.

Description

Geomagnetic field model-assisted carrier attitude and heading calculation method and system

Technical Field

The invention relates to the technical field of communication, in particular to a method and a system for calculating attitude and heading of a carrier assisted by a geomagnetic field model.

Background

An attitude and heading reference system (AHRS for short) is a measuring device capable of accurately measuring the three-axis attitude of a carrier in a space coordinate system, and is composed of hardware such as a three-axis magnetometer, a three-axis gyroscope, a three-axis accelerometer and the like and corresponding software, provides real-time attitude and heading information for the carrier, and is widely applied to the fields of aerospace, robots, the automobile industry and pedestrian navigation positioning. With the development of micro-electromechanical technology, attitude and heading reference systems based on micro-electromechanical system sensors are more widely applied. However, due to the drift of the gyroscope, the errors are accumulated continuously when the attitude and heading information of the carrier is calculated. In order to solve the problem, information of an accelerometer and a magnetic sensor is introduced into an attitude and heading reference system, a gravity field and a geomagnetic field are used as reference vectors, and attitude and heading information of a carrier is calculated in real time by using output values of the accelerometer and the magnetometer and output values of a gyroscope.

Although attitude and heading reference systems can provide accurate attitude and heading information for a carrier in real time, the carrier moving in the near ground is often interfered by the external environment, and particularly, a magnetic sensor is obviously interfered by environmental magnetism. When external magnetic interference occurs, the output value of the magnetic sensor is used as a measurement value to estimate the attitude and the heading information of the carrier, so that errors exist, and even the attitude and the heading information of the carrier cannot be correctly calculated, and great risk is brought to the autonomous control of the carrier.

Disclosure of Invention

The invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method and system, which aim to solve the technical problem of real-time calculation of carrier attitude and heading in the presence of environmental magnetic interference.

The invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method, which comprises the following steps of:

calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;

judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;

when environmental magnetic interference exists, constructing a self-adaptive weight of a measured value of a magnetic sensor according to a normalized residual error of the measured value of the magnetic sensor and a covariance matrix of the measured value, and constructing an equivalent weight matrix of the measured value of the magnetic sensor according to the self-adaptive weight of the measured value of the magnetic sensor;

and calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.

Optionally, calculating the carrier attitude information and the heading information according to the accelerometer measurement value, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value, specifically including:

updating the state prediction value according to the accelerometer measurement value to obtain a state update value, and updating the estimation value of the attitude quaternion by using the state update value;

updating the state updating value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, and updating the estimation value of the attitude quaternion by using the state estimation value;

and calculating the attitude information and the course information of the carrier according to the estimated value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.

Optionally, the updating the state prediction value according to the accelerometer measurement value to obtain a state update value specifically includes:

updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector;

and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.

Optionally, updating the state update value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, specifically including:

updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model;

and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.

Optionally, the state equation of the Kalman filter specifically includes:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

$W\left(t\right)=\left[\begin{array}{c}{-0.5v}_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1T is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, v_ωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, q_eVector portion being error quaternion, b_ωIs a random constant of the gyroscope, b_aIs the accelerometer bias.

Optionally, the first measurement equation specifically includes:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a$

wherein L is_aFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gⁿIs the projection of the gravitational acceleration in a navigation coordinate system, v_aIs the random error of the accelerometer.

Optionally, the state prediction value is updated according to the Kalman filtered state equation, the first measurement equation, and the accelerometer measurement value, so as to obtain a state update value, specifically:

updating the state predicted value by adopting the following formula to obtain a state updated value:

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)$

$K_{a,k}={\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\mathrm{\Σ}}_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

wherein,the value is updated for the state at time k,is a predicted value of the state at time k, L_a，kThe accelerometer measurement at time k, A_a，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, K_a，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, R_aMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).

Optionally, updating the estimation value of the attitude quaternion by using the state update value, specifically:

updating the estimated value of the attitude quaternion by adopting the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eThe components of the update values for the states,updating the state with the value.

Optionally, the second measurement equation specifically includes:

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m$

wherein L is_mAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mⁿAs a result of the normalization of the ideal geomagnetic field output value, v_mRandom error of the magnetic sensor.

Optionally, updating a state update value according to the Kalman filtered state equation, the second measurement equation, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, specifically:

updating the state update value by adopting the following formula to obtain a state estimation value:

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\mathrm{\Σ}}_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\mathrm{\Σ}}_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}$

${\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

$r_3=C_n^b\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$

wherein,is an estimate of the state at time k,updating the value of the state at time k, L_m，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, K_m，kIs the gain matrix at time k, A_m，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.

Optionally, updating the estimated value of the attitude quaternion by using the state estimated value, specifically:

updating the estimated value of the attitude quaternion by adopting the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eIs a component of the state estimate that is,is the state estimate.

Optionally, the estimated magnetic tilt angle is calculated according to the output value of the accelerometer and the output value of the magnetic sensor, specifically:

the estimated declination angle is calculated using the following formula:

D_cacul＝arccos(n_a·n_m)

wherein D is_caculFor said estimated declination angle, n_aFor the direction vector of the output value of the accelerometer in the carrier system, n_mThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.

Optionally, judging whether there is environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, specifically:

judging whether the ideal magnetic dip angle and the estimated magnetic dip angle meet the following conditions:

|D_cacul-D_reference|＞λ_D

wherein D is_referenceIs the ideal magnetic inclination angle, D_caculFor said estimation of the magnetic tilt angle, λ_DIs a preset threshold value;

if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

Optionally, constructing an adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix, specifically:

constructing an adaptive weight of the magnetic sensor measurement value by adopting the following formula:

in the absence of ambient magnetic interference;

in the presence of ambient magnetic interference;

wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time k_kiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, V_i' is the normalized residual of the magnetic sensor measurements.

Optionally, before constructing the adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix, the method further includes:

calculating a normalized residual of the magnetic sensor measurement values using the following formula:

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2,3}$

wherein L is_miIs the ith measurement equation of the second measurement equation, A_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element.

The invention also provides an attitude and heading reference system, comprising:

the first calculation module is used for calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model;

the second calculation module is used for calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;

the judging module is used for judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;

the first construction module is used for constructing the self-adaptive weight of the magnetic sensor measuring value according to the normalized residual error of the magnetic sensor measuring value and the measuring value covariance matrix when the judgment module judges that the environmental magnetic interference exists;

the second construction module is used for constructing an equivalent weight matrix of the magnetic sensor measurement values according to the self-adaptive weights of the magnetic sensor measurement values;

and the third calculation module is used for calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.

Optionally, the third computing module comprises:

the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;

a second update submodule for updating the estimated value of the attitude quaternion using the state update value;

the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;

a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;

and the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.

Optionally, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.

Optionally, the third update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model; and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.

Optionally, the state equation of the Kalman filter specifically includes:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

$W\left(t\right)=\left[\begin{array}{c}{-0.5v}_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1T is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, v_ωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, q_eVector portion being error quaternion, b_ωIs a random constant of the gyroscope, b_aIs the accelerometer bias.

Optionally, the first measurement equation specifically includes:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a$

wherein L is_aFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gⁿIs the projection of the gravitational acceleration in a navigation coordinate system, v_aIs the random error of the accelerometer.

Optionally, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)$

$K_{a,k}={\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\mathrm{\Σ}}_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

wherein,the value is updated for the state at time k,is a predicted value of the state at time k, L_a，kThe accelerometer measurement at time k, A_a，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, K_a，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, R_aMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).

Optionally, the second updating sub-module is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eThe components of the update values for the states,updating the state with the value.

Optionally, the second measurement equation specifically includes:

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m$

wherein L is_mAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mⁿAs a result of the normalization of the ideal geomagnetic field output value, v_mRandom error of the magnetic sensor.

Optionally, the third update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model, a second measurement equation is constructed, and a state updating value is updated by adopting the following formula to obtain a state estimation value:

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\mathrm{\Σ}}_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\mathrm{\Σ}}_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}$

${\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

$r_3=C_n^b\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$

wherein,is an estimate of the state at time k,updating the value of the state at time k, L_m，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, K_m，kIs the gain matrix at time k, A_m，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.

Optionally, the fourth updating sub-module is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eIs a component of the state estimate that is,is the state estimate.

Optionally, the second calculating module is specifically configured to calculate the estimated magnetic tilt angle by using the following formula:

D_cacul＝arccos(n_a·n_m)

wherein D is_caculFor said estimated declination angle, n_aFor the direction vector of the output value of the accelerometer in the carrier system, n_mFor the magnetic sensorThe vector of the vector is output.

Optionally, the determining module is specifically configured to determine whether the ideal magnetic tilt angle and the estimated magnetic tilt angle satisfy the following conditions:

|D_cacul-D_reference|＞λ_D

wherein D is_referenceIs the ideal magnetic inclination angle, D_caculFor said estimation of the magnetic tilt angle, λ_DIs a preset threshold value;

if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

Optionally, the first constructing module is specifically configured to, when the determining module determines that the environmental magnetic interference exists, construct an adaptive weight of the magnetic sensor measurement value by using the following formula:

in the absence of ambient magnetic interference;

in the presence of ambient magnetic interference;

wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time k_kiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, V_i' is the normalized residual of the magnetic sensor measurements.

Optionally, the system further includes:

a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2,3}$

wherein L is_miIs the ith measurement equation of the second measurement equation, A_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element.

The method detects the environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, constructs an equivalent weight matrix of the measured value of the magnetic sensor when the environmental magnetic interference is detected, and realizes the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value, so that the attitude information and the course information obtained by calculation have the tolerance, can adapt to the geomagnetic distortion environment, and solves the technical problem of real-time calculation of the attitude and the course of a carrier when the environmental magnetic interference exists.

Drawings

FIG. 1 is a flowchart of a geomagnetic field model-assisted carrier attitude and heading calculation method in an embodiment of the present invention;

FIG. 2 is a flowchart of another geomagnetic model-assisted method for calculating attitude and heading of a carrier in an embodiment of the present invention;

fig. 3 is a schematic structural diagram of an attitude and navigation reference system in an embodiment of the present invention.

Detailed Description

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.

The embodiment of the invention provides a geomagnetic field model-assisted carrier attitude and heading calculation method, which comprises the following steps of:

step 101, calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; and calculating the estimated magnetic tilt angle according to the output value of the accelerometer and the output value of the magnetic sensor.

And 102, judging whether environmental magnetic interference exists or not according to the ideal magnetic inclination angle and the estimated magnetic inclination angle.

And 103, when the environmental magnetic interference exists, constructing an adaptive weight of the magnetic sensor measuring value according to the normalized residual error and the measuring value covariance matrix of the magnetic sensor measuring value, and constructing an equivalent weight matrix of the magnetic sensor measuring value according to the adaptive weight of the magnetic sensor measuring value.

And step 104, calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.

The embodiment of the invention detects the environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle, constructs the equivalent weight matrix of the measured value of the magnetic sensor when the environmental magnetic interference is detected, and realizes the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value, so that the attitude information and the course information obtained by calculation have the tolerance, can adapt to the geomagnetic distortion environment, and solves the technical problem of real-time calculation of the attitude and the course of a carrier when the environmental magnetic interference exists.

The embodiment of the invention also provides another geomagnetic field model-assisted carrier attitude and heading calculation method, as shown in FIG. 2, which comprises the following steps:

step 201, calculating the total magnetic field intensity, the magnetic dip angle and the magnetic declination angle of the ideal geomagnetic field according to the position information of the carrier and the geomagnetic field model, and taking the magnetic dip angle of the ideal geomagnetic field as the ideal magnetic dip angle.

Specifically, the geomagnetic field information at the position of the carrier can be calculated in real time according to a world geomagnetic model (WMM) published by the national geographic information system (NGA) and trajectory data of carrier motion, wherein the data is manufactured by the united states geophysical information center (NGDC) and the british geological survey Bureau (BGS), and the geomagnetic field information includes the total magnetic field strength, the declination angle and the declination angle.

In step 202, an estimated magnetic tilt angle is calculated based on the output value of the accelerometer and the output value of the magnetic sensor.

Specifically, the estimated declination angle may be calculated using the following formula:

D_cacul＝arccos(n_a·n_m)(1)

wherein D is_caculTo estimate the magnetic tilt angle, n_aAnd n_mRespectively accelerometer and magnetic sensorThe vector of the vector is output.

And step 203, judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle.

Specifically, it may be determined whether the ideal and estimated declination angles satisfy the following conditions:

|D_cacul-D_reference|＞λ_D(2)

wherein D is_referenceTo an ideal magnetic inclination angle, D_caculTo estimate the magnetic tilt angle, λ_DIs a preset threshold.

If the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

In this embodiment, λ_D＝0.55°。

And 204, updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation.

In particular, the vector portion q of the error quaternion can be taken_eGyroscope random constant value b_ωAnd accelerometer bias b_aAs the state quantities, a state equation of Kalman filtering is constructed:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)---\left(3\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]---\left(4\right)$

$W\left(t\right)=\left[\begin{array}{c}{-0.5v}_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]---\left(5\right)$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1Is a state quantity; t is transposition; f (t) is a state transition matrix; w (t) is process noise;for gyroscope output valuesAn inverse matrix of (d); v. of_ωRandom drift noise for the gyroscope;noise of a random constant model of the gyroscope;random constant model noise for the accelerometer;

and step 205, establishing a measurement equation according to the difference between the projection values of the earth gravity field reference vector and the earth magnetic field reference vector in the carrier coordinate system and the actual measurement values of the accelerometer and the magnetic sensor.

In particular, the difference L between the output value of the accelerometer and the estimated value of the output value of the accelerometer is used_aAnd a difference L between the magnetic sensor output value and the estimated value of the magnetic sensor output value_mRespectively constructing a first measurement equation and a second measurement equation:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a---\left(6\right)$

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m---\left(7\right)$

wherein,is the output value of the accelerometer;is the output value of the magnetic sensor;is a transformation matrix from a navigation coordinate system n to a carrier coordinate system b;is an estimate of a quaternion; gⁿ＝[00-g]^TIs the projection of the gravity acceleration g under the navigation coordinate system n; m isⁿ＝[0cosαsinα]^TNormalized by the ideal geomagnetic field output value α is the magnetic tilt angle v_aAnd v_mRandom errors of the accelerometer and magnetometer, respectively.

In this example, gⁿ＝9.8m/s2。

And step 206, estimating quaternion errors and inertial sensor errors by adopting a Kalman filtering algorithm of two-step observation updating, and realizing the estimation of the attitude and the heading of the carrier through an attitude quaternion updating equation.

Specifically, according to equation (6), the observed value L at time k is used_a，kUpdating the state prediction value at time k

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)---\left(8\right)$

$K_{a,k}={\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}---\left(9\right)$

${\mathrm{\Σ}}_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T---\left(10\right)$

Wherein,is the measurement matrix of measurement equation (6) at time k; i is a unit array;for state prediction at time kAn updated value of (d); k_a，kIs a gain matrix;for state prediction at time kThe covariance matrix of (a); r_aMeasuring a covariance matrix of the noise for the accelerometer;is in a state at time kThe covariance matrix of (a);

in this example, R_a＝(500μg)²I。

Using state quantitiesComponent (b) ofUpdating

$\hat{q}\⇐\hat{q}\⊗q_e---\left(11\right)$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|---\left(12\right)$

Wherein, the symbolMultiplication is carried out for quaternion;is a quaternionNormalization of (1);assigning the calculated value on the right side of the equation to the variable on the left side of the equation for assigning the sign;

according to equation (7), the measured value L at time k is used_m，kUpdating time kObtaining a state estimate at time kCovariance matrix of sum state estimate

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})---\left(13\right)$

${\mathrm{\Σ}}_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\mathrm{\Σ}}_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T---\left(14\right)$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}---\left(15\right)$

${\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]---\left(16\right)$

Wherein, K_m，kA gain matrix for time k; l is_m，kA measurement value calculated by the magnetic sensor for the time k; a. the_m，kA measurement matrix at the time k; an equivalent weight matrix of the measured value of the magnetic sensor at the moment k;

using the state quantities according to equations (11) and (12)Component (b) ofTo updateAnd useTo calculate attitude and heading information of the carrier.

And step 207, utilizing the equivalent weight matrix to online adjust the weight of the measurement value of the magnetic sensor according to the detection result of the environmental magnetic interference in the step 203, so that the calculated attitude and heading information of the carrier have tolerance.

Calculating the normalized residual error of the observed value of the magnetic sensor according to the measurement equation (7) as

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2,3}---\left(17\right)$

Wherein L is_miThe ith measurement equation which is measurement equation (7); a. the_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element;

in this example, R_m＝0.001I；

Constructing a measurement value weight factor of

In the formula,an adaptive weight being an i-th measured value of the magnetic sensor at time k; p is a radical of_kiFor the ith main diagonal element of a given metrology covariance matrix; c is a constant;

in this example, c is 1.3 to 2.0.

The embodiment of the invention utilizes the self-adaptive weight of the measured value of the magnetic sensor at the moment kConstructing an equivalence weight matrix of magnetic sensor measurement valuesAnd the self-adaptive adjustment of the contribution of the measured value of the magnetic sensor to the state estimated value is realized, so that the attitude and heading information calculation has the tolerance.

The invention has the advantages that: the method comprises the following steps that the process of calculating attitude and heading reference system information is realized by adopting a two-step filtering method, and in the first step, an accelerometer is used as a measuring sensor to estimate the error of a quaternion and update the quaternion; and secondly, estimating the error of the quaternion by taking the magnetic sensor as a measuring sensor, detecting the magnetic interference of the environment where the carrier is positioned in real time by using a magnetic interference detection algorithm, adaptively adjusting the weight of the observation data of the magnetic sensor in filtering estimation by the ratio of a measuring residual error to an actual residual error, updating the quaternion and calculating the attitude of the carrier and the heading reference system information. The tolerance and the reliability of the carrier attitude and the heading reference system information are improved.

Based on the above geomagnetic field model-assisted carrier attitude and heading calculation method, an embodiment of the present invention further provides an attitude and heading reference system, as shown in fig. 3, including:

the first calculation module 310 is configured to calculate an ideal magnetic dip angle according to the position information of the carrier and the geomagnetic field model;

a second calculating module 320, configured to calculate an estimated magnetic tilt angle according to the output value of the accelerometer and the output value of the magnetic sensor;

the judging module 330 is configured to judge whether there is environmental magnetic interference according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;

a first constructing module 340, configured to construct an adaptive weight of the magnetic sensor measurement value according to the normalized residual of the magnetic sensor measurement value and the measurement value covariance matrix when the determining module 330 determines that the environmental magnetic interference exists;

a second constructing module 350, configured to construct an equivalent weight matrix of the magnetic sensor measurement values according to the adaptive weights of the magnetic sensor measurement values;

and the third calculating module 360 is configured to calculate the carrier attitude information and the heading information according to the accelerometer measurement value, the magnetic sensor measurement value, and the equivalent weight matrix of the magnetic sensor measurement value.

Wherein, the third calculating module 360 comprises:

the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;

a second update submodule for updating the estimated value of the attitude quaternion using the state update value;

the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;

a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;

and the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.

Specifically, the first update submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.

The third updating submodule is specifically configured to update the model according to the inertial sensor error model and the attitude error quaternion, and construct a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model; and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.

The Kalman filtering state equation specifically comprises the following steps:

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

$W\left(t\right)=\left[\begin{array}{c}{-0.5v}_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1T is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, v_ωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, q_eVector portion being error quaternion, b_ωIs a random constant of the gyroscope, b_aIs the accelerometer bias.

The first measurement equation specifically includes:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a$

wherein L is_aFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gⁿIs the projection of the gravitational acceleration in a navigation coordinate system, v_aIs the random error of the accelerometer.

Correspondingly, the first updating submodule is specifically configured to update a model according to an inertial sensor error model and an attitude error quaternion, and construct a state equation of Kalman filtering; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)$

$K_{a,k}={\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\mathrm{\Σ}}_{{\hat{X}}_{a,k}}=\left({I-K}_{a,k}{A}_{a,k}\right){\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_k}{\left({I-K}_{a,k}{A}_{a,k}\right)}^T+K_{a,k}{R}_a{K}_{a,k}^T$

wherein,the value is updated for the state at time k,is a predicted value of the state at time k, L_a，kThe accelerometer measurement at time k, A_a，kIs the measurement matrix of the first measurement equation at time K, I is the unit matrix, K_a，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, R_aMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).

The second updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eThe components of the update values for the states,updating the state with the value.

The second measurement equation specifically includes:

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m$

wherein L is_mAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mⁿAs a result of the normalization of the ideal geomagnetic field output value, v_mRandom error of the magnetic sensor.

Correspondingly, the third updating submodule is specifically configured to update the model according to the inertial sensor error model and the attitude error quaternion, and construct a state equation of Kalman filtering; according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model, a second measurement equation is constructed, and a state updating value is updated by adopting the following formula to obtain a state estimation value:

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\mathrm{\Σ}}_{{\hat{X}}_k}=\left({I-K}_{m,k}{A}_{m,k}\right){\mathrm{\Σ}}_{{\hat{X}}_{a,k}}{\left({I-K}_{m,k}{A}_{m,k}\right)}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}$

${\mathrm{\Σ}}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\mathrm{\Σ}}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& 0_{3\×6}\\ 0_{6\×3}& 0_{6\×6}\end{array}\right]$

$r_3=C_n^b\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$

wherein,is an estimate of the state at time k,updating the value of the state at time k, L_m，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, K_m，kIs the gain matrix at time k, A_m，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.

The fourth updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eIs a component of the state estimate that is,is the state estimate.

The second calculating module 320 is specifically configured to calculate the estimated magnetic tilt angle by using the following formula:

D_cacul＝arccos(n_a·n_m)

wherein D is_caculFor said estimated declination angle, n_aFor the direction vector of the output value of the accelerometer in the carrier system, n_mThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.

The determining module 330 is specifically configured to determine whether the ideal magnetic tilt angle and the estimated magnetic tilt angle satisfy the following conditions:

|D_cacul-D_reference|＞λ_D

wherein D is_referenceIs the ideal magnetic inclination angle, D_caculFor said estimation of the magnetic tilt angle, λ_DIs a preset threshold value;

if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

The first constructing module 340 is specifically configured to, when the determining module 330 determines that the environmental magnetic interference exists, construct the adaptive weight of the magnetic sensor measurement value by using the following formula:

in the absence of ambient magnetic interference;

in the presence of ambient magnetic interference;

wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time k_kiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, V_i' is the normalized residual of the magnetic sensor measurements.

Further, the above system further includes:

a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2,3}$

wherein L is_miIs the ith measurement equation of the second measurement equation, A_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element.

The invention has the advantages that: the method comprises the following steps that the process of calculating attitude and heading reference system information is realized by adopting a two-step filtering method, and in the first step, an accelerometer is used as a measuring sensor to estimate the error of a quaternion and update the quaternion; and secondly, estimating the error of the quaternion by taking the magnetic sensor as a measuring sensor, detecting the magnetic interference of the environment where the carrier is positioned in real time by using a magnetic interference detection algorithm, adaptively adjusting the weight of the observation data of the magnetic sensor in filtering estimation by the ratio of a measuring residual error to an actual residual error, updating the quaternion and calculating the attitude of the carrier and the heading reference system information. The tolerance and the reliability of the carrier attitude and the heading reference system information are improved.

In the several embodiments provided in the present application, it should be understood that the disclosed method and system may be implemented in other ways. The above-described device embodiments are merely illustrative, for example, the division of the unit is only a logical functional division, and there may be other division ways in actual implementation, for example: multiple units or components may be combined, or may be integrated into another system, or some features may be omitted, or not implemented. Furthermore, the coupling, direct coupling, or communication between the components shown or discussed may be through some interfaces, and the indirect coupling or communication between the devices or units may be electrical, mechanical, or other forms.

The units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, that is, may be located in one place, or may be distributed on a plurality of network units; some or all of the units can be selected according to actual needs to achieve the purpose of the solution of the embodiment.

In addition, all the functional units in the embodiments of the present invention may be integrated into one processing module, or each unit may be separately used as one unit, or two or more units may be integrated into one unit; the integrated unit can be realized in a form of hardware, or in a form of hardware plus a software functional unit.

Those of ordinary skill in the art will understand that: all or part of the steps for implementing the method embodiments may be implemented by hardware related to program instructions, and the program may be stored in a computer readable storage medium, and when executed, the program performs the steps including the method embodiments; and the aforementioned storage medium includes: various media that can store program codes, such as a removable memory device, a Read-only memory (ROM), a Random Access Memory (RAM), a magnetic disk, and an optical disk.

The method is suitable for calculating the carrier attitude and the heading reference system information. The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the appended claims.

The above description is only for the specific embodiments of the present invention, but the scope of the present invention is not limited thereto, and any person skilled in the art can easily conceive of the changes or substitutions within the technical scope of the present invention, and all the changes or substitutions should be covered within the scope of the present invention. Therefore, the protection scope of the present invention shall be subject to the protection scope of the claims.

Claims (30)

1. A geomagnetic field model-assisted carrier attitude and heading calculation method is characterized by comprising the following steps of:

calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model; calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;

judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;

when environmental magnetic interference exists, constructing a self-adaptive weight of a measured value of a magnetic sensor according to a normalized residual error of the measured value of the magnetic sensor and a covariance matrix of the measured value, and constructing an equivalent weight matrix of the measured value of the magnetic sensor according to the self-adaptive weight of the measured value of the magnetic sensor;

and calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.

2. The method of claim 1, wherein calculating the carrier attitude information and heading information based on the accelerometer measurements, the magnetic sensor measurements, and an equivalent weight matrix of the magnetic sensor measurements comprises:

updating the state prediction value according to the accelerometer measurement value to obtain a state update value, and updating the estimation value of the attitude quaternion by using the state update value;

updating the state updating value according to the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value, and updating the estimation value of the attitude quaternion by using the state estimation value;

and calculating the attitude information and the course information of the carrier according to the estimated value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.

3. The method of claim 2, wherein updating the state prediction value according to the accelerometer measurement value to obtain a state update value comprises:

updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector;

and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.

4. The method of claim 2, wherein updating the state update value according to the magnetic sensor measurement value and an equivalent weight matrix of the magnetic sensor measurement value to obtain the state estimation value comprises:

updating the model according to the inertial sensor error model and the attitude error quaternion, and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model;

and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.

5. The method according to claim 3 or 4, characterized in that the Kalman filtering state equation is embodied as:

$\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

$W\left(t\right)=\left[\begin{array}{c}-0.5v_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1T is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, v_ωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, q_eVector portion being error quaternion, b_ωIs a random constant of the gyroscope, b_aIs the accelerometer bias.

6. The method of claim 3, wherein the first measurement equation is specifically:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a$

wherein L is_aFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gⁿIs the projection of the gravitational acceleration in a navigation coordinate system, v_aIs the random error of the accelerometer.

7. The method of claim 6, wherein updating the state prediction value according to the Kalman filtered state equation, the first measurement equation, and the accelerometer measurement value to obtain a state update value comprises:

updating the state predicted value by adopting the following formula to obtain a state updated value:

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)$

$K_{a,k}={\∑}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\∑}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\∑}_{{\hat{X}}_{a,k}}=(I-K_{a,k}{A}_{a,k}){\∑}_{{\stackrel{\‾}{X}}_k}{(I-K_{a,k}{A}_{a,k})}^T+K_{a,k}{R}_a{K}_{a,k}^T$

wherein,the value is updated for the state at time k,is a predicted value of the state at time k, L_a，kThe accelerometer measurement at time k, A_a，kIs the measurement matrix of the first measurement equation at the time K, i is the unit matrix, K_a，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, R_aMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).

8. The method of claim 2, wherein the state update value is used to update the estimate of the attitude quaternion by:

updating the estimated value of the attitude quaternion by adopting the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eThe components of the update values for the states,updating the state with the value.

9. The method of claim 4, wherein the second measurement equation is specifically:

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m$

wherein L is_mAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mⁿAs a result of the normalization of the ideal geomagnetic field output value, v_mRandom error of the magnetic sensor.

10. The method of claim 9, wherein the state update value is updated according to the Kalman filtered state equation, the second measurement equation, the magnetic sensor measurement value, and an equivalent weight matrix of the magnetic sensor measurement value to obtain the state estimation value, and specifically:

updating the state update value by adopting the following formula to obtain a state estimation value:

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\∑}_{{\hat{X}}_k}=(I-K_{m,k}{A}_{m,k}){\∑}_{{\hat{X}}_{a,k}}{(I-K_{m,k}{A}_{m,k})}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& O_{3\×6}\\ O_{6\×3}& O_{6\×6}\end{array}\right]{\∑}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\∑}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}$

${\∑}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\∑}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& O_{3\×6}\\ O_{6\×3}& O_{6\×6}\end{array}\right]$

$r_3=C_n^b\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$

wherein,is an estimate of the state at time k,updating the value of the state at time k, L_m，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, K_m，kIs the gain matrix at time k, A_m，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.

11. The method of claim 2, wherein the state estimate is used to update the estimate of the attitude quaternion by:

updating the estimated value of the attitude quaternion by adopting the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eIs a component of the state estimate that is,is the state estimate.

12. The method according to claim 1, wherein the estimated declination angle is calculated from the output of the accelerometer and the output of the magnetic sensor by:

the estimated declination angle is calculated using the following formula:

D_cacul＝arccos(n_a·n_m)

wherein D is_caculFor said estimated declination angle, n_aVector of direction of output value of said accelerometer in carrier systemAmount, n_mThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.

13. The method according to claim 1, wherein determining whether there is an environmental magnetic interference based on the ideal and estimated declination angles comprises:

judging whether the ideal magnetic dip angle and the estimated magnetic dip angle meet the following conditions:

|D_cacul-D_riference|＞λ_D

wherein D is_riferenceIs the ideal magnetic inclination angle, D_caculFor said estimation of the magnetic tilt angle, λ_DIs a preset threshold value;

if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

14. The method of claim 1, wherein the adaptive weights for the magnetic sensor measurements are constructed from normalized residuals of the magnetic sensor measurements and a measurement covariance matrix, in particular:

constructing an adaptive weight of the magnetic sensor measurement value by adopting the following formula:

in the absence of ambient magnetic interference;

in the presence of ambient magnetic interference;

wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time k_kiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, V_i' is a magnetic sensor measurement valueNormalized residual of (2).

15. The method of claim 9, wherein prior to constructing the adaptive weights for the magnetic sensor measurements from the normalized residuals of the magnetic sensor measurements and the measurement covariance matrix, further comprising:

calculating a normalized residual of the magnetic sensor measurement values using the following formula:

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2},3$

wherein L is_miIs the ith measurement equation of the second measurement equation, A_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element.

16. An attitude and heading reference system, comprising:

the first calculation module is used for calculating an ideal magnetic inclination angle according to the position information of the carrier and the geomagnetic field model;

the second calculation module is used for calculating and estimating a magnetic inclination angle according to the output value of the accelerometer and the output value of the magnetic sensor;

the judging module is used for judging whether environmental magnetic interference exists according to the ideal magnetic inclination angle and the estimated magnetic inclination angle;

the first construction module is used for constructing the self-adaptive weight of the magnetic sensor measuring value according to the normalized residual error of the magnetic sensor measuring value and the measuring value covariance matrix when the judgment module judges that the environmental magnetic interference exists;

the second construction module is used for constructing an equivalent weight matrix of the magnetic sensor measurement values according to the self-adaptive weights of the magnetic sensor measurement values;

and the third calculation module is used for calculating the attitude information and the course information of the carrier according to the accelerometer measurement value, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value.

17. The system of claim 16, wherein the third computing module comprises:

the first updating submodule is used for updating the state predicted value according to the accelerometer measurement value to obtain a state updating value;

a second update submodule for updating the estimated value of the attitude quaternion using the state update value;

the third updating submodule is used for updating the state updating value according to the magnetic sensor measuring value and the equivalent weight matrix of the magnetic sensor measuring value to obtain a state estimation value;

a fourth updating submodule for updating the estimated value of the attitude quaternion using the state estimated value;

and the calculation submodule is used for calculating the attitude information and the course information of the carrier according to the estimation value of the attitude quaternion and a conversion matrix from a navigation coordinate system to a carrier coordinate system.

18. The system of claim 17,

the first updating submodule is specifically used for updating a model according to an inertial sensor error model and an attitude error quaternion and constructing a Kalman filtering state equation; constructing a first measurement equation according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector; and updating the state prediction value according to the Kalman filtering state equation, the first measurement equation and the accelerometer measurement value to obtain a state update value.

19. The system of claim 17,

the third updating submodule is specifically used for updating the model according to the inertial sensor error model and the attitude error quaternion and constructing a Kalman filtering state equation; constructing a second measurement equation according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model; and updating a state updating value according to the Kalman filtering state equation, the second measurement equation, the magnetic sensor measurement value and the equivalent weight matrix of the magnetic sensor measurement value to obtain a state estimation value.

20. The system according to claim 18 or 19, wherein the Kalman filtering state equation is specifically:

$\stackrel{.}{X}\left(t\right)=F\left(t\right)X\left(t\right)+W\left(t\right)$

$F\left(t\right)=\left[\begin{array}{ccc}-[{\widehat{\mathrm{\ω}}}^b\×]& -0.5I& 0\\ 0& 0& 0\\ 0& 0& 0\end{array}\right]$

$W\left(t\right)=\left[\begin{array}{c}-0.5v_{\mathrm{\ω}}\\ v_{b_{\mathrm{\ω}}}\\ v_{b_a}\end{array}\right]$

wherein x (t) ═ q_eb_ωb_a]^T∈R^9×1T is the state quantity, T is the transpose, F (T) is the state transition matrix, W (T) is the process noise,for gyroscope output valuesOf an inverse matrix, v_ωIs the random drift noise of the gyroscope,is the noise of the random constant model of the gyroscope,for accelerometer stochastic constant model noise, q_eVector portion being error quaternion, b_ωIs a random constant of the gyroscope, b_aIs the accelerometer bias.

21. The system of claim 18, wherein the first measurement equation is specifically:

$L_a={\hat{a}}^b-C_n^b\left(\hat{q}\right)g^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+b_a+v_a$

wherein L is_aFor the difference between the output value of the accelerometer and the output value of the accelerometer estimated from the gravity field reference vector,is the output value of the accelerometer and is,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, gⁿIs the projection of the gravitational acceleration in a navigation coordinate system, v_aIs the random error of the accelerometer.

22. The system of claim 21,

the first updating submodule is specifically used for updating a model according to an inertial sensor error model and an attitude error quaternion and constructing a Kalman filtering state equation; according to the output value of the accelerometer and the output value of the accelerometer estimated according to the gravity field reference vector, a first measurement equation is constructed, and a state prediction value is updated by adopting the following formula to obtain a state update value:

${\hat{X}}_{a,k}={\stackrel{\‾}{X}}_k+K_{a,k}(L_{a,k}-A_{a,k}{\stackrel{\‾}{X}}_k)$

$K_{a,k}={\∑}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}^T{(A_{a,k}^T{\∑}_{{\stackrel{\‾}{X}}_k}{A}_{a,k}+R_a)}^{-1}$

${\∑}_{{\hat{X}}_{a,k}}=(I-K_{a,k}{A}_{a,k}){\∑}_{{\stackrel{\‾}{X}}_k}{(I-K_{a,k}{A}_{a,k})}^T+K_{a,k}{R}_a{K}_{a,k}^T$

wherein,the value is updated for the state at time k,is a predicted value of the state at time k, L_a，kThe accelerometer measurement at time k, A_a，kIs the measurement matrix of the first measurement equation at the time K, i is the unit matrix, K_a，kIn order to be a matrix of gains, the gain matrix,is composed ofOf covariance matrix, R_aMeasuring a covariance matrix of the noise for the accelerometer;is time kThe covariance matrix of (2).

23. The system of claim 17,

the second updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_{a,k}(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eThe components of the update values for the states,updating the state with the value.

24. The system of claim 19, wherein the second measurement equation is specifically:

$L_m={\hat{m}}^b-C_n^b\left(\hat{q}\right)m^n=2[C_n^b\left(\hat{q}\right)g^n\×]q_e+v_m$

wherein L is_mAs a difference between the output value of the magnetic sensor and the output value of the magnetic sensor estimated from the world geomagnetic field model,is an output value of the magnetic sensor,for the transformation matrix from the navigation coordinate system to the carrier coordinate system,as an estimate of the attitude quaternion, mⁿAs a result of the normalization of the ideal geomagnetic field output value, v_mRandom error of the magnetic sensor.

25. The system of claim 24,

the third updating submodule is specifically used for updating the model according to the inertial sensor error model and the attitude error quaternion and constructing a Kalman filtering state equation; according to the output value of the magnetic sensor and the output value of the magnetic sensor estimated according to the world geomagnetic field model, a second measurement equation is constructed, and a state updating value is updated by adopting the following formula to obtain a state estimation value:

${\hat{X}}_k={\hat{X}}_{a,k}+K_{m,k}(L_{m,k}-A_{m,k}{\hat{X}}_{a,k})$

${\∑}_{{\hat{X}}_k}=(I-K_{m,k}{A}_{m,k}){\∑}_{{\hat{X}}_{a,k}}{(I-K_{m,k}{A}_{m,k})}^T+K_{m,k}{{\stackrel{\‾}{P}}_{m,k}}^{-1}{K}_{m,k}^T$

$K_{m,k}=\left[\begin{array}{cc}{r}_3{r}_3^T& O_{3\×6}\\ O_{6\×3}& O_{6\×6}\end{array}\right]{\∑}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}^T{(A_{m,k}^T{\∑}_{{\stackrel{\‾}{X}}_{m,k}}{A}_{m,k}+{{\stackrel{\‾}{P}}_{m,k}}^{-1})}^{-1}$

${\∑}_{{\stackrel{\‾}{X}}_{m,k}}=\left[\begin{array}{cc}{\∑}_{{\hat{X}}_{a,k}}(1:\mathrm{3,1}:3)& O_{3\×6}\\ O_{6\×3}& O_{6\×6}\end{array}\right]$

$r_3=C_n^b\left(\hat{q}\right){\left[\begin{array}{ccc}0& 0& 1\end{array}\right]}^T$

wherein,is an estimate of the state at time k,updating the value of the state at time k, L_m，kThe magnetic sensor measurement value at time k,is composed ofCovariance matrix of, K_m，kIs the gain matrix at time k, A_m，kIs a measurement matrix for the time k,an equivalent weight matrix of the magnetic sensor measurement values at time k.

26. The system of claim 17,

the fourth updating submodule is specifically configured to update the estimation value of the attitude quaternion by using the following formula:

$\hat{q}\⇐\hat{q}\⊗q_e$

$\hat{q}\⇐\hat{q}/\left|\right|\hat{q}\left|\right|$

$q_e={\hat{X}}_a(1:3)$

wherein,is an estimate of the attitude quaternion,in order to be a quaternion multiplication,is composed ofThe normalization of (a) to (b) is performed,to assign a symbol, q_eIs a component of the state estimate that is,is the state estimate.

27. The system of claim 16,

the second calculating module is specifically configured to calculate the estimated declination angle by using the following formula:

D_cacul＝arccos(n_a·n_m)

wherein D is_caculFor said estimated declination angle, n_aFor the direction vector of the output value of the accelerometer in the carrier system, n_mThe vector is a direction vector of an output value of the magnetic sensor in the carrier system.

28. The system of claim 16,

the judging module is specifically configured to judge whether the ideal magnetic dip angle and the estimated magnetic dip angle satisfy the following conditions:

|D_cacul-D_riference|＞λ_D

wherein D is_riferenceIs the ideal magnetic inclination angle, D_caculFor said estimation of the magnetic tilt angle, λ_DIs a preset threshold value;

if the above conditions are met, determining that environmental magnetic interference exists; otherwise, it is determined that there is no ambient magnetic interference.

29. The system of claim 16,

the first constructing module is specifically configured to, when the determining module determines that the environmental magnetic interference exists, construct the adaptive weight of the measured value of the magnetic sensor by using the following formula:

in the absence of ambient magnetic interference;

in the presence of ambient magnetic interference;

wherein,adaptive weighting, p, of the i-th measured value of the magnetic sensor for time k_kiIs the ith main diagonal element of the covariance matrix of the measured values, c is a constant, V_i' is the normalized residual of the magnetic sensor measurements.

30. The system of claim 24, further comprising:

a fourth calculating module, configured to calculate a normalized residual error of the magnetic sensor measurement value by using the following formula:

${V_{i,k}}^{\′}=(L_{\mathrm{mi},k}-A_{\mathrm{mi},k}{\stackrel{\‾}{X}}_k)/\left|{\mathrm{\σ}}_i\right|,i=\mathrm{1,2},3$

wherein L is_miIs the ith measurement equation of the second measurement equation, A_miIs a measurement matrixRow i of (1); i sigma_iI is the covariance matrix R of the measured noise of the magnetic sensor_mThe square root of the ith main diagonal element.