CN105928519B - Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer - Google Patents
Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer Download PDFInfo
- Publication number
- CN105928519B CN105928519B CN201610244759.3A CN201610244759A CN105928519B CN 105928519 B CN105928519 B CN 105928519B CN 201610244759 A CN201610244759 A CN 201610244759A CN 105928519 B CN105928519 B CN 105928519B
- Authority
- CN
- China
- Prior art keywords
- inertial navigation
- navigation
- carrier
- ins inertial
- navigation system
- 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.)
- Expired - Fee Related
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Abstract
The posture of unmanned plane and the navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer of location information can be accurately reflected the invention discloses one kind.The navigation algorithm using GPS navigation have the advantages that high positioning accuracy for a long time make up the increase at any time of INS inertial navigation cumulative errors and the shortcomings that dissipate；Using INS inertial navigation the GPS disadvantage limited vulnerable to interference and output frequency is not made up by the continuous feature of the navigation information of external interference, output, and in order to solve that geographical north can not be found by the calculated yaw angle of inertial navigation, and the situation that drift is larger, this algorithm is corrected using the calculated yaw angle of magnetometer, obtain geographical real north and stable yaw angle, in addition, this algorithm stability is stronger, satisfied navigator fix information can be exported, so as to accurately reflect the posture and location information of carrier.It is suitble to promote and apply in field of navigation technology.
Description
Technical field
It is especially a kind of based on INS inertial navigation and GPS navigation and magnetometer the present invention relates to field of navigation technology
Navigation algorithm.
Background technique
In terms of airmanship, navigation mode with the most use, most mature is answered to have INS inertial navigation and satellite to lead at present
Boat.The advantages of GPS satellite navigation is that have the characteristics that global, round-the-clock, long-time positioning accuracy is high, but the disadvantage is that signal is easy
It being disturbed and blocks, when under forceful electric power magnetic environment and having high building to block, signal quality is deteriorated, and its output frequency is limited, and one
As be 1-10Hz, output is discontinuous, in the occasion for needing quickly more new information, such as mobility and the higher nothing of requirement of real-time
In man-machine system, the shortcomings that GPS satellite navigation, is just highlighted.And INS inertial navigation system is a kind of navigation of full self-determination type
Mode, therefore there is very strong concealment and jamproof ability, and output information is continuous, positioning accuracy is high in the short time.
But the characteristics of due to MEMS-INS device itself, gyroscope and acceleration be in respect of initial zero bias, random drift equal error, with when
Between accumulative effect, error is increasing, and long-time positioning accuracy is poor, can not finally accurately reflect unmanned plane posture and
Location information.
Summary of the invention
Technical problem to be solved by the invention is to provide postures and location information that one kind can accurately obtain carrier
Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer.
The technical solution adopted by the present invention to solve the technical problems are as follows: should based on INS inertial navigation and GPS navigation and
The navigation algorithm of magnetometer, comprising the following steps:
A, the carrier angular speed parameter for measuring the gyroscope of INS inertial navigation systemSubstitute into quaternary
Fractional differentiation equation solution obtains quaternary number q0, q1, q2, q3；WhereinIt is gyroscope in carrier local Coordinate System
Under three measured axis angular velocity information；
The quaternion differential equation are as follows:
B, the q that will be solved in step A0, q1, q2, q3Following formula is substituted into solve to obtain attitude matrix
According to followingWith the relational expression of direction cosines
Be calculated carrier INS inertial navigation module attitude angle θ, γ,
C, the yaw angle measured using magnetometerThe yaw angle that replacement step B is calculated
D, the acceleration parameter f for measuring the accelerometer of INS inertial navigation systembPosture is obtained with solving in step B
MatrixIt substitutes into solve in following differential equations and obtains carrier on three directions in east, north, day under INS inertial navigation coordinate system
Velocity information vN vE vU, the differential equation are as follows:
Wherein, vn=[vN vE vU]TThe respectively upward velocity projections of the INS inertial navigation coordinate system Middle East, north, old name for the Arabian countries in the Middle East arrive
Speed in inertial navigation coordinate system,The speed of the earth rotation angle in inertial navigation coordinate system is projected to for rotational-angular velocity of the earth
Degree,It projects in inertial navigation coordinate system and turns around each axial rotation angular speed of INS inertial navigation coordinate system for unmanned plane
Dynamic angular speed, gnThe acceleration of gravity in inertial navigation coordinate system is projected to for acceleration of gravity；
E, the v that step D is calculatedN vE vUSubstituting into following formula solution respectively obtains carrier in INS inertial navigation system
Location information, wherein L is latitude, and λ is longitude, and h is height,
H=h (0)+∫ vUDt, wherein L (0) indicates carrier initial bit
The latitude value set, λ (0) indicate the longitude of carrier initial position, and h (0) indicates elemental height of the carrier apart from earth surface.RM
Indicate the radius of curvature on earth meridian circle, RNIndicate the radius of curvature in the parallel of latitude；
F, state equation is establishedWith observational equation Z (t)=H (t) XI(t)+V
(t),
XI(t) INS inertial navigation system is indicated in the error state of t moment, it is the vector of one 15 dimension, following institute
Show:
δvx,δvy,δvzFor
The upward velocity error of INS inertial navigation system Yan Dong, north, old name for the Arabian countries in the Middle East；φx,φy,φzFor the attitude error of carrier；δL,δλ,
δ h respectively represents latitude, longitude and altitude error where carrier；εx,εy,εzRespectively represent the random drift of gyroscope；The respectively random drift of accelerometer, whereinIt is one 15 × 15 square
Battle array；Wherein FN(t) correspond to δ vx,δvy,δvz, φx,φy,φz, the INS inertial navigation system square of this 9 parameters of δ L, δ λ, δ h
Battle array, nonzero element are as follows:
F (1,5)=- fzF (1,6)=fy
F (2,4)=fzF (2,6)=- fx
F (3,4)=- fyF (3,5)=fxF (3,7)=- 2 ωievx sinL
F (5,7)=- ωiesin L
F (9,3)=1
FSIt (t) is δ vx,δvy,δvz, φx,φy,φz, this 9 parameters of δ L, δ λ, δ h and gyroscope and accelerometer are drifted about
Between transformation matrix, dimension is 9 × 6,
FMIt (t) is εx,εy,εz,INS inertial navigation system square corresponding with gyroscope and accelerometer drift
Battle array is the diagonal matrix that a dimension is 6 × 6, is expressed as follows:
FM(t)=diag [- 1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；Wherein, TgxIndicate gyroscope x
The time constant of the error model of axis, TgyIndicate the time constant of the error model of gyroscope y-axis, TgzIndicate gyroscope z-axis
The time constant of error model, TaxIndicate the time constant of accelerometer x-axis error model, TayIndicate accelerometer y-axis error
The time constant of model, TazIndicate the time constant of accelerometer z-axis error model；
GI(t)=diag [1 1 1......1 1]15×15；
WIIt (t) is one 15 vector tieed up, as follows:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15Indicate systematic procedure noise sequence；
Z (t) is position speed of position and speed information and carrier of the carrier in INS inertial navigation in GPS navigation system
The difference for spending information, is 6 dimensional vectors,
Z (t)=[δ vx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T, wherein Nvx
Indicate the velocity error of GPS navigation system in the x direction, NvyIndicate the velocity error of GPS navigation system in y-direction, NvzTable
Show the velocity error of GPS navigation system in a z-direction, NxIndicate the location error of GPS navigation system in the x direction, NyIt indicates
The location error of GPS navigation system in y-direction, NhIndicate the location error of GPS navigation system in a z-direction；
Wherein
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G, by continuous state equation obtained aboveX is obtained after discretizationk=
Φk,k-1Xk-1+Wk-1, wherein
By continuous observation equation Z (t) obtained above=H (t) XI(t) Z is obtained after+V (t) discretizationk=HkXk+Vk；
Wherein I is unit matrix, and F is the state-transition matrix of INS inertial navigation system, and Δ t is INS inertia after discretization
The sampling time of navigation system；
H, the position of position and speed information and carrier in GPS navigation system by carrier in INS inertial navigation system
Velocity information obtains Z (t) in the observation information z at k moment as difference；
I, the optimal estimation value of k moment INS inertial navigation system state is calculated
Wherein, For in the optimal estimation value of k-1 moment INS inertial navigation system state,Qk-1It is making an uproar for INS inertial navigation system
Sound matrix, size are determined by the performance of INS inertial navigation element,
RkThe variance matrix of systematic survey noise, size be by
The performance of GPS receiver determines；
J, it will be calculatedThe position and speed information of value and carrier in INS inertial navigation system obtains most as difference
Excellent navigational parameter；
K, step H-J is repeated, the navigation information parameter of continuous carrier is obtained.
Beneficial effects of the present invention: it should be utilized based on the navigation algorithm of INS inertial navigation and GPS navigation and magnetometer
INS inertial navigation and the method for GPS navigation integrated navigation solve single GPS navigation technology vulnerable to interfering and blocking, in short-term
Positioning accuracy is not high, and output frequency is limited and exports discontinuous disadvantage；Also solves single INS inertial navigation ginseng simultaneously
The shortcomings that number cumulative errors are increasing, and long-time positioning accuracy dissipates has high positioning accuracy using GPS navigation for a long time
The advantages of come the shortcomings that making up the increase at any time of INS inertial navigation cumulative errors and dissipate；Using INS inertial navigation not by outer
Boundary's interference, the feature that the navigation information of output is continuous make up the GPS disadvantage limited vulnerable to interference and output frequency, and to understand
Geographical north, and the situation that drift is larger certainly can not be found by the calculated yaw angle of inertial navigation, this algorithm utilizes magnetometer meter
The yaw angle of calculating corrects, and obtains geographical real north and stable yaw angle, in addition, this algorithm stability is stronger, it can
Satisfied navigator fix information is exported, so as to accurately reflect the posture and location information of carrier.
Detailed description of the invention
Fig. 1 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Latitude error value；
Fig. 2 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Latitude error variance yields；
Fig. 3 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Longitude error value；
Fig. 4 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Longitude error variance yields；
Fig. 5 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Height error value；
Fig. 6 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Height error variance yields；
Fig. 7 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
East orientation speed error amount；
Fig. 8 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
East orientation speed error variance value；
Fig. 9 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
North orientation speed error amount；
Figure 10 is calculated to be of the present invention based on INS inertial navigation and the navigation algorithm of GPS navigation and magnetometer
North orientation speed error variance value；
Figure 11 is calculated to be of the present invention based on INS inertial navigation and the navigation algorithm of GPS navigation and magnetometer
Sky orientation speed error amount；
Figure 12 is calculated to be of the present invention based on INS inertial navigation and the navigation algorithm of GPS navigation and magnetometer
Sky orientation speed error variance value.
Specific embodiment
Navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer, including following step
It is rapid:
A, the carrier angular speed parameter for measuring the gyroscope of INS inertial navigation systemSubstitute into quaternary
Fractional differentiation equation solution obtains quaternary number q0, q1, q2, q3；WhereinIt is gyroscope in carrier local Coordinate System
Under three measured axis angular velocity information；
The quaternion differential equation are as follows:
B, the q that will be solved in step A0, q1, q2, q3Following formula is substituted into solve to obtain attitude matrix
According to followingWith the relational expression of direction cosines
Be calculated carrier INS inertial navigation module attitude angle θ, γ,
C, the yaw angle measured using magnetometerThe yaw angle that replacement step B is calculated
D, the acceleration parameter f for measuring the accelerometer of INS inertial navigation systembPosture is obtained with solving in step B
MatrixIt substitutes into solve in following differential equations and obtains carrier on three directions in east, north, day under INS inertial navigation coordinate system
Velocity information vN vE vU, the differential equation are as follows:
Wherein, vn=[vN vE vU]TThe respectively upward velocity projections of the INS inertial navigation coordinate system Middle East, north, old name for the Arabian countries in the Middle East arrive
Speed in inertial navigation coordinate system,The speed of the earth rotation angle in inertial navigation coordinate system is projected to for rotational-angular velocity of the earth
Degree,It projects in inertial navigation coordinate system and turns around each axial rotation angular speed of INS inertial navigation coordinate system for unmanned plane
Dynamic angular speed, gnThe acceleration of gravity in inertial navigation coordinate system is projected to for acceleration of gravity；
E, the v that step D is calculatedN vE vUSubstituting into following formula solution respectively obtains carrier in INS inertial navigation system
Location information, wherein L is latitude, and λ is longitude, and h is height,
H=h (0)+∫ vUDt, wherein L (0) indicates carrier initial bit
The latitude value set, λ (0) indicate the longitude of carrier initial position, and h (0) indicates elemental height of the carrier apart from earth surface.RM
Indicate the radius of curvature on earth meridian circle, RNIndicate the radius of curvature in the parallel of latitude；
F, state equation is establishedWith observational equation Z (t)=H (t) XI(t)+V
(t),
XI(t) INS inertial navigation system is indicated in the error state of t moment, it is the vector of one 15 dimension, following institute
Show:
δvx,δvy,δvzFor
The upward velocity error of INS inertial navigation system Yan Dong, north, old name for the Arabian countries in the Middle East；φx,φy,φzFor the attitude error of carrier；δL,δλ,
δ h respectively represents latitude, longitude and altitude error where carrier；εx,εy,εzRespectively represent the random drift of gyroscope；The respectively random drift of accelerometer, whereinIt is one 15 × 15 square
Battle array；Wherein FN(t) correspond to δ vx,δvy,δvz, φx,φy,φz, the INS inertial navigation system square of this 9 parameters of δ L, δ λ, δ h
Battle array, nonzero element are as follows:
F (1,5)=- fzF (1,6)=fy
F (2,4)=fzF (2,6)=- fx
F (3,4)=- fyF (3,5)=fxF (3,7)=- 2 ωievx sinL
F (5,7)=- ωiesin L
F (9,3)=1
FSIt (t) is δ vx,δvy,δvz, φx,φy,φz, this 9 parameters of δ L, δ λ, δ h and gyroscope and accelerometer are drifted about
Between transformation matrix, dimension is 9 × 6,
FMIt (t) is εx,εy,εz,INS inertial navigation system square corresponding with gyroscope and accelerometer drift
Battle array is the diagonal matrix that a dimension is 6 × 6, is expressed as follows:
FM(t)=diag [- 1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；Wherein, TgxIndicate gyroscope x
The time constant of the error model of axis, TgyIndicate the time constant of the error model of gyroscope y-axis, TgzIndicate gyroscope z-axis
The time constant of error model, TaxIndicate the time constant of accelerometer x-axis error model, TayIndicate accelerometer y-axis error
The time constant of model, TazIndicate the time constant of accelerometer z-axis error model；
GI(t)=diag [1 1 1......1 1]15×15；
WIIt (t) is one 15 vector tieed up, as follows:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15Indicate systematic procedure noise sequence；
Z (t) is position speed of position and speed information and carrier of the carrier in INS inertial navigation in GPS navigation system
The difference for spending information, is 6 dimensional vectors,
Z (t)=[δ vx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cosLδλ+Nx δh+Nh]T, wherein Nvx
Indicate the velocity error of GPS navigation system in the x direction, NvyIndicate the velocity error of GPS navigation system in y-direction, NvzTable
Show the velocity error of GPS navigation system in a z-direction, NxIndicate the location error of GPS navigation system in the x direction, NyIt indicates
The location error of GPS navigation system in y-direction, NhIndicate the location error of GPS navigation system in a z-direction；
Wherein
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G, by continuous state equation obtained aboveX is obtained after discretizationk=
Φk,k-1Xk-1+Wk-1, wherein
By continuous observation equation Z (t) obtained above=H (t) XI(t) Z is obtained after+V (t) discretizationk=HkXk+Vk；
Wherein I is unit matrix, and F is the state-transition matrix of INS inertial navigation system, and Δ t is INS inertia after discretization
The sampling time of navigation system；
H, the position of position and speed information and carrier in GPS navigation system by carrier in INS inertial navigation system
Velocity information obtains Z (t) in the observation information z at k moment as difference；
I, the optimal estimation value of k moment INS inertial navigation system state is calculated
Wherein, For in the optimal estimation value of k-1 moment INS inertial navigation system state,Qk-1It is making an uproar for INS inertial navigation system
Sound matrix, size are determined by the performance of INS inertial navigation element,
RkThe variance matrix of systematic survey noise, size be by
The performance of GPS receiver determines；
J, it will be calculatedThe position and speed information of value and carrier in INS inertial navigation system obtains most as difference
Excellent navigational parameter；
K, step H-J is repeated, the navigation information parameter of continuous carrier is obtained.
Beneficial effects of the present invention: it should be utilized based on the navigation algorithm of INS inertial navigation and GPS navigation and magnetometer
INS inertial navigation and the method for GPS navigation integrated navigation solve single GPS navigation technology vulnerable to interfering and blocking, in short-term
Positioning accuracy is not high, and output frequency is limited and exports discontinuous disadvantage；Also solves single INS inertial navigation ginseng simultaneously
The shortcomings that number cumulative errors are increasing, and long-time positioning accuracy dissipates has high positioning accuracy using GPS navigation for a long time
The advantages of come the shortcomings that making up the increase at any time of INS inertial navigation cumulative errors and dissipate；Using INS inertial navigation not by outer
Boundary's interference, the feature that the navigation information of output is continuous make up the GPS disadvantage limited vulnerable to interference and output frequency, and to understand
Geographical north, and the situation that drift is larger certainly can not be found by the calculated yaw angle of inertial navigation, this algorithm utilizes magnetometer meter
The yaw angle of calculating corrects, and obtains geographical real north and stable yaw angle, in addition, this algorithm stability is stronger, it can
Satisfied navigator fix information is exported, so as to accurately reflect the posture and location information of carrier.
Fig. 1 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Latitude error value；Fig. 2 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer calculates
Latitude error variance yields out；Fig. 3 is that the navigation of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
The longitude error value that method is calculated；Fig. 4 is leading based on INS inertial navigation and GPS navigation and magnetometer of the present invention
The longitude error variance yields that boat algorithm is calculated；Fig. 5 is based on INS inertial navigation and GPS navigation and magnetic to be of the present invention
The height error value that the navigation algorithm of power meter is calculated；Fig. 6 be it is of the present invention based on INS inertial navigation and GPS navigation with
And the height error variance yields that the navigation algorithm of magnetometer is calculated；Fig. 7 be it is of the present invention based on INS inertial navigation with
The east orientation speed error amount that the navigation algorithm of GPS navigation and magnetometer is calculated；Fig. 8 is of the present invention used based on INS
Property navigation and GPS navigation and magnetometer the east orientation speed error variance value that is calculated of navigation algorithm；Fig. 9 is institute of the present invention
State the north orientation speed error amount that the navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer is calculated；Figure 10 is
The north orientation speed error that navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Variance yields；Figure 11 is that the navigation algorithm of the present invention based on INS inertial navigation and GPS navigation and magnetometer is calculated
Sky orientation speed error amount；Figure 12 is of the present invention based on the navigation algorithm of INS inertial navigation and GPS navigation and magnetometer
The sky orientation speed error variance value obtained；
It can be seen that from above-mentioned test result figure of the present invention based on INS inertial navigation and GPS navigation and magnetometer
Navigation algorithm obtain longitude, latitude, height error variance can fast convergence to smaller numerical value；To position, speed
The navigation informations such as degree are also able to achieve filter smoothing effect, will not generate big jump, the stability of algorithm is stronger.
Claims (1)
1. the navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer, it is characterised in that the following steps are included:
A, the carrier angular speed parameter for measuring the gyroscope of INS inertial navigation systemIt is micro- to substitute into quaternary number
Equation solution is divided to obtain quaternary number q0, q1, q2, q3；WhereinIt is gyroscope under carrier local Coordinate System
The angular velocity information of three axis measured；
The quaternion differential equation are as follows:
B, the q that will be solved in step A0, q1, q2, q3Following formula is substituted into solve to obtain attitude matrix
According to followingWith the relational expression of direction cosines
Be calculated carrier INS inertial navigation module attitude angle θ, γ,
C, the yaw angle measured using magnetometerThe yaw angle that replacement step B is calculated
D, the acceleration parameter f for measuring the accelerometer of INS inertial navigation systembAttitude matrix is obtained with solving in step B
It substitutes into solve in following differential equations and obtains speed of the carrier on three directions in east, north, day under INS inertial navigation coordinate system
Information vN vE vU, the differential equation are as follows:
Wherein, vn=[vN vE vU]TThe respectively upward velocity projections of the INS inertial navigation coordinate system Middle East, north, old name for the Arabian countries in the Middle East are to inertia
Speed in navigational coordinate system,The rotational-angular velocity of the earth in inertial navigation coordinate system is projected to for rotational-angular velocity of the earth,The rotation in inertial navigation coordinate system is projected to around each axial rotation angular speed of INS inertial navigation coordinate system for unmanned plane
Angular speed, gnThe acceleration of gravity in inertial navigation coordinate system is projected to for acceleration of gravity；
E, the v that step D is calculatedN vE vUFollowing formula solution is substituted into respectively obtains position of the carrier in INS inertial navigation system
Confidence breath, wherein L is latitude, and λ is longitude, and h is height,
H=h (0)+∫ vUDt, wherein L (0) indicates carrier initial position
Latitude value, λ (0) indicate the longitude of carrier initial position, and h (0) indicates elemental height of the carrier apart from earth surface, RMIt indicates
Radius of curvature on earth meridian circle, RNIndicate the radius of curvature in the parallel of latitude；
F, state equation is establishedWith observational equation Z (t)=H (t) XI(t)+V (t),
XI(t) INS inertial navigation system is indicated in the error state of t moment, it is the vector of one 15 dimension, as follows:
XI(t)=[δ vx δvy δvz φx φy φz δL δλ δh εx εy εz ▽x ▽y ▽z], δ vx,δvy,δvzFor INS
The upward velocity error of inertial navigation system Yan Dong, north, old name for the Arabian countries in the Middle East；φx,φy,φzFor the attitude error of carrier；δL,δλ,δh
Latitude, longitude and altitude error where respectively representing carrier；εx,εy,εzRespectively represent the random drift of gyroscope；▽x,▽y,
▽zThe respectively random drift of accelerometer, whereinIt is one 15 × 15 matrix；Its
Middle FN(t) correspond to δ vx,δvy,δvz, φx,φy,φz, the INS inertial navigation system matrix of this 9 parameters of δ L, δ λ, δ h,
Nonzero element is as follows:
F (3,4)=- fyF (3,5)=fxF (3,7)=- 2 ωievxsin L
FSIt (t) is δ vx,δvy,δvz, φx,φy,φz, between this 9 parameters of δ L, δ λ, δ h and gyroscope and accelerometer drift
Transformation matrix, dimension are 9 × 6,
FMIt (t) is εx,εy,εz, ▽x,▽y,▽zINS inertial navigation system matrix corresponding with gyroscope and accelerometer drift,
It is the diagonal matrix that a dimension is 6 × 6, is expressed as follows:
FM(t)=diag [- 1/Tgx -1/Tgy -1/Tgz -1/Tax -1/Tay -1/Taz]；Wherein, TgxIndicate gyroscope x-axis
The time constant of error model, TgyIndicate the time constant of the error model of gyroscope y-axis, TgzIndicate the error of gyroscope z-axis
The time constant of model, TaxIndicate the time constant of accelerometer x-axis error model, TayIndicate accelerometer y-axis error model
Time constant, TazIndicate the time constant of accelerometer z-axis error model；
GI(t)=diag [1 1 1......1 1]15×15；
WIIt (t) is one 15 vector tieed up, as follows:
WI(t)=[a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15],
a1 a2 a3 a4 a5 a6 a7 a8 a9 a10 a11 a12 a13 a14 a15Indicate systematic procedure noise sequence；
Z (t) is position and speed letter of position and speed information and carrier of the carrier in INS inertial navigation in GPS navigation system
The difference of breath is 6 dimensional vectors,
Z (t)=[δ vx+Nvx δvy+Nvy δvz+Nvz (RM+h)δL+Ny (RM+h)cos Lδλ+Nx δh+Nh]T, wherein NvxTable
Show the velocity error of GPS navigation system in the x direction, NvyIndicate the velocity error of GPS navigation system in y-direction, NvzIt indicates
The velocity error of GPS navigation system in a z-direction, NxIndicate the location error of GPS navigation system in the x direction, NyIndicate GPS
The location error of navigation system in y-direction, NhIndicate the location error of GPS navigation system in a z-direction；
Wherein
Vv(t)=[Nvx Nvy Nvz]T
Vp(t)=[Nx Ny Nz]T
G, by continuous state equation obtained aboveX is obtained after discretizationk=
Φk,k-1Xk-1+Wk-1, wherein
By continuous observation equation Z (t) obtained above=H (t) XI(t) Z is obtained after+V (t) discretizationk=HkXk+Vk；
Wherein I is unit matrix, and F is the state-transition matrix of INS inertial navigation system, and Δ t is INS inertial navigation after discretization
The sampling time of system；
H, the position and speed of position and speed information and carrier in GPS navigation system by carrier in INS inertial navigation system
Information obtains Z (t) in the observation information z at k moment as difference；
I, the optimal estimation value of k moment INS inertial navigation system state is calculated
Wherein, For in the optimal estimation value of k-1 moment INS inertial navigation system state,Qk-1It is making an uproar for INS inertial navigation system
Sound matrix, size are determined by the performance of INS inertial navigation element,
RkThe variance matrix of systematic survey noise, size be by
The performance of GPS receiver determines；
J, it will be calculatedThe position and speed information work difference of value and carrier in INS inertial navigation system obtains optimal
Navigational parameter；
K, step H-J is repeated, the navigation information parameter of continuous carrier is obtained.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610244759.3A CN105928519B (en) | 2016-04-19 | 2016-04-19 | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201610244759.3A CN105928519B (en) | 2016-04-19 | 2016-04-19 | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105928519A CN105928519A (en) | 2016-09-07 |
| CN105928519B true CN105928519B (en) | 2019-03-29 |
Family
ID=56838544
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201610244759.3A Expired - Fee Related CN105928519B (en) | 2016-04-19 | 2016-04-19 | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105928519B (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2784859C1 (en) * | 2022-06-14 | 2022-11-30 | Федеральное государственное бюджетное образовательное учреждение высшего образования "Саратовский государственный технический университет имени Гагарина Ю.А." (СГТУ имени Гагарина Ю.А.) | Method for platform-free orientation of moving objects |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106949889A (en) * | 2017-03-17 | 2017-07-14 | 南京航空航天大学 | For the inexpensive MEMS/GPS integrated navigation systems and method of pedestrian navigation |
| CN109932739A (en) * | 2017-12-15 | 2019-06-25 | 财团法人车辆研究测试中心 | The localization method of Adaptive Weight adjustment |
| CN110095121B (en) * | 2019-04-10 | 2021-07-30 | 北京微克智飞科技有限公司 | Unmanned aerial vehicle course resolving method and system capable of resisting body magnetic interference |
| CN111765887A (en) * | 2020-07-10 | 2020-10-13 | 北京航空航天大学 | Indoor three-dimensional positioning method based on MEMS sensor and FM broadcast signal |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104765377A (en) * | 2015-04-10 | 2015-07-08 | 哈尔滨工业大学深圳研究生院 | Unmanned helicopter flying control platform system based on QNX |
| CN105021183A (en) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | Low-cost GPS and INS integrated navigation system for multi-rotor aircrafts |
-
2016
- 2016-04-19 CN CN201610244759.3A patent/CN105928519B/en not_active Expired - Fee Related
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104765377A (en) * | 2015-04-10 | 2015-07-08 | 哈尔滨工业大学深圳研究生院 | Unmanned helicopter flying control platform system based on QNX |
| CN105021183A (en) * | 2015-07-05 | 2015-11-04 | 电子科技大学 | Low-cost GPS and INS integrated navigation system for multi-rotor aircrafts |
Non-Patent Citations (2)
| Title |
|---|
| "Information Fusion Distributed Navigation for UAVs Formation Flight";ZHEN Ziyang;《Proceedings of 2014 IEEE Chinese Guidance, Navigation and Control Conference》;20140810;正文第1520-1524页 |
| "一种基于INS/GPS的无人机组合导航控制系统的设计";何清华;《飞航导弹》;20071231(第2期);正文第34-37页 |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| RU2784859C1 (en) * | 2022-06-14 | 2022-11-30 | Федеральное государственное бюджетное образовательное учреждение высшего образования "Саратовский государственный технический университет имени Гагарина Ю.А." (СГТУ имени Гагарина Ю.А.) | Method for platform-free orientation of moving objects |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105928519A (en) | 2016-09-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105928515B (en) | A kind of UAV Navigation System | |
| CN107655476B (en) | Pedestrian high-precision foot navigation method based on multi-information fusion compensation | |
| CN109556632B (en) | INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering | |
| CN103245360B (en) | Carrier-borne aircraft rotation type strapdown inertial navigation system Alignment Method under swaying base | |
| CN104698486B (en) | A kind of distribution POS data processing computer system real-time navigation methods | |
| CN106767787A (en) | A kind of close coupling GNSS/INS combined navigation devices | |
| CN105929836B (en) | Control method for quadrotor | |
| CN110926468B (en) | Communication-in-motion antenna multi-platform navigation attitude determination method based on transfer alignment | |
| CN106643709B (en) | Combined navigation method and device for offshore carrier | |
| CN103389092B (en) | A kind of kite balloon airship attitude measuring and measuring method | |
| CN103196445B (en) | Based on the carrier posture measuring method of the earth magnetism supplementary inertial of matching technique | |
| CN104697520B (en) | Integrated gyro free strap down inertial navigation system and gps system Combinated navigation method | |
| CN105928519B (en) | Navigation algorithm based on INS inertial navigation and GPS navigation and magnetometer | |
| CN111121766B (en) | Astronomical and inertial integrated navigation method based on starlight vector | |
| CN109916395A (en) | A kind of autonomous Fault-tolerant Integrated navigation algorithm of posture | |
| CN109073388B (en) | Gyromagnetic geographic positioning system | |
| CN104049269B (en) | A kind of target navigation mapping method based on laser ranging and MEMS/GPS integrated navigation system | |
| CN104374388A (en) | Flight attitude determining method based on polarized light sensor | |
| CN101162147A (en) | Marine fiber optic gyroscope attitude heading reference system mooring extractive alignment method under the large heading errors | |
| CN111323050A (en) | Strapdown inertial navigation and Doppler combined system calibration method | |
| CN112432642B (en) | Gravity beacon and inertial navigation fusion positioning method and system | |
| CN109931955A (en) | Strapdown inertial navigation system Initial Alignment Method based on the filtering of state correlation Lie group | |
| CN107677292B (en) | Vertical line deviation compensation method based on gravity field model | |
| CN110296719B (en) | On-orbit calibration method | |
| CN103278165A (en) | Remanence-calibration-based autonomous navigation method of magnetic survey and starlight backup based on |
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 | ||
| CB03 | Change of inventor or designer information |
Inventor after: Zhang Yu Inventor after: Li Shiyang Inventor after: Zheng Wulue Inventor before: Zhang Yu Inventor before: Li Shiyang |
|
| CB03 | Change of inventor or designer information | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20191126 Address after: 510663, science Avenue, Luogang District Science City, Guangdong, 181, A4, ninth to 11, Guangzhou Patentee after: Guangzhou Bureau of Extra High Voltage Transmission Company of China Southern Power Grid Co., Ltd. Address before: 1, No. 610000, 11 floor, No. 69, Tianfu Third Street, Chengdu hi tech Zone, Sichuan, 1115 Patentee before: CHENGDU EBIT AUTOMATION EQUIPMENT CO., LTD. |
|
| TR01 | Transfer of patent right | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20190329 Termination date: 20200419 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |