CN104698485B - Integrated navigation system and air navigation aid based on BD, GPS and MEMS - Google Patents
Integrated navigation system and air navigation aid based on BD, GPS and MEMS Download PDFInfo
- Publication number
- CN104698485B CN104698485B CN201510010594.9A CN201510010594A CN104698485B CN 104698485 B CN104698485 B CN 104698485B CN 201510010594 A CN201510010594 A CN 201510010594A CN 104698485 B CN104698485 B CN 104698485B
- Authority
- CN
- China
- Prior art keywords
- mrow
- mfrac
- mmultiscripts
- signal
- integrated navigation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Computer Networks & Wireless Communication (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
For the deficiency of existing navigation system, the present invention provides a kind of integrated navigation system and air navigation aid based on BD, GPS and MEMS, wherein, integrated navigation system of the present invention, including MEMS IMU modules, GNSS module, magnetometer module and integrated navigation computer；MEMS IMU modules, GNSS module and magnetometer module are connected with integrated navigation computer respectively；Air navigation aid of the present invention can be after output calibration position numerical value, speed values and correction after attitude angle numerical value.Beneficial technique effect：Integrated navigation system of the present invention has overload-resistant, miniaturization, low-power consumption, low cost, and with high impact-resistant, is especially suitable for wars environmental applications.Air navigation aid of the present invention, can suppress the drift of gyroscope, accelerometer and magnetometer, improve the stability of a system and precision.
Description
Technical field
The invention belongs to field of navigation technology, it is adaptable to which the control of microminiature platform stable, posture refer to course, combine and lead
The system regions such as boat, are specially integrated navigation system and air navigation aid based on BD, GPS and MEMS.
Background technology
As the continuous propulsion of New military transform thought, and domestic weaponry persistently upgrade, national defence pair
The precision strike of armament systems proposes a series of new requirements：Overload-resistant, miniaturization, low-power consumption, low cost, high accuracy.But
It is that existing integrated navigation system generally existing that volume is big, be difficult to the shortcomings of integrated, precision is poor, has both been difficult to meet microminiature
The growth requirement of system, do not meet yet military industry equipment development in the urgent need to.And it is developed in recent years, using integrated circuit
Processing technology inertial microsensors (MEMS-IMU) performance improve constantly, and with small volume, it is lightweight, low in energy consumption,
The characteristics of cost is low.MEMS-IMU is applied in armament systems, especially in the navigation system of weapon, so that it is guaranteed that weapon
The precision strike capability of equipment, is domestic weapon development from now on while reducing the volume of the integrated navigation system in armament systems
Important directions.But, current MEMS-IMU precision is still in reduced levels, it is impossible to navigation is implemented separately, it is necessary to further
Design of Hardware Architecture and navigation algorithm optimization, could meet technology development the need for.
The content of the invention
The purpose of the present invention is to propose to a kind of integrated navigation system based on BD, GPS and MEMS and air navigation aid, it is allowed to have
The characteristics of having overload-resistant, miniaturization, low-power consumption, low cost, degree of precision, the especially gesture stability with degree of precision and navigation
Positioning performance.It is specific as follows：
Integrated navigation system based on BD, GPS and MEMS, including MEMS-IMU modules 100, GNSS module 200, magnetometer
Module 300 and integrated navigation computer 400.MEMS-IMU modules 100, GNSS module 200 and magnetometer module 300 respectively with group
Navigational computer 400 is closed to connect.The described integrated navigation system based on BD, GPS and MEMS is arranged on carrier and used.
Described MEMS-IMU modules 100 include three axis MEMS gyro 101 and 3 axis MEMS accelerometer 102, wherein,
Three axis MEMS gyro 101 is the digitized angular velocity detector of single-chip, is responsible for the output angle of integrated navigation computer 400 speed
Spend signal.3 axis MEMS accelerometer 102 is that single-chip is digitized than force detector, is responsible for integrated navigation computer 400
Force signal is compared in output.
Described GNSS module 200 includes a piece of integrated BD2 and GPS navigation chip 205, passes through integrated BD2's and GPS
Navigation chip 205 exports navigation signal to integrated navigation computer 400.
Described magnetometer module 300 is responsible for exporting three-dimensional magnetic field strength signal to integrated navigation computer 400.
The integrated navigation computer 400 is by the output signal of three axis MEMS gyro 101,3 axis MEMS accelerometer
102 output signal is filtered computing with the output signal of GNSS module 200, obtains the three-dimensional position of this integrated navigation system
Value, three-dimensional velocity value and accelerometer drift estimate value.
Integrated navigation computer 400 is by the output signal of three axis MEMS gyro 101,3 axis MEMS accelerometer 102
The accelerometer drift estimate value that output signal, the output signal of magnetometer module 300 and computing are obtained carries out fusion fortune
Calculate, obtain the pose estimation value of this integrated navigation system.
Navigation data of above-mentioned three-dimensional position value, three-dimensional velocity value and the pose estimation value as this integrated navigation system
Output valve is externally exported.
The method that navigation coordinate is obtained using the integrated navigation system of the present invention based on BD, GPS and MEMS, by such as
Lower step is carried out：
Step 1：Angular velocity signal, three axles that three axis MEMS gyro 101 is exported are read by integrated navigation computer 400
The navigation coordinate signal that the ratio force signal and GNSS module 200 that mems accelerometer 102 is exported are exported, and expanded by integrated navigation
Exhibition Kalman filter 402 is converted to the position numerical value, speed values and accelerometer drift value of this integrated navigation system.
Step 2：Angular velocity signal, three axles that three axis MEMS gyro 101 is exported are read by integrated navigation computer 400
The three-dimensional magnetic field strength signal that the ratio force signal of the output of mems accelerometer 102, magnetometer module 300 are exported, and pass through posture
Resolve the attitude angle numerical value that wave filter 401 is converted to the absolute value of this integrated navigation system.
Step 3：Step 1 is obtained into accelerometer drift value by integrated navigation computer 400 and changes into gyroscopic drift number
Value and magnetometer distortion value.
Step 4：Step 3 is obtained into gyroscopic drift numerical value and magnetometer distortion value input attitude algorithm wave filter 401,
The attitude angle numerical value of absolute value to being obtained by step 2 is corrected, and obtains the attitude angle after the correction of this integrated navigation system
Numerical value.
Step 5：It regard the attitude angle numerical value after the position numerical value, speed values and correction of this integrated navigation system as navigation
The result of computing is externally exported.
The present invention has advantages below compared with prior art：
1st, MEMS inertia devices are employed, greatly reduction navigation system cost, at the same time, MEMS inertia devices have height
The characteristics such as shock resistance, are especially suitable for wars environmental applications.MEMS-IMU is combined by the present invention with GPS, magnetometer, structure
Integrated navigation system is built, degree of precision gesture stability and navigator fix is realized, and overcomes that the volume of legacy equipment is big, weight weight
Technical barrier.
2nd, present invention employs a kind of brand-new MEMS-IMU modules.MEMS-IMU modules digitize three axles using single-chip
MEMS gyroscope and single-chip digitlization 3 axis MEMS accelerometer, compared with conventional combination navigation system, greatly reduction
System bulk, while reduce due to systematic error caused by quadrature error.Due to MEMS gyroscope and mems accelerometer
Equal output digit signals, are that application and development offers convenience.
3rd, the GNSS module that the present invention is used is a piece of monolithic BD2/GPS navigation chips, has both Beidou II BD2 and GPS
Homing capability.Adapt to the Big Dipper, GPS demands, wide application.In addition, GNSS module uses single chip solution, greatly reduce
GNSS module volume and power consumption, so as to reduce total system volume and power consumption.
4th, present invention employs a kind of new attitude algorithm method, roll angle roll can be obtained by accelerometer, bowed
Elevation angle pitch, roll angle, the angle of pitch are merged with the three-dimensional magnetic field intensity that magnetometer is exported, and just obtain course angle yaw.
Importantly, being used as benchmark using gravity shafting and magnetic field shafting, it is ensured that system surveys appearance error bounded, so as to ensure that system is stable.
Data fusion is carried out by gyroscope and accelerometer, magnetometer, using new attitude algorithm method to attitude quaternion and top
The drift of spiral shell instrument, the drift of accelerometer and magnetometer distortion are filtered estimation, thus to gyroscope, accelerometer and
Magnetometer carries out error compensation, it is suppressed that the drift of gyroscope, accelerometer and magnetometer, improves the stability of a system and essence
Degree.
Brief description of the drawings
Fig. 1 is integrated navigation system of the invention.
Fig. 2 is GNSS module topological structure schematic diagram of the invention.
Fig. 3 is magnetometer module topology structural representation of the invention.
Fig. 4 is the data processing schematic diagram of integrated navigation computer of the present invention.
Fig. 5 is the schematic diagram of the attitude algorithm method of the present invention.
Fig. 6 is the flow chart of existing expanded Kalman filtration algorithm.
Embodiment
Describe the ins and outs of the present invention in detail in conjunction with accompanying drawing.
Referring to Fig. 1, the integrated navigation system based on BD, GPS and MEMS, on carrier, including MEMS-IMU modules
100th, GNSS module 200, magnetometer module 300 and integrated navigation computer 400.MEMS-IMU modules 100, GNSS module 200
It is connected respectively with integrated navigation computer 400 with magnetometer module 300.
Described MEMS-IMU modules 100 include three axis MEMS gyro 101 and 3 axis MEMS accelerometer 102, wherein,
Three axis MEMS gyro 101 is the digitized angular velocity detector of single-chip, is responsible for the output angle of integrated navigation computer 400 speed
Spend signal.3 axis MEMS accelerometer 102 is that single-chip is digitized than force detector, is responsible for integrated navigation computer 400
Force signal is compared in output.
Described GNSS module 200 includes a piece of integrated BD2 and GPS navigation chip 205, passes through integrated BD2's and GPS
Navigation chip 205 exports navigation signal to integrated navigation computer 400.
Described magnetometer module 300 is responsible for exporting three-dimensional magnetic field strength signal to integrated navigation computer 400.
The integrated navigation computer 400 is by the output signal of three axis MEMS gyro 101,3 axis MEMS accelerometer
102 output signal is filtered computing with the output signal of GNSS module 200, obtains the three-dimensional position of this integrated navigation system
Value, three-dimensional velocity value and accelerometer drift estimate value.
Integrated navigation computer 400 is by the output signal of three axis MEMS gyro 101,3 axis MEMS accelerometer 102
The accelerometer drift estimate value that output signal, the output signal of magnetometer module 300 and computing are obtained carries out fusion fortune
Calculate, obtain the pose estimation value of this integrated navigation system.
Navigation data of above-mentioned three-dimensional position value, three-dimensional velocity value and the pose estimation value as this integrated navigation system
Output valve is externally exported.
Referring to Fig. 2, described GNSS module 200 also includes antenna 201.Described integrated BD2 and GPS navigation chip 205
It is made up of BD2 signal front-end processings unit 202, gps signal front-end processing unit 203 and Base-Band Processing SOC204.Wherein, antenna
201 signal input part, the signal of gps signal front-end processing unit 203 respectively with BD2 signal front-end processings unit 202 is inputted
End is connected, the signal output part of BD2 signal front-end processings unit 202 and the signal output of gps signal front-end processing unit 203
Signal input part of the end respectively with Base-Band Processing SOC (also referred to as Base-Band Processing on-chip system SOC) 204 is connected.
Referring to Fig. 3, described magnetometer module 300 includes x-axis magnetoresistive transducer 301, y-axis magnetoresistive transducer 302, z-axis
Magnetoresistive transducer 303 and magnetoresistance signal process circuit 304.Wherein, the signal output part of x-axis magnetoresistive transducer 301, y-axis magnetic resistance
The signal output part of sensor 302, the signal output part of z-axis magnetoresistive transducer 303 respectively with magnetoresistance signal process circuit 304
Signal input part is connected, and is carried out by 304 pairs of signals received of magnetoresistance signal process circuit after analog-to-digital conversion, output digitlization
Three-dimensional magnetic field strength signal.
Referring to Fig. 4, the integrated navigation computer 400 includes attitude algorithm wave filter 401 and integrated navigation extension karr
Graceful wave filter (also referred to as integrated navigation EKF) 402.Wherein, attitude algorithm wave filter 401 obtains three axis MEMS gyro 101
The signal of signal, the signal of 3 axis MEMS accelerometer 102 and magnetometer module 300.Integrated navigation EKF
Device integrated navigation EKF402 obtain the signal of three axis MEMS gyro 101, the signal of 3 axis MEMS accelerometer 102 and
The signal of GNSS module 200.The output signal of integrated navigation extended Kalman filter 402 feeds back to attitude algorithm wave filter
401。
The integrated navigation extended Kalman filter integrated navigation EKF402 is to the output of the GNSS module 200 received
The output signal of signal, the output signal of three axis MEMS gyro 101 and 3 axis MEMS accelerometer 102 is filtered computing,
Obtain the three-dimensional position value, three-dimensional velocity value and accelerometer drift estimate value of this integrated navigation system.
The output signal for the three axis MEMS gyro 101 that 401 pairs of the attitude algorithm wave filter is received, 3 axis MEMS add
The output signal of speedometer 102, the output signal of magnetometer module 300 and obtained by integrated navigation EKF402 by filtering operation
The accelerometer drift estimate value obtained carries out fusion operation, obtains the pose estimation value of this integrated navigation system.
The pose estimation value of this integrated navigation system, three-dimensional position value and three-dimensional velocity value are outside as the result of navigation
Output.
The method that navigation coordinate is obtained using the integrated navigation system of the present invention based on BD, GPS and MEMS, by such as
Lower step is carried out：
Step 1：Angular velocity signal, three axles that three axis MEMS gyro 101 is exported are read by integrated navigation computer 400
The navigation coordinate signal that the ratio force signal and GNSS module 200 that mems accelerometer 102 is exported are exported, and expanded by integrated navigation
Exhibition Kalman filter 402 is converted to the position numerical value, speed values and accelerometer drift value of this integrated navigation system.
Step 2：Angular velocity signal, three axles that three axis MEMS gyro 101 is exported are read by integrated navigation computer 400
The three-dimensional magnetic field strength signal that the ratio force signal of the output of mems accelerometer 102, magnetometer module 300 are exported, and pass through posture
Resolve the attitude angle numerical value that wave filter 401 is converted to the absolute value of this integrated navigation system.
Step 3：Step 1 is obtained into accelerometer drift value by integrated navigation computer 400 and changes into gyroscopic drift number
Value and magnetometer distortion value.
Step 4：Step 3 is obtained into gyroscopic drift numerical value and magnetometer distortion value input attitude algorithm wave filter 401,
The attitude angle numerical value of absolute value to being obtained by step 2 is corrected, and obtains the attitude angle after the correction of this integrated navigation system
Numerical value.
Step 5：It regard the attitude angle numerical value after the position numerical value, speed values and correction of this integrated navigation system as navigation
The result of computing is externally exported.
Furtherly, the pose estimation value of this integrated navigation system is obtained as follows：
Step a：The pose estimation value of last moment is obtained by integrated navigation computer 400Magnetic field intensity signalbm、
Compare force signalbA and earth magnetic dip anglenb.Wherein, the pose estimation value of last momentBy quaternary number [q1 q2 q3 q4]
Represent, i.e.,Magnetic field intensity signalbM=[0 mx my mz], it is 400 pairs of integrated navigation computer
The output data of magnetometer module 300 is obtained after being normalized.Compare force signalbA=[0 ax ay az], led for combination
Boat computer 400 is obtained after the output data of 3 axis MEMS accelerometer 102 is normalized.Earth magnetic dip anglenb
=[0 bx 0 bz], it is integrated navigation computer 400 by formula
What is calculated arrives.H in preceding formulax、hy、hzPass through formula respectivelyComputing is obtained.
Step b：Kinetic model is set up using the step a parameters obtained by integrated navigation computer 400.The power
Learn model and include object functionAnd Jacobian matrixSpecific formula is as follows：
Step c：The object function for being obtained step b by integrated navigation computer 400With
Jacobian matrixEquation below is substituted into, calculates and obtains attitude angular rate normalization evaluated error
Step d：The attitude angular rate normalization evaluated error for being obtained step c by integrated navigation computer 400 is substituted into such as
Lower formula, calculates the drift estimate value for obtaining gyroscopebωBias。
bωBias=α ΣbωErrorΔt。
In formula, factor alpha is the gyro output error of calibration zero-bit.
Step e：By integrated navigation computer 400 by the drift estimate value of the step d gyroscopes obtainedbωBiasSubstitute into as follows
Formula, calculates the gyro output angle speed after being correctedbωC。
bωC=bω-bωBias。
In formula,bω is the angular velocity data of initial gyroscope normalized output.
Step f：Gyro output angle speed after the correction for being obtained step d by integrated navigation computer 400bωCSubstitute into such as
Lower formula, solves real-time attitude quaternary number
In formula, filter coefficient β is the gyro output error of calibration nonzero digit.
The real-time attitude quaternary numberThe as pose estimation value of this integrated navigation system.
Furtherly, the ratio force signal and magnetometer of 3 axis MEMS accelerometer 102 are read by integrated navigation computer 400
The magnetic field intensity signal of module 300, and the absolute three-dimension altitude angle angle value [phi of this integrated navigation system is obtained as the following formula
theta psi]：
Wherein：For 102 three-dimensional output valve of 3 axis MEMS accelerometer,For magnetic strength
Count the three-dimensional output valve of module 300.
By integrated navigation computer 400 by absolute three-dimension altitude angle angle value [the phi theta of this integrated navigation system
Psi] the quaternary numerical value for the original state for obtaining this integrated navigation system is converted to as the following formula：
If integrated navigation computer 400 does not read the pose estimation value of last momentThen directly use this combination
The quaternary numerical value of the original state of navigation system.
Because attitude algorithm method and integrated navigation extended Kalman filter EKF are the main points that the present invention is realized, therefore
Hereafter emphasis is illustrated：
Fig. 5 is combined first, and labor is carried out to the attitude algorithm method of attitude algorithm wave filter.
Gyro to measure angular speed, if it is known that initial attitude angle, then by the integration to the time, just can be obtained in real time
Sensor attitude angle.However, MEMS gyro precision is relatively low, it is integrated, attitude angle accumulated error will be caused.Therefore, low essence
Degree MEMS gyro can not provide absolute direction measurement.Accelerometer and magnetometer will measure acceleration of gravity and magnetic respectively
, so that absolute pose angle benchmark can be provided.However, they are easily subject to the interference of higher-order noise, for example, carrier
The variable motion of itself, will influence the measurement of gravity axial direction, and distortion is occurred for the output for causing magnetometer by external magnetic field interference.
This attitude algorithm method is based on Quaternion Method, and attitude error caused by gyro to measure error is converted into quaternary
Several derivatives.By the way that gyro, accelerometer, magnetometer survey value are carried out into fusion calculation, gyroscopic drift and magnetic can be accurately estimated
Strong meter distortion, and obtain three-dimension altitude angle estimate.Meanwhile, this algorithm avoids depositing for the singular value of Eulerian angles method
Due to effectively estimating gyroscopic drift and magnetometer distortion so that system has more complex environment adaptability.It is same with this
When, the algorithm causes system to cannot be only used in low sampling rate 10Hz, it can also be used to high sampling rate 300Hz.In addition, New Algorithm
Application so that system overall calculation amount is small, and the key parameter of the wave filter can adjust, it is easy to which engineering is realized.
First, the angular speed of 3 axis MEMS gyro to measure carrier coordinate system x, y, z axial direction, respectively ωx、ωy、ωz, can
Use vectorbω represents MEMS gyro angular speed state, as shown in Equation 1.In formula 2It is the derivative of quaternary number, description
The speed that attitude angle changes.Represent the quaternary number of last moment estimation.Wherein b represents vehicle coordinate system, and n represents navigation
Coordinate system.
bω=[0 ωx ωy ωz] (formula 1)
(formula 2)
So, the quaternary number updating method based on gyro angular velocity data, real-time attitude quaternary numberIt can be obtained by formula 3
.
(formula 3)
Quaternary number meets following relationship with three-dimension altitude angle.That is, quaternary number is solved, 3 d pose just can be obtained
Angle.
Wherein quaternary number isThree-dimension altitude angle is [phi theta psi]：
Roll angle phi：
Angle of pitch theta：Theta=arcsin2 (q2·q4-q1·q3)；
Course angle psi：
Gyro zero is inclined over time, temperature and motion will produce drift, is directed to this, it is necessary to by integrated navigation computer
400 pairs of temperature carry out overcompensation.For zero inclined drift, attitude algorithm wave filter has carried out integrated correction compensation by algorithm.
As shown in Equation 4,Be attitude angular rate normalization evaluated error, can by the angular speed on three axial directions of gyro come
Represent.The drift of gyrobωBiasEstimation formulas as shown in 5 formulas,bωBiasIt isbωErrorIn constant value part, one can be passed through
Factor alpha is adjustedbωErrorThe drift of gyro is fallen in integrated value, compensation.
(formula 4)
bωBias=α ΣbωErrorΔ t (formula 5)
Gyro output angle velocity expression after so correcting is as shown in 6 formulas.After correctionbωCInstead of initialbω
Carry out computing.
bωC=bω-bωBias(formula 6)
As shown in figure 5, filter coefficient α is used for the gyro output error for calibrating zero-bit.Filter coefficient β is used to calibrate non-
The gyro output error of zero-bit, the quaternion derivative after correction is as shown in Equation 7
(formula 7)
Magnetometer eliminating hard iron and soft iron interference, demarcate constant multiplier and zero it is inclined after, when running into magnetic interference,
Magnetometer will be caused to export distortion.Magnetometer normalized output data are：
bM=[0 mx my mz], then by quaternary number computing, a certain moment magnetic field of the earth can be obtained in navigation coordinate
Direction in system：
(formula 8)
If now magnetic field has interference, the magnetic dip angle of mistake can be formed, and each regional dipping magnetic inclination angle number is normal
Number magnetic field reference direction.By calculating, can be in the hope of the now earth magnetic dip angle tried to achieve by magnetometer：
(formula 9)
And the magnetic dip angle for calculating formula 9 is with when geomagnetic inclination is compared, and being corrected.Compensate magnetic in this way
Field distortion, it can be ensured that magnetic disturbance affects only course angle part, without influenceing the angle of pitch and roll angle.This mode can be kept away
Exempt from preset reference magnetic direction.This is that conventional filter can not be realized.
Wherein, kinetic model passes through object functionAnd its Jacobian matrix
Build.Specific formula such as 10 formulas and 11 formulas.WhereinbA=[0 ax ay az],bM=[0 mx my mz],nB=[0 bx 0 bz],
(formula 10)
(formula 11)
Calculated by formula 12
(formula 12)
Integrated navigation extended Kalman filter EKF, also referred to as integrated navigation EKF, refer to GNSS and gyro and acceleration
The data of degree meter carry out fusion operation wave filter, obtain three dimensional local information, three-dimensional velocity information and accelerometer drift estimate
Value.Integrated navigation EKF is Integrated Navigation Algorithm commonly used in the art, does simple introduction, integrated navigation EKF bags to the algorithm below
System state equation and measurement equation are included, respectively as shown in formula 13 and 14,
1st, system state equation：(formula 13)
Wherein, X is system state vector, and W is system noise vector, and F is system transfer matrix, and G changes for system noise
Matrix：
For system mode vector,For mathematical platform misalignment.Respectively carrier
East orientation, north orientation and sky orientation speed error.δ L, δ l, δ h are respectively latitude error, longitude error and height error.εbx、εby、
εbz、The respectively random constant value zero of Gyro Random Constant Drift and accelerometer is inclined.
System transfer matrix F is：
F1,10=1, F2,11=1,F3,12=1, F4,2=-
fU, F4,3=fN,F4,13=1, F5,1=
fU, F5,3=-fE, F5,14=1, F6,1=-
fN, F6,2=fE,F6,15=1, F9,6=1, wherein R are earth radius.
System noise transfer matrix is：
System noise vector is made up of the random error of gyro and accelerometer, and expression formula is：
W=[wgx wgy wgz wax way waz]T；
2nd, system measurements equation：Z=HX+V (formula 14)
Wherein, Z is measurement vector, and H is observing matrix, and V is measurement noise.
The measurement vector Z of speedvFor：
Wherein, Hv=[03×3 diag(1 1 1) 03×9]；
Similarly：Position measures vector ZpFor：In formula,
Hp=[03×6 diag(RM RNcosL 1) 03×6]；
3rd, on the basis of above mathematical modeling, information fusion is carried out using extended Kalman filter EKF methods, obtained
The quantity of state estimate of integrated navigation system
State one-step prediction equation：
State Estimation accounting equation：
Filtering gain equation：Kk=Pk/k-1HT(HPk/k-1HT+R)-1；
One-step prediction mean square error equation：Pk/k-1=Φ Pk/k-1ΦT+GQk-1G；
Estimate mean square error equation：
The flow chart of expanded Kalman filtration algorithm is as shown in Figure 6.
Integrated navigation system and air navigation aid of the present invention based on BD, GPS and MEMS, angular speed is realized by algorithm
Calibration, acceleration calibration, and magnetic calibration.Estimate attitude of carrier angle, position and velocity information, and with higher output speed
Real-time update.By merging GNSS location velocity information and magnetometer information, real-time calibration system posture, position, speed and biography
Sensor error.
The content not being described in detail in description of the invention belongs to the known technology of professional and technical personnel in the field.Although knot
Close accompanying drawing and describe embodiments of the present invention, but those of ordinary skill in the art can be within the scope of the appended claims
Make various deformations or amendments.
Claims (2)
1. obtain the method for navigation coordinate using the integrated navigation system based on BD, GPS and MEMS, it is described based on BD, GPS and
MEMS integrated navigation system, including MEMS-IMU modules (100), GNSS module (200), magnetometer module (300) and combination
Navigational computer (400)；MEMS-IMU modules (100), GNSS module (200) and magnetometer module (300) are led with combining respectively
The computer (400) that navigates is connected；
Described MEMS-IMU modules (100) include three axis MEMS gyro (101) and 3 axis MEMS accelerometer (102), its
In, three axis MEMS gyro (101) is the digitized angular velocity detector of single-chip, is responsible for integrated navigation computer (400)
Output speed signal；3 axis MEMS accelerometer (102) is that single-chip is digitized than force detector, is responsible for integrated navigation
Force signal is compared in computer (400) output；
Described GNSS module (200) includes a piece of integrated BD2 and GPS navigation chip (205), passes through integrated BD2's and GPS
Navigation chip (205) exports navigation signal to integrated navigation computer (400)；
Described magnetometer module (300) is responsible for exporting three-dimensional magnetic field strength signal to integrated navigation computer (400)；
The integrated navigation computer (400) is by the output signal of three axis MEMS gyro (101), 3 axis MEMS accelerometer
(102) output signal and the output signal of GNSS module (200) are filtered computing, obtain the three-dimensional of this integrated navigation system
Positional value, three-dimensional velocity value and accelerometer drift estimate value；
Integrated navigation computer (400) is by the output signal of three axis MEMS gyro (101), 3 axis MEMS accelerometer (102)
Output signal, the accelerometer drift estimate value that obtains of the output signal of magnetometer module (300) and computing merged
Computing, obtains the pose estimation value of this integrated navigation system；
Above-mentioned three-dimensional position value, three-dimensional velocity value and pose estimation value is exported as the navigation data of this integrated navigation system
Value externally output；
Described GNSS module (200) also includes antenna (201)；Described integrated BD2 and GPS navigation chip (205) is believed by BD2
Number front-end processing unit (202), gps signal front-end processing unit (203) and Base-Band Processing SOC (204) compositions；Wherein, antenna
(201) signal input part, the letter of gps signal front-end processing unit (203) respectively with BD2 signal front-end processings unit (202)
Number input is connected, signal output part and the gps signal front-end processing unit (203) of BD2 signal front-end processings unit (202)
Signal input part of the signal output part respectively with Base-Band Processing SOC (204) be connected；
Described magnetometer module (300) includes x-axis magnetoresistive transducer (301), y-axis magnetoresistive transducer (302), z-axis magnetic resistance and passed
Sensor (303) and magnetoresistance signal process circuit (304)；Wherein, the signal output part of x-axis magnetoresistive transducer (301), y-axis magnetic resistance
The signal output part of sensor (302), the signal output part of z-axis magnetoresistive transducer (303) respectively with magnetoresistance signal process circuit
(304) signal input part is connected, and the signal received is carried out after analog-to-digital conversion by magnetoresistance signal process circuit (304),
Export digitized three-dimensional magnetic field strength signal；
The integrated navigation computer (400) includes attitude algorithm wave filter (401) and integrated navigation extended Kalman filter
(402)；Wherein, attitude algorithm wave filter (401) obtains signal, the 3 axis MEMS accelerometer of three axis MEMS gyro (101)
(102) signal and the signal of magnetometer module (300)；Integrated navigation extended Kalman filter (402) obtains three axles
The signal of the signal of MEMS gyroscope (101), the signal of 3 axis MEMS accelerometer (102) and GNSS module (200)；Group
The output signal for closing navigation extended Kalman filter (402) feeds back to attitude algorithm wave filter (401)；
Output signal of the graceful wave filters of the integrated navigation expansion card Lovell (402) to the GNSS module (200) that receives, three axles
The output signal of MEMS gyroscope (101) and the output signal of 3 axis MEMS accelerometer (102) are filtered computing, obtain this
Three-dimensional position value, three-dimensional velocity value and the accelerometer drift estimate value of integrated navigation system；
The attitude algorithm wave filter (401) adds to the output signal of the three axis MEMS gyro (101) that receives, 3 axis MEMS
The output signal of speedometer (102), the output signal of magnetometer module (300) and by the graceful wave filters of integrated navigation expansion card Lovell
(402) the accelerometer drift estimate value obtained by filtering operation carries out fusion operation, obtains the appearance of this integrated navigation system
State angular estimation value；
The pose estimation value, three-dimensional position value and three-dimensional velocity value of this integrated navigation system are outwards defeated as the result of navigation
Go out；
Characterized in that, the integrated navigation system based on BD, GPS and MEMS is as follows the step of obtaining navigation coordinate：
Step 1：Angular velocity signal, three axles of three axis MEMS gyro (101) output are read by integrated navigation computer (400)
The ratio force signal of mems accelerometer (102) output and the navigation coordinate signal of GNSS module (200) output, and led by combination
Boat extended Kalman filter (402) is converted to the three-dimensional position value, three-dimensional velocity value and acceleration of this integrated navigation system
Count drift value；
Step 2：Angular velocity signal, three axles of three axis MEMS gyro (101) output are read by integrated navigation computer (400)
Ratio force signal, the three-dimensional magnetic field strength signal of magnetometer module (300) output of mems accelerometer (102) output, and pass through
Attitude algorithm wave filter (401) is converted to the attitude angle numerical value of the absolute value of this integrated navigation system；
Step 3：Step 1 is obtained into accelerometer drift value by integrated navigation computer (400) and changes into gyroscopic drift numerical value
With magnetometer distortion value；
Step 4：Step 3 is obtained into gyroscopic drift numerical value and magnetometer distortion value input attitude algorithm wave filter (401), it is right
The attitude angle numerical value of the absolute value obtained by step 2 is corrected, and obtains the pose estimation value of this integrated navigation system；
Step 5：It regard the three-dimensional position value, three-dimensional velocity value and pose estimation value of this integrated navigation system as navigation operations
As a result externally output；
The pose estimation value of this integrated navigation system is obtained as follows：
Step a：The pose estimation value of last moment is obtained by integrated navigation computer (400)Magnetic field intensity signalbm、
Compare force signalbA and earth magnetic dip anglenb；
Step b：Kinetic model is set up using the step a parameters obtained by integrated navigation computer (400)；
The kinetic model includes object functionAnd Jacobian matrix
Step c：The object function for being obtained step b by integrated navigation computer (400)With refined gram
Compare matrixEquation below is substituted into, calculates and obtains attitude angular rate normalization evaluated error
<mrow>
<msub>
<mmultiscripts>
<mover>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mrow>
<mi>E</mi>
<mi>r</mi>
<mi>r</mi>
<mi>o</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mi>J</mi>
<msup>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mmultiscripts>
<mi>b</mi>
<mi>n</mi>
</mmultiscripts>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>&CenterDot;</mo>
<mi>f</mi>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mmultiscripts>
<mi>a</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mmultiscripts>
<mi>b</mi>
<mi>n</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mmultiscripts>
<mi>m</mi>
<mi>b</mi>
</mmultiscripts>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mo>|</mo>
<mo>|</mo>
<mi>J</mi>
<msup>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mmultiscripts>
<mi>b</mi>
<mi>n</mi>
</mmultiscripts>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
<mo>&CenterDot;</mo>
<mi>f</mi>
<mfenced open = "(" close = ")">
<mtable>
<mtr>
<mtd>
<mrow>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mmultiscripts>
<mi>a</mi>
<mi>b</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mmultiscripts>
<mi>b</mi>
<mi>n</mi>
</mmultiscripts>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mmultiscripts>
<mi>m</mi>
<mi>b</mi>
</mmultiscripts>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>|</mo>
<mo>|</mo>
</mrow>
</mfrac>
<mo>;</mo>
</mrow>
Step d：The attitude angular rate normalization evaluated error for being obtained step c by integrated navigation computer (400) substitutes into as follows
Formula, calculates the drift estimate value for obtaining gyroscopebωBias；
<mrow>
<msub>
<mmultiscripts>
<mi>&omega;</mi>
<mi>b</mi>
</mmultiscripts>
<mrow>
<mi>E</mi>
<mi>r</mi>
<mi>r</mi>
<mi>o</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>=</mo>
<msup>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>*</mo>
</msup>
<mo>&CircleTimes;</mo>
<msub>
<mmultiscripts>
<mover>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mrow>
<mi>E</mi>
<mi>r</mi>
<mi>r</mi>
<mi>o</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>;</mo>
</mrow>
bωBias=α ΣbωErrorΔt；
In formula, factor alpha is the gyro output error of calibration zero-bit；
Step e：By integrated navigation computer (400) by the drift estimate value of the step d gyroscopes obtainedbωBiasSubstitute into following public
Formula, calculates the gyro output angle speed after being correctedbωC；
bωC=bω-bωBias；
In formula,bω is the angular velocity data of initial gyroscope normalized output；
Step f：Gyro output angle speed after the correction for being obtained step d by integrated navigation computer (400)bωCSubstitute into as follows
Formula, solves real-time attitude quaternary number
<mrow>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>=</mo>
<mn>0.5</mn>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>&CircleTimes;</mo>
<msub>
<mmultiscripts>
<mi>&omega;</mi>
<mi>b</mi>
</mmultiscripts>
<mi>C</mi>
</msub>
<mo>-</mo>
<mi>&beta;</mi>
<msub>
<mmultiscripts>
<mover>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mrow>
<mi>E</mi>
<mi>r</mi>
<mi>r</mi>
<mi>o</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>;</mo>
</mrow>
<mrow>
<mmultiscripts>
<mi>q</mi>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>=</mo>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>^</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>+</mo>
<mmultiscripts>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>n</mi>
<mi>b</mi>
</mmultiscripts>
<mo>&CenterDot;</mo>
<mi>&Delta;</mi>
<mi>t</mi>
<mo>;</mo>
</mrow>
In formula, filter coefficient β is the gyro output error of calibration nonzero digit；
The real-time attitude quaternary numberThe as pose estimation value of this integrated navigation system.
2. the method as claimed in claim 1 that navigation coordinate is obtained using the integrated navigation system based on BD, GPS and MEMS,
Characterized in that, reading the ratio force signal and magnetometer mould of 3 axis MEMS accelerometer (102) by integrated navigation computer (400)
The magnetic field intensity signal of block (300), and the absolute three-dimension altitude angle angle value [phi of this integrated navigation system is obtained as the following formula
theta psi]：The pose estimation value of last moment is not read for integrated navigation computer (400)When, led with this combination
The quaternary numerical value of the original state of boat system replaces the pose estimation value of last moment to use；
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
<mo>=</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
<mo>=</mo>
<mi>arcsin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
<mo>=</mo>
<mo>-</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mrow>
<mrow>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
Wherein：For the three-dimensional output valve of 3 axis MEMS accelerometer (102),For magnetometer mould
The three-dimensional output valve of block (300)；
By integrated navigation computer (400) by absolute three-dimension altitude angle angle value [the phi theta of this integrated navigation system
Psi] the quaternary numerical value for the original state for obtaining this integrated navigation system is converted to as the following formula：
<mrow>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>=</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>=</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>q</mi>
<mn>4</mn>
</msub>
<mo>=</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>h</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>t</mi>
<mi>h</mi>
<mi>e</mi>
<mi>t</mi>
<mi>a</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<mi>cos</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mi>p</mi>
<mi>s</mi>
<mi>i</mi>
</mrow>
<mn>2</mn>
</mfrac>
<mo>)</mo>
</mrow>
<mo>.</mo>
</mrow>
3
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510010594.9A CN104698485B (en) | 2015-01-09 | 2015-01-09 | Integrated navigation system and air navigation aid based on BD, GPS and MEMS |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510010594.9A CN104698485B (en) | 2015-01-09 | 2015-01-09 | Integrated navigation system and air navigation aid based on BD, GPS and MEMS |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN104698485A CN104698485A (en) | 2015-06-10 |
| CN104698485B true CN104698485B (en) | 2017-10-03 |
Family
ID=53345805
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510010594.9A Active CN104698485B (en) | 2015-01-09 | 2015-01-09 | Integrated navigation system and air navigation aid based on BD, GPS and MEMS |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN104698485B (en) |
Families Citing this family (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105607093B (en) * | 2015-12-20 | 2018-05-08 | 上海华测导航技术股份有限公司 | A kind of integrated navigation system and the method for obtaining navigation coordinate |
| CN105823481B (en) | 2015-12-21 | 2019-02-12 | 上海华测导航技术股份有限公司 | A kind of GNSS-INS vehicle method for determining posture based on single antenna |
| CN105698792A (en) * | 2016-01-26 | 2016-06-22 | 上海实汇机电科技有限公司 | Dynamic MEMS (micro-electromechanical systems) inertial attitude measuring system based on self-adaptive robust integration algorithm |
| CN107289930B (en) * | 2016-04-01 | 2020-09-11 | 南京理工大学 | Pure inertial vehicle navigation method based on MEMS inertial measurement unit |
| CN106597017B (en) * | 2016-12-16 | 2019-07-26 | 上海拓攻机器人有限公司 | A kind of unmanned plane Angular Acceleration Estimation and device based on Extended Kalman filter |
| CN106979780B (en) * | 2017-05-22 | 2019-06-14 | 江苏亘德科技有限公司 | A kind of unmanned vehicle real-time attitude measurement method |
| CN107894603A (en) * | 2017-12-21 | 2018-04-10 | 黑龙江惠达科技发展有限公司 | A kind of method based on Low-cost GPS inertia combined navigation positioning and optimizing |
| CN108344413B (en) * | 2018-02-07 | 2020-08-25 | 东南大学 | Underwater glider navigation system and low-precision and high-precision conversion method thereof |
| CN110325822B (en) | 2018-04-25 | 2023-06-27 | 深圳市大疆创新科技有限公司 | Cradle head pose correction method and cradle head pose correction device |
| CN109325705A (en) * | 2018-10-11 | 2019-02-12 | 北京三驰惯性科技股份有限公司 | A kind of driving habit methods of marking and system based on inertia integration technology |
| CN110058288B (en) * | 2019-04-28 | 2021-04-06 | 北京微克智飞科技有限公司 | Course error correction method and system for unmanned aerial vehicle INS/GNSS combined navigation system |
| CN111272175B (en) * | 2020-03-05 | 2022-05-20 | 北京航空航天大学 | Micro-mechanical gyroscope POS data acquisition and processing system |
| CN113063417A (en) * | 2021-03-12 | 2021-07-02 | 东莞市攀星智能运动科技有限公司 | Control method based on foot posture interaction |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN201266089Y (en) * | 2008-09-05 | 2009-07-01 | 北京七维航测科技发展有限公司 | INS/GPS combined navigation system |
| CN103217700A (en) * | 2013-04-10 | 2013-07-24 | 南昌大学 | GPS (global positioning system), IMU (inertial measurement unit), magnetometer and barometer combinational navigation system device |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CA2701529C (en) * | 2007-10-04 | 2017-07-25 | Trusted Positioning Inc. | System and method for intelligent tuning of kalman filters for ins/gps navigation applications |
| US20120086598A1 (en) * | 2010-10-08 | 2012-04-12 | Canadian Space Agency | Apparatus and methods for driftless attitude determination and reliable localization of vehicles |
-
2015
- 2015-01-09 CN CN201510010594.9A patent/CN104698485B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN201266089Y (en) * | 2008-09-05 | 2009-07-01 | 北京七维航测科技发展有限公司 | INS/GPS combined navigation system |
| CN103217700A (en) * | 2013-04-10 | 2013-07-24 | 南昌大学 | GPS (global positioning system), IMU (inertial measurement unit), magnetometer and barometer combinational navigation system device |
Non-Patent Citations (3)
| Title |
|---|
| EKF与UKF在紧耦合组合导航系统中的应用;赵思浩等;《系统工程与电子技术》;20091031;第31卷(第10期);第2450-2454页 * |
| 国产化卫星/MEMS组合导航模块的设计实现;王佩生等;《电子产品世界》;20120731(第7期);第77页第5段至第78页第4段，图1、图6 * |
| 开放式多模导航接收机设计;李文杰等;《电子产品世界》;20120930(第9期);第80页第3段，图1 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN104698485A (en) | 2015-06-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN107314718B (en) | High speed rotation bullet Attitude estimation method based on magnetic survey rolling angular rate information | |
| CN107270893B (en) | Lever arm and time asynchronous error estimation and compensation method for real estate measurement | |
| CN109596018B (en) | High-precision estimation method for flight attitude of spinning projectile based on magnetic roll angle rate information | |
| CN106052685B (en) | A kind of posture and course estimation method of two-stage separation fusion | |
| CN108225308A (en) | A kind of attitude algorithm method of the expanded Kalman filtration algorithm based on quaternary number | |
| CN112697138B (en) | Bionic polarization synchronous positioning and composition method based on factor graph optimization | |
| CN103712598B (en) | Attitude determination method of small unmanned aerial vehicle | |
| CN111722295B (en) | Underwater strapdown gravity measurement data processing method | |
| CN111189442A (en) | Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF | |
| CN110849360A (en) | Distributed relative navigation method for multi-machine cooperative formation flight | |
| CN105988129A (en) | Scalar-estimation-algorithm-based INS/GNSS combined navigation method | |
| CN107860382A (en) | A kind of method for measuring posture using AHRS in the case of magnetic anomaly | |
| CN111207734B (en) | EKF-based unmanned aerial vehicle integrated navigation method | |
| CN116660579A (en) | Wind speed data correction method, system and device | |
| Xue et al. | MEMS-based multi-sensor integrated attitude estimation technology for MAV applications | |
| CN112649001B (en) | Gesture and position resolving method for small unmanned aerial vehicle | |
| CN113932803A (en) | Inertia/geomagnetic/satellite combined navigation system suitable for high-dynamic aircraft | |
| CN111473786A (en) | Two-layer distributed multi-sensor combined navigation filtering method based on local feedback | |
| CN114264292B (en) | Gesture determining method based on accelerometer, sun sensor and GNSS and digital compass | |
| Edwan et al. | GPS/INS integration for GF-IMU of twelve mono-axial accelerometers configurations | |
| CN113984042B (en) | Series combined navigation method applicable to high-dynamic aircraft | |
| CN113124863B (en) | Mixed particle federated filtering data processing method based on KLD sampling | |
| Cheng et al. | Modeling and simulation of low-cost integrated navigation system on vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |