CN110672130B - EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle - Google Patents

EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle Download PDF

Info

Publication number
CN110672130B
CN110672130B CN201911131570.3A CN201911131570A CN110672130B CN 110672130 B CN110672130 B CN 110672130B CN 201911131570 A CN201911131570 A CN 201911131570A CN 110672130 B CN110672130 B CN 110672130B
Authority
CN
China
Prior art keywords
polarized light
navigation system
inertial
equation
state
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
Application number
CN201911131570.3A
Other languages
Chinese (zh)
Other versions
CN110672130A (en
Inventor
杜涛
田常正
曾云豪
星艳
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
North China University of Technology
Original Assignee
North China University of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by North China University of Technology filed Critical North China University of Technology
Priority to CN201911131570.3A priority Critical patent/CN110672130B/en
Publication of CN110672130A publication Critical patent/CN110672130A/en
Application granted granted Critical
Publication of CN110672130B publication Critical patent/CN110672130B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices

Landscapes

  • Engineering & Computer Science (AREA)
  • Manufacturing & Machinery (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Navigation (AREA)

Abstract

The invention relates to an EKF (extended Kalman filter) alignment method of an inertial/polarized light integrated navigation system under a large misalignment angle, which comprises the steps of selecting an initially aligned state vector of the inertial/polarized light integrated navigation system, and establishing a nonlinear error state equation of the inertial/polarized light integrated navigation system under the large misalignment angle; calculating a sun vector according to a polarization azimuth angle measured by the polarized light sensor, and establishing a polarized light nonlinear measurement equation; establishing a speed error measurement equation according to the speed output of the inertial navigation system; establishing a unified nonlinear measurement equation of the inertial/polarized light combined navigation system by using an augmentation technology; discretizing a nonlinear equation of the inertial/polarized light combined navigation system; designing an extended Kalman filter to estimate error states of a misalignment angle, a speed error, gyro drift, accelerometer constant bias and the like of the inertial/polarized light integrated navigation system; and performing feedback correction on the attitude and the speed of the inertial/polarized light combined navigation system, and improving the estimation precision and the speed of the initial alignment under a large misalignment angle. The invention has the advantages of high precision, high speed and strong autonomy.

Description

EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle
Technical Field
The invention relates to an EKF (extended Kalman filter) alignment method of an inertial/polarized light combined navigation system under a large misalignment angle, which can effectively improve the initial alignment precision of a static base under the large misalignment angle of a ground carrier in the earth atmosphere and shorten the initial alignment time.
Background
Navigation is a key technology in the fields of aviation, aerospace, navigation, ground traffic and the like. Since navigation is an integration process, the initial state has a large impact on its accuracy. In order to improve navigation positioning and constant speed precision, the navigation system needs to perform initial alignment before entering a navigation state. Initial navigation parameters including attitude, speed and position information are accurately obtained through initial alignment. In the static base condition, the carrier remains stationary, with zero speed and unchanged position, so the initial alignment under the static base is to obtain the initial attitude of the vehicle. How to realize the initial alignment of the static base with high precision and large misalignment angle in a short time is a hot issue of current research.
At present, the static base initial alignment method is mainly divided into two stages: coarse alignment and fine alignment. The coarse alignment requires that the carrier attitude with lower precision is obtained by using methods such as double-vector attitude determination and the like in a shorter time, and the precision is lower; in the fine alignment stage, according to the rough attitude obtained by the coarse alignment, the high-precision initial attitude is obtained by estimating the misalignment angle and performing feedback correction on the misalignment angle, but the time is long. When the initial attitude precision obtained by coarse alignment is higher, the misalignment angle between the computed navigation system and the real navigation system can be considered to be smaller, an attitude matrix formed by the misalignment angle can be linearized, and a linear inertial navigation error state model is further established; however, when the misalignment angle is too large, the formed attitude matrix cannot meet the requirement of linearization, and if the initial attitude error obtained by linearization is still large, an inertial navigation nonlinear error state model under a large misalignment angle should be established, and the initial alignment is realized by designing a nonlinear filter.
The traditional alignment method adopts an inertial navigation system, and performs strapdown calculation on the output of an accelerometer to obtain a speed error as a measurement value for initial alignment. The method has the problems that only speed errors are used as measurement information, so that the east gyro has inconspicuous drifting, the alignment of a coupling antenna misalignment angle is slow, the alignment time is long, and the alignment precision is low. In order to improve the observability of the system, the gyro output of the inertial navigation system can be used as a new measurement to shorten the alignment time and improve the alignment precision. However, when the gyro accuracy is low, the output is greatly affected by noise, and the alignment accuracy is seriously affected. Therefore, in order to overcome the disadvantages of a single sensor, the inertial navigation system can be initially aligned with other sensors such as a satellite, a magnetic compass and a star sensor to form a combined navigation system, and the problems also exist: the satellite only provides speed and position information, and extra measurement cannot be provided under the static base, so that the accuracy of initial alignment under the static base is not improved greatly; the magnetic compass is greatly interfered by electromagnetism, has poor robustness, can generate large errors when the surrounding magnetic field is abnormal, and has not ideal alignment effect in the initial alignment; the star sensor can estimate the attitude error of the carrier with high precision, but is expensive and not suitable for the initial alignment of the static base in the daytime period in the earth atmosphere. Based on the reasons, it is important to find an autonomous sensor with strong anti-interference capability and low cost and an inertial navigation which are mutually fused to realize the initial alignment under the large misalignment angle of the static base.
Navigation by using sky polarized light is a novel bionic navigation mode. After sunlight enters an atmosphere, original parallel light is scattered through atmospheric particles, so that the polarization phenomenon of the sunlight is caused. Research shows that organisms such as solenopsis invicta can return to nests along an approximately straight path after foraging, which is inseparable from the sun polarization phenomenon. The sandants utilize a polarization countermeasure unit in the compound eye to perform decoding calculation on polarized light incident into the compound eye, thereby obtaining navigation information. The polarized light sensor designed according to the bionic navigation principle can sense the maximum polarization direction of incident light in the measuring direction of the sensor and output a polarization azimuth angle. The polarized light navigation based on the polarized azimuth angle has the advantages of strong autonomy, no accumulated error and the like, and can make up the conditions that the error of an inertial navigation system is accumulated along with time and is dispersed in speed, thereby obtaining high-precision attitude information. Thus, it is feasible to initially align the stationary base with the polarized light sensor in combination with the inertial navigation system.
In summary, in order to overcome the defects of low alignment precision, long alignment time and the like of the method for initially aligning the static base under the large misalignment angle, the inertial and polarized light sensors are combined to form an inertial/polarized light combined navigation system, the observability of the system is improved by introducing new measurement information, and the rapidity and the accuracy of the initial alignment of the static base under the large misalignment angle are realized. Wherein a large misalignment angle refers to a misalignment angle of more than 5 °.
Related patents in the prior art are:
compared with the invention patent with patent number 201310069511.4 "initial alignment method of movable base of SINS/GPS/polarized light combined navigation system", the invention has the following differences:
(1) the problem to be solved is different, the invention aims at the problem of initial alignment of a large misalignment angle under a static base, so that extra information is not required to be provided by a GPS (global positioning system), and autonomous initial alignment can be realized;
(2) the established state models are different, the method is provided aiming at the initial alignment under a large misalignment angle, the system model is nonlinear under the large misalignment angle, and the larger the misalignment angle is, the stronger the nonlinearity degree is; the invention patent with patent number 201310069511.4 'the initial alignment method of the moving base of the SINS/GPS/polarized light combined navigation system' aims at the condition of small misalignment angle, and the state model is linear;
(3) the polarization mechanism model is different, the polarization measurement equation obtained by the invention is completely different from the invention patent with patent number 201310069511.4 'an initial alignment method of a movable base of an SINS/GPS/polarized light combined navigation system'. The invention uses two vertically placed polarized light sensors to calculate the sun vector according to the space position of the sensors, thereby obtaining more accurate polarization measurement information. In the patent of 201310069511.4 entitled "initial alignment method for moving base of SINS/GPS/polarized light integrated navigation system", only one polarized light sensor is used to obtain polarization measurement, the calculation process is complicated, and division exists at the same time, which causes the noise not to satisfy the gaussian distribution, resulting in poor filtering result.
Compared with the invention patents of the patent No. 201811421552.4, "an INS/GNSS/polarization/geomagnetism combined navigation alignment method based on kalman filtering" and the patent No. 201811414200.6, "an INS/GNSS/polarization/geomagnetism combined navigation system alignment method based on least squares", the invention has the following differences:
(1) the application scenes are different, the invention aims at the initial alignment problem of the large misalignment angle of the static base, does not need the GPS to provide speed information, and can realize autonomous alignment;
(2) the state and the measurement model are different, the established state and the measurement model are nonlinear aiming at the problem of large misalignment angle alignment, and the invention only uses polarized light and speed error as measurement information and does not use geomagnetic information;
(3) the filtering methods are different, and because the model established by the method is nonlinear, the extended Kalman filter is designed to realize the fusion of the sensors for state estimation.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the large misalignment angle alignment method of the inertial/polarized light integrated navigation system based on the extended Kalman filtering is high in precision, fast in alignment and strong in autonomy.
The technical solution of the invention is as follows: an EKF alignment method of an inertial/polarized light combined navigation system under a large misalignment angle is realized by the following steps:
(1) selecting a misalignment angle, a speed error, a gyro drift and an accelerometer constant bias as a state vector of the inertial/polarized light combined navigation system, and establishing a nonlinear error state equation of the initially aligned inertial/polarized light combined navigation system under a large misalignment angle;
(2) calculating a sun vector according to a polarization azimuth angle measured by the polarized light sensor, and establishing a polarized light nonlinear measurement equation;
(3) establishing a speed error measurement equation according to the speed output of the inertial navigation system;
(4) vectorizing the polarized light nonlinear measurement equation established in the step (2) and the speed error measurement equation established in the step (3) by using an augmentation technology, and establishing a unified inertial/polarized light combined navigation system nonlinear measurement equation;
(5) establishing a nonlinear state equation and a measurement equation of the inertial/polarized light combined navigation system on the basis of the steps (1) and (4), and discretizing the established nonlinear state equation and the measurement equation of the inertial/polarized light combined navigation system to obtain a discrete state equation and a discrete measurement equation of the inertial/polarized light combined navigation system;
(6) designing an extended Kalman filter according to the state equation and the measurement equation of the discrete inertia/polarized light integrated navigation system established in the step (5), estimating the unknown state of the inertia/polarized light integrated navigation system, and obtaining the misalignment angle, the speed error, the gyro drift and the accelerometer constant deviation of the inertia/polarized light integrated navigation system;
(7) and (4) compensating the attitude and the speed based on the inertial navigation system according to the misalignment angle and the speed error estimated in the step (6), and obtaining the corrected attitude and speed of the inertial/polarized light combined navigation system by using a feedback correction method.
In the step (1), a misalignment angle, a speed error, a gyro drift and an accelerometer constant bias are selected as state vectors of the inertial/polarized light combined navigation system, and a nonlinear error state equation of the initial alignment inertial/polarized light combined navigation system under a large misalignment angle is established. The state vector of the initial alignment of the selected inertial/polarized light combined navigation system is as follows:
Figure BDA0002278454880000051
wherein phi isE，φNAnd phiUThe misalignment angles of the east direction, the north direction and the sky direction under the navigation coordinate system n are respectively expressed as error angles between the navigation coordinate system n' and the n system;
Figure BDA0002278454880000052
and
Figure BDA0002278454880000053
is n is the lower triaxial velocity error;
Figure BDA0002278454880000054
and
Figure BDA0002278454880000055
constant bias of the three-axis gyroscope in a carrier coordinate system b;
Figure BDA0002278454880000056
and
Figure BDA0002278454880000057
and the constant bias is the constant bias of the three-axis accelerometer under the b system.
Under the initial alignment of a large misalignment angle, the nonlinear error state equation of the inertial/polarized light combined navigation system is as follows:
Figure BDA0002278454880000058
Figure BDA0002278454880000059
Figure BDA00022784548800000510
Figure BDA00022784548800000511
wherein the content of the first and second substances,
Figure BDA00022784548800000512
Figure BDA00022784548800000513
is an attitude transformation matrix between n and n' of the computational navigation system, expressed as:
Figure BDA00022784548800000514
fbb is the lower specific force obtained by an accelerometer; gnIs the local gravity vector;
Figure BDA00022784548800000515
an attitude transformation matrix of b and n';
Figure BDA00022784548800000516
the representation of the angular velocity of the system e of the earth relative to the system i of the inertia system under the system n is generated by the rotation of the earth;
Figure BDA00022784548800000517
as vectors
Figure BDA00022784548800000518
The asymmetric matrix form of (a) is:
Figure BDA0002278454880000061
wherein the content of the first and second substances,
Figure BDA0002278454880000062
and
Figure BDA0002278454880000063
is composed of
Figure BDA0002278454880000064
The three-axis component of (a); i is3×3Is a 3 × 3 dimensional identity matrix, 03×1Is a 3 × 1 dimensional all 0 vector;
Figure BDA0002278454880000065
and
Figure BDA0002278454880000066
respectively, the noise vectors of the gyroscope and the accelerometer, and the order state noise vector is
Figure BDA0002278454880000067
And obey Gaussian white noise distribution with mean value of 0 and variance of Q, namely Q to N (0)12×1,Q)，Q＝E[qqT]，01×6Is a 1 × 6 dimensional all-0 vector, 012×1Is a 12 x 1 dimensional all 0 vector.
In the step (2), the nonlinear measurement equation of the polarized light is established as follows:
Figure BDA0002278454880000068
wherein the content of the first and second substances,
Figure BDA0002278454880000069
to use a polarized light sensor m1The obtained measurement vector is measured by the measuring device,
Figure BDA00022784548800000610
for a system of polarized light sensor modules m1The attitude transformation relation between b system is the polarized light sensor m1The mounting relation with the carrier can be obtained by calibration before alignment; sm1Is the sun vector at m1The following expressions can be obtained by calculation of a polarized light sensor; snThe representation of the sun vector under the n system is obtained by calculation according to the astronomical calendar by using the local position and the time; delta Sm1Is Sm1The measurement error of (2);
Figure BDA00022784548800000611
for polarimetric measurement of noise vector, obey mean 0 and variance R1Of white Gaussian noise distribution, i.e. rpol～N(03×1,R1)，
Figure BDA00022784548800000612
03×1Is a 3 x 1 dimensional all 0 vector.
In the step (3), the velocity error measurement equation is established as follows:
δv＝[03×3 I3×3 03×3 03×3]X+rv
wherein the content of the first and second substances,
Figure BDA00022784548800000613
outputting the calculated triaxial velocity value for the accelerometer of the inertial navigation system, 03×3Is a 3 x 3 dimensional full 0 matrix, rvFor the velocity error noise vector, obey mean 0 and variance R2Of white Gaussian noise distribution, i.e. rv～N(03×1,R2)，
Figure BDA00022784548800000614
In the step (4), the polarized light nonlinear measurement equation established in the step (2) and the velocity error measurement equation established in the step (3) are vectorized by using an augmentation technology, and a unified inertial/polarized light combined navigation system nonlinear measurement equation is established. The established unified inertial/polarized light combined navigation nonlinear measurement equation is as follows:
y＝h(X)+r
wherein the content of the first and second substances,
Figure BDA0002278454880000071
h (-) is the nonlinear measurement function of the inertial/polarized light integrated navigation system, and the specific form is as follows:
Figure BDA0002278454880000072
r is a measurement noise vector, and
Figure BDA0002278454880000073
06×1is a 6 x 1 dimensional all 0 vector.
In the step (5), based on the steps (1) and (4), a nonlinear state equation and a measurement equation of the inertial/polarized light integrated navigation system are established, and the established nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system are discretized to obtain a discrete state equation and a discrete measurement equation of the inertial/polarized light integrated navigation system. The established nonlinear state equation and the measurement equation of the inertial/polarized light combined navigation system are as follows:
Figure BDA0002278454880000074
y＝h(X)+r
wherein f (-) is nonlinear state function of the inertial/polarized light combined navigation system, and the specific form is
Figure BDA0002278454880000075
Q is a state noise vector obeying Gaussian white noise distribution with a mean value of 0 and a variance of Q, namely Q-N (0)12×1,Q)，Q＝E[qqT]，012×1Is a 12 x 1 dimensional all 0 vector.
Discretizing the nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system to obtain a discrete inertial/polarized light integrated navigation system state equation and a discrete measurement equation:
Figure BDA0002278454880000076
y(k)＝h(X(k))+r(k)
where k denotes the kth instant, at is the sampling time interval,
Figure BDA0002278454880000077
is a first order Taylor series expansion of the nonlinear state function f (-) at X (k-1).
In the step (6), according to the state equation and the measurement equation of the discrete inertia/polarized light integrated navigation system established in the step (5), an extended kalman filter is designed, the unknown state of the inertia/polarized light integrated navigation system is estimated, and the misalignment angle, the velocity error, the gyro drift and the accelerometer constant deviation of the inertia/polarized light integrated navigation system are obtained. The designed extended kalman filter is as follows:
time updating:
one-step prediction equation of state:
Figure BDA0002278454880000081
wherein the content of the first and second substances,
Figure BDA0002278454880000082
is an estimated value of the time k-1,
Figure BDA0002278454880000083
a one-step predicted value at the moment k is obtained;
one-step prediction state error covariance equation:
P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)ΦT(k|k-1)+Q(k-1)
wherein, P (k-1| k-1) is an error covariance matrix at the moment k-1, P (k | k-1) is a one-step prediction error covariance matrix of a state at the moment k, a state transition matrix phi (k | k-1) ═ I + A (X (k-1| k-1)) deltat, and Q (k-1) is a covariance matrix of state noise at the moment k-1;
measurement updating:
the state estimation equation:
Figure BDA0002278454880000084
wherein the content of the first and second substances,
Figure BDA0002278454880000085
for the optimal estimation of the state at the moment k, k (k) is a Kalman filtering gain matrix, and the calculation method comprises the following steps:
K(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R(k))-1
wherein the content of the first and second substances,
Figure BDA0002278454880000086
is a nonlinear measurement function h (-) in
Figure BDA0002278454880000087
(k) is a covariance matrix of the measured noise at time k;
estimation error covariance equation:
P(k|k)＝(I-K(k)H(k))P(k|k-1)(I-K(k)H(k))T+K(k)R(k)KT(k)
where I is an identity matrix of the same dimension as the covariance matrix P (k | k-1).
In the step (7), the feedback correction method is as follows:
(1) attitude correction method
First, the transformation matrix of n' and n systems is calculated
Figure BDA0002278454880000091
Wherein the content of the first and second substances,
Figure BDA0002278454880000092
and
Figure BDA0002278454880000093
is the estimated misalignment angle for the state. The corrected attitude matrix
Figure BDA0002278454880000094
Comprises the following steps:
Figure BDA0002278454880000095
(2) speed correction method
Setting the corrected speed of the triaxial carrier as Vx、VyAnd VzThen the velocity correction is expressed as:
Figure BDA0002278454880000096
wherein the content of the first and second substances,
Figure BDA0002278454880000097
and
Figure BDA0002278454880000098
respectively taking the three-axis speed values of the inertial navigation system under the n series;
Figure BDA0002278454880000099
and
Figure BDA00022784548800000910
is the three-axis velocity error of the state estimation under n.
Compared with the prior art, the invention has the advantages that: the invention combines the polarized light sensor and the inertial navigation system, can improve the precision of the initial alignment of the carrier and shorten the alignment time. The polarization azimuth angle output by the polarized light sensor is not accumulated along with time, and the method has high autonomy and high-precision attitude correction capability. Under a large misalignment angle, the traditional inertia only adopts a speed error as measurement, so that the east gyro drift cannot be directly observed, and further the alignment of the sky misalignment angle is slow and the alignment accuracy is poor. The sun vector obtained by the polarized light sensor is used as a measurement vector, so that the defects of the traditional inertial navigation system can be overcome, the misalignment angle can be quickly aligned, and the alignment precision is high. Where the initial misalignment angle is [10 ° 5 ° 90 ° ], the antenna misalignment angle may be aligned to 0.03 ° within 80s through the initial alignment.
The method has the advantages of high precision, high speed and strong autonomy, and can be used in the alignment field of the polarization combined navigation system in the fields of steamships, unmanned aerial vehicles, ground robots and the like.
Drawings
FIG. 1 is a design flow diagram of the present invention;
fig. 2 is a coordinate relation diagram of the dual-polarization light sensor.
Detailed Description
The present invention will be described in detail below with reference to the accompanying drawings and examples.
As shown in FIG. 1, the method comprises the following steps:
the method comprises the steps of firstly, selecting a misalignment angle, a speed error, a gyro drift and an accelerometer constant bias as state vectors of the inertial/polarized light combined navigation system, and establishing a nonlinear error state equation of the initially aligned inertial/polarized light combined navigation system under a large misalignment angle. The state vector of the initial alignment of the selected inertial/polarized light combined navigation system is
Figure BDA0002278454880000101
Wherein phi isE，φNAnd phiUAs misalignment angles of east, north and sky respectively in the navigation coordinate system n, expressed as calculating the navigation seatThe index n' is the error angle between n and n;
Figure BDA0002278454880000102
and
Figure BDA0002278454880000103
is n is the lower triaxial velocity error;
Figure BDA0002278454880000104
and
Figure BDA0002278454880000105
constant bias of the three-axis gyroscope in a carrier coordinate system b;
Figure BDA0002278454880000106
and
Figure BDA0002278454880000107
and the constant bias is the constant bias of the three-axis accelerometer under the b system.
Under the initial alignment of a large misalignment angle, the nonlinear error state equation of the inertial/polarized light combined navigation system is as follows:
Figure BDA0002278454880000108
Figure BDA0002278454880000109
Figure BDA00022784548800001010
Figure BDA00022784548800001011
wherein the content of the first and second substances,
Figure BDA00022784548800001012
Figure BDA00022784548800001013
is an attitude transformation matrix between n and n' of the computational navigation system, expressed as:
Figure BDA00022784548800001014
fbb is a specific force, which can be obtained by an accelerometer; gnIs the local gravity vector;
Figure BDA00022784548800001015
is an attitude transformation matrix of b and n.
Figure BDA00022784548800001016
The angular velocity of the system e of the earth relative to the system i of the inertial system is expressed by the system n, and is generated by the rotation of the earth,
Figure BDA0002278454880000111
as vectors
Figure BDA0002278454880000112
The asymmetric matrix form of (a) is:
Figure BDA0002278454880000113
wherein the content of the first and second substances,
Figure BDA0002278454880000114
and
Figure BDA0002278454880000115
is composed of
Figure BDA0002278454880000116
The three-axis component of (a). I is3×3Is a 3 × 3 dimensional identity matrix, 03×1Is 3X 1A dimension full 0 vector.
Figure BDA0002278454880000117
And
Figure BDA0002278454880000118
noise vectors of the gyroscope and the accelerometer, respectively, order-state noise vectors
Figure BDA0002278454880000119
And obey Gaussian white noise distribution with mean value of 0 and variance of Q, namely Q to N (0)12×1,Q)，Q＝E[qqT]，012×1Is a 12 x 1 dimensional all 0 vector.
Secondly, calculating a sun vector according to the polarization azimuth angle measured by the polarized light sensor, and establishing a polarized light nonlinear measurement equation:
sun vector Sm1Module system m of polarized light sensor1Can be expressed as follows:
Figure BDA00022784548800001110
wherein λ ism1And
Figure BDA00022784548800001111
respectively a series of polarized light sensor modules m1Is tied to the solar altitude and azimuth, and
Figure BDA00022784548800001112
is the projection of the sun vector on the horizontal plane and m1Is an included angle of a positive half shaft of x,
Figure BDA00022784548800001113
similarly, the sun vector under n is:
Figure BDA00022784548800001114
wherein λ isnAnd
Figure BDA00022784548800001115
respectively an altitude angle and an azimuth angle of the sun vector under n system, and
Figure BDA00022784548800001116
is the angle between the sun vector and the north direction of n. Under a static base, the position of the carrier is fixed, and accurate S can be obtained by giving specific position and time information and utilizing an astronomical calendarn。Sm1And SnCan be converted to each other by the following relationship:
Figure BDA00022784548800001117
wherein the content of the first and second substances,
Figure BDA00022784548800001118
is n is the same as m1The attitude transformation matrix of the system. According to the chain rule, there are:
Figure BDA0002278454880000121
wherein the content of the first and second substances,
Figure BDA0002278454880000122
is b is a group of m1The coordinate transformation matrix between the systems shows the installation relationship between the polarized light sensor and the carrier, and can be calibrated before alignment. Substituting formula (12) into formula (11), and arranging to obtain:
Figure BDA0002278454880000123
the polarized light sensor can be interfered by the outside world in the measuring process, so that the m obtained actually1The lower sun vector is:
Figure BDA0002278454880000124
wherein the content of the first and second substances,
Figure BDA0002278454880000125
is m1Is the measured value of the sun vector, δ Sm1Is m1Is the measurement error of the lower sun vector. Substituting formula (14) for formula (13) to obtain:
Figure BDA0002278454880000126
order to
Figure BDA0002278454880000127
03×1Is 3 × 1 for all 0 vectors. In the above formula (15), the nonlinear measurement equation based on the polarized light measurement is:
Figure BDA0002278454880000128
because the single polarized light sensor can not obtain lambdam1To solve for Sm1Therefore, a dual-polarized light sensor model is set up to calculate Sm1。
A schematic diagram of a dual polarization light sensor is shown in fig. 2. Wherein Oxyz is the module base coordinate system, the coordinate system and m1Are overlapped. The coordinate systems of the two polarized light sensors are m respectively1System and m2Is a member of the series, P1，P2For each sensor vertex, and the two sensors are respectively rotated clockwise and counterclockwise around the x-axis of the b-system
Figure BDA0002278454880000129
And (4) installing and placing.
Thus, there are:
Figure BDA00022784548800001210
Figure BDA00022784548800001211
Figure BDA00022784548800001212
the sunlight enters an observation point O after being scattered at the point S, OS is a sun vector and is respectively represented as S under each coordinate systemm1、Sm2And SbAnd satisfies the following relationship:
Figure BDA0002278454880000131
Figure BDA0002278454880000132
Figure BDA0002278454880000133
Spm1and Spm2Are respectively m1System and m2The included angle between the projection of the sun vector under the system and the positive half shaft of the respective module system x is the sun azimuth angle and is recorded as
Figure BDA0002278454880000134
And
Figure BDA0002278454880000135
and is
Figure BDA0002278454880000136
λm1，λm2Are respectively the solar altitude angles under two coordinate systems, and
Figure BDA0002278454880000137
θm1，θm2the respective modules are at the sun scattering angle, and
Figure BDA0002278454880000138
em1，em2the included angle between the polarization vector under each module system and the positive semiaxis x of each module system is the polarization angle, namely
Figure BDA0002278454880000139
And is
Figure BDA00022784548800001310
m1Is the polarization vector em1Can be expressed as:
Figure BDA00022784548800001311
sun vector is in m1The projection under can be expressed as:
Figure BDA00022784548800001312
defined by the azimuth of polarization, the polarization vector is perpendicular to the sun vector and its scattering plane and thus, has
Figure BDA00022784548800001313
Wherein, | | Spm1| is the projection vector Spm1Norm of (d). It can be obtained from the formula (25),
Figure BDA00022784548800001314
likewise, at m2The following are:
Figure BDA0002278454880000141
for spherical triangle SP1P2Sine and cosine of spherical triangleThe chord theorem can know that:
Figure BDA0002278454880000142
cos(P1S)＝cos(P2S)cos(P1P2)+sin(P2S)sin(P1P2)cos(∠P1P2S) (29)
wherein, P1S，P2S is the scattering angle thetam1And thetam2Arc, P, of said pair1P2Subtend an arc angle of
Figure BDA0002278454880000143
And recording the angle P of the spherical surface2P1S，∠P1P2S is respectively gamma1And gamma2Thus, equations (28), (29) may be rewritten as:
Figure BDA0002278454880000144
cos(θm1)＝sin(θm2)cos(γ2) (31)
equation (30) is divided by equation (31) to yield:
Figure BDA0002278454880000145
in fact, γ1Is the sun vector projection Spm1And m1The negative half axis of the system y axis forms an angle, and therefore, there is:
Figure BDA0002278454880000146
the vertical connection type (26) and (33) are provided with:
Figure BDA0002278454880000147
from formula (34):
Figure BDA0002278454880000148
likewise, γ2Is the sun vector projection Spm2And m2The positive half axis of the system y axis forms an included angle, and therefore, there are:
Figure BDA0002278454880000149
united (27) and (36) and arranged as follows:
Figure BDA00022784548800001410
m1the relationship between the solar altitude and the scattering angle is:
Figure BDA0002278454880000151
substituting equation (37) into (31) yields:
Figure BDA0002278454880000152
further, equation (39) can be developed to yield:
Figure BDA0002278454880000153
Figure BDA0002278454880000154
in the formula (40) and (41), the formulae (35) and (37) are substituted by:
Figure BDA0002278454880000155
Figure BDA0002278454880000156
wherein the content of the first and second substances,
Figure BDA0002278454880000157
it is noted that the signs in equations (42) and (43) are irrelevant.
When the expressions (42) and (43) are substituted into the expression (9), the sun vector Sm1Can be re-expressed as:
Figure BDA0002278454880000158
the last equation of equation (44) holds because the signs in equations (42) and (43) are uncorrelated, and if we refer to ± outside the matrix, the last term needs to be added with extra ±.
The solar azimuth angle and the polarization angle satisfy the following relation:
Figure BDA0002278454880000159
substituting formula (45) into formula (44) to obtain:
Figure BDA0002278454880000161
expanding equation (46) by:
Figure BDA0002278454880000162
the expression (47) shows 4 candidates
Figure BDA0002278454880000163
And there are two groupsThe candidate vectors are in opposite directions. Likewise, at m2The following formula can also be obtained according to the above method:
Figure BDA0002278454880000164
wherein the content of the first and second substances,
Figure BDA0002278454880000165
similarly, the expression (48) also shows 4 candidates
Figure BDA0002278454880000166
And there are two sets of candidate vectors with opposite directions. Through calculation, the following results are obtained:
Figure BDA0002278454880000167
since the sun vector is m1System and m2The following relationships are present:
Figure BDA0002278454880000168
Figure BDA0002278454880000169
by using the formulae (47) and (50), it is possible to obtain:
Figure BDA00022784548800001610
due to the fact that after transformation
Figure BDA00022784548800001614
And Sm2Are equal to each other, order
Figure BDA00022784548800001612
Can reserve 2 m1Is the sun vector, namely:
Figure BDA00022784548800001613
equation (53) shows that 2 candidate vectors remain
Figure BDA0002278454880000171
In opposite directions and at the same time perpendicular to em1And em2. Therefore, further calculations are still needed to determine the unique sun vector.
Sun vector is in m1Lines and b have the following relationship:
Figure BDA0002278454880000172
wherein the content of the first and second substances,
Figure BDA0002278454880000173
substituting (53) into (54) to obtain candidate sun vector under b system
Figure BDA0002278454880000174
To resolve directional ambiguity, a gravity vector is introduced here. Because the included angle between different vectors is not changed under different coordinate systems, the following are included:
Figure BDA0002278454880000175
wherein, gnAnd gbRespectively, the representation of the gravity vector under the n system and the b system. sign () is a sign function, with a value of-1 or 1. Any sun vector
Figure BDA0002278454880000176
All the substitution (56) can only obtain a unique sun vector Sb. Order to
Figure BDA0002278454880000177
And substitute into
Figure BDA0002278454880000178
Obtaining the polarization measurement information ypol。
Thirdly, calculating a sun vector according to the polarization azimuth angle measured by the polarized light sensor, and establishing a polarized light nonlinear measurement equation as follows:
δv＝[03×3 I3×3 03×3 03×3]X+rv (57)
wherein the content of the first and second substances,
Figure BDA0002278454880000179
outputting the calculated triaxial velocity value for the accelerometer of the inertial navigation system, 03×3Is a 3 x 3 dimensional full 0 matrix, rvFor the velocity error noise vector, obey mean 0 and variance R2Of white Gaussian noise distribution, i.e. rv～N(03×1,R2)，
Figure BDA00022784548800001710
And fourthly, vectorizing the polarized light nonlinear measurement equation established in the second step and the velocity error measurement equation established in the third step by using an augmentation technology, and establishing a unified inertial/polarized light combined navigation system nonlinear measurement equation. The established unified inertial/polarized light combined navigation nonlinear measurement equation is as follows:
y＝h(X)+r (58)
wherein the content of the first and second substances,
Figure BDA0002278454880000181
h (-) is the nonlinear measurement function of the inertial/polarized light integrated navigation system, and the specific form is as follows:
Figure BDA0002278454880000182
r is a measurement noise vector, and
Figure BDA0002278454880000183
06×1is a 6 x 1 dimensional all 0 vector.
And fifthly, on the basis of the first step and the fourth step, establishing a nonlinear state equation and a measurement equation of the inertial/polarized light combined navigation system, and discretizing the established nonlinear state equation and the measurement equation of the inertial/polarized light combined navigation system to obtain a discrete state equation and a discrete measurement equation of the inertial/polarized light combined navigation system. The established nonlinear state equation and the measurement equation of the inertial/polarized light combined navigation system are as follows:
Figure BDA0002278454880000184
y＝h(X)+r (61)
wherein f (-) is the nonlinear state function of the inertial/polarized light combined navigation system, and the specific form is as follows:
Figure BDA0002278454880000185
q is a state noise vector obeying Gaussian white noise distribution with a mean value of 0 and a variance of Q, namely Q-N (0)12×1,Q)，Q＝[qqT]，012×1Is a 12 x 1 dimensional all 0 vector.
Discretizing the nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system to obtain a discrete inertial/polarized light integrated navigation system state equation and a discrete measurement equation:
Figure BDA0002278454880000186
y(k)＝h(X(k))+r(k) (64)
where k denotes the kth instant, at is the sampling time interval,
Figure BDA0002278454880000187
is a first order Taylor series expansion of the nonlinear state function f (-) at X (k-1).
And sixthly, designing an extended Kalman filter according to the state equation and the measurement equation of the discrete inertial/polarized light integrated navigation system established in the step five, estimating the unknown state of the inertial/polarized light integrated navigation system, and obtaining the misalignment angle, the speed error, the gyro drift and the accelerometer constant deviation of the inertial/polarized light integrated navigation system. The designed extended kalman filter is as follows:
time updating:
one-step prediction equation of state:
Figure BDA0002278454880000191
wherein the content of the first and second substances,
Figure BDA0002278454880000192
is an estimated value of the time k-1,
Figure BDA0002278454880000193
and predicting the value of one step at the moment k.
One-step prediction state error covariance equation:
P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)ΦT(k|k-1)+Q(k-1) (66)
wherein, P (k-1| k-1) is an error covariance matrix at the time k-1, P (k | k-1) is a one-step prediction error covariance matrix of a state at the time k, a state transition matrix Φ (k | k-1) ═ I + a (X (k-1| k-1)) Δ t, and Q (k-1) is a covariance matrix of state noise at the time k-1.
Measurement updating:
the state estimation equation:
Figure BDA0002278454880000194
wherein the content of the first and second substances,
Figure BDA0002278454880000195
for the optimal estimation of the state at the moment k, k (k) is a Kalman filtering gain matrix, and the calculation method comprises the following steps:
K(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R(k))-1 (68)
wherein the content of the first and second substances,
Figure BDA0002278454880000196
is a nonlinear measurement function h (-) in
Figure BDA0002278454880000197
The first order Taylor series expansion of (A), (B), R (k) is the covariance matrix of the measured noise at time k.
Estimation error covariance equation:
P(k|k)＝(I-K(k)H(k))P(k|k-1)(I-K(k)H(k))T+K(k)R(k)KT(k) (69)
where I is an identity matrix with the same dimension as the covariance P (k | k-1).
And seventhly, compensating the attitude and the speed based on the inertial navigation system according to the misalignment angle and the speed error estimated in the sixth step, and obtaining the corrected attitude and speed of the inertial/polarized light combined navigation system by using a feedback correction method. The feedback correction method used is as follows:
the system state vector estimated by the extended Kalman filter is set as follows:
Figure BDA0002278454880000201
(1) attitude correction method
First, the transformation matrix of n' and n systems is calculated
Figure BDA0002278454880000202
Wherein the content of the first and second substances,
Figure BDA0002278454880000203
and
Figure BDA0002278454880000204
is the estimated misalignment angle for the state. Then, the corrected attitude matrix
Figure BDA0002278454880000205
Comprises the following steps:
Figure BDA0002278454880000206
(2) speed correction method
Setting the corrected speed of the triaxial carrier as Vx、VyAnd VzThen the velocity correction can be expressed as:
Figure BDA0002278454880000207
wherein the content of the first and second substances,
Figure BDA0002278454880000208
and
Figure BDA0002278454880000209
respectively taking the three-axis speed values of the inertial navigation system under the n series;
Figure BDA00022784548800002010
and
Figure BDA00022784548800002011
is the three-axis velocity error of the state estimation under n.
And after the navigation system carries out feedback correction, entering the next navigation calculation process.
The invention combines the polarized light sensor and the inertial navigation system, can improve the precision of the initial alignment of the carrier and shorten the alignment time. The polarization azimuth angle output by the polarized light sensor is not accumulated along with time, and the method has high autonomy and high-precision attitude correction capability. Because the traditional inertia only adopts speed errors as measurement, the east gyro drift cannot be directly observed, and the alignment of the sky misalignment angle is slow, and the alignment precision is low. The sun vector obtained by the polarized light sensor is used as a measurement vector, so that the defect of the traditional inertia can be overcome, the antenna misalignment angle can be aligned quickly, and the alignment precision is high.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Claims (1)

1. An EKF alignment method of an inertial/polarized light combined navigation system under a large misalignment angle is characterized in that the inertial/polarized light combined navigation system consists of an inertial navigation system and a polarized light navigation system, and the EKF alignment method comprises the following steps:
(1) selecting a misalignment angle, a speed error, a gyro drift and an accelerometer constant bias as a state vector of the inertial/polarized light combined navigation system, and establishing a nonlinear error state equation of the initially aligned inertial/polarized light combined navigation system under a large misalignment angle;
(2) calculating a sun vector according to a polarization azimuth angle measured by the polarized light sensor, and establishing a polarized light nonlinear measurement equation;
(3) establishing a speed error measurement equation according to the speed output of the inertial navigation system;
(4) vectorizing the polarized light nonlinear measurement equation established in the step (2) and the speed error measurement equation established in the step (3) by using an augmentation technology, and establishing a unified inertial/polarized light combined navigation system nonlinear measurement equation;
(5) establishing a nonlinear state equation and a measurement equation of the inertial/polarized light combined navigation system on the basis of the steps (1) and (4), and discretizing the established nonlinear state equation and the measurement equation of the inertial/polarized light combined navigation system to obtain a discrete state equation and a discrete measurement equation of the inertial/polarized light combined navigation system;
(6) designing an extended Kalman filter according to the state equation and the measurement equation of the discrete inertia/polarized light integrated navigation system established in the step (5), estimating the unknown state of the inertia/polarized light integrated navigation system, and obtaining the misalignment angle, the speed error, the gyro drift and the accelerometer constant deviation of the inertia/polarized light integrated navigation system;
(7) compensating the attitude and the speed based on the inertial navigation system according to the misalignment angle and the speed error estimated in the step (6), and obtaining the attitude and the speed of the corrected inertial/polarized light combined navigation system by using a feedback correction method;
in the step (1), a misalignment angle, a velocity error, a gyro drift and an accelerometer constant bias are selected as state vectors of the inertial/polarized light integrated navigation system, and a nonlinear error state equation of the initially aligned inertial/polarized light integrated navigation system under a large misalignment angle is established, wherein the initially aligned state vectors of the selected inertial/polarized light integrated navigation system are as follows:
Figure FDA0003143454830000021
wherein phi isE，φNAnd phiUThe misalignment angles of the east direction, the north direction and the sky direction under the navigation coordinate system n are respectively expressed as error angles between the navigation coordinate system n' and the n system;
Figure FDA0003143454830000022
and
Figure FDA0003143454830000023
is n is the lower triaxial velocity error;
Figure FDA0003143454830000024
and
Figure FDA0003143454830000025
constant bias of the three-axis gyroscope in a carrier coordinate system b;
Figure FDA0003143454830000026
and
Figure FDA0003143454830000027
constant bias of the triaxial accelerometer under the b series;
under the initial alignment of a large misalignment angle, the nonlinear error state equation of the inertial/polarized light combined navigation system is as follows:
Figure FDA0003143454830000028
Figure FDA0003143454830000029
Figure FDA00031434548300000210
Figure FDA00031434548300000211
wherein the content of the first and second substances,
Figure FDA00031434548300000212
Figure FDA00031434548300000213
is an attitude transformation matrix between n and n' of the computational navigation system, expressed as:
Figure FDA00031434548300000214
fbb is the lower specific force obtained by an accelerometer; gnIs the local gravity vector;
Figure FDA00031434548300000215
an attitude transformation matrix of b and n';
Figure FDA00031434548300000216
the representation of the angular velocity of the system e of the earth relative to the system i of the inertia system under the system n is generated by the rotation of the earth;
Figure FDA00031434548300000217
as vectors
Figure FDA00031434548300000218
The asymmetric matrix form of (a) is:
Figure FDA00031434548300000219
wherein the content of the first and second substances,
Figure FDA0003143454830000031
and
Figure FDA0003143454830000032
is composed of
Figure FDA0003143454830000033
The three-axis component of (a); i is3×3Is a 3 × 3 dimensional identity matrix, 03×1Is a 3 × 1 dimensional all 0 vector;
Figure FDA0003143454830000034
and
Figure FDA0003143454830000035
respectively, the noise vectors of the gyroscope and the accelerometer, and the order state noise vector is
Figure FDA0003143454830000036
And obey Gaussian white noise distribution with mean value of 0 and variance of Q, namely Q to N (0)12×1,Q)，Q＝E[qqT]，01×6Is a 1 × 6 dimensional all-0 vector, 012×1Is a 12 x 1 dimensional all 0 vector;
in the step (2), the nonlinear measurement equation of the polarized light is established as follows:
Figure FDA0003143454830000037
wherein the content of the first and second substances,
Figure FDA0003143454830000038
to use a polarized light sensor m1The obtained measurement vector is measured by the measuring device,
Figure FDA0003143454830000039
for a system of polarized light sensor modules m1The attitude transformation relation between b system is the polarized light sensor m1The mounting relation with the carrier can be obtained by calibration before alignment; sm1Is the sun vector at m1The following expressions can be obtained by calculation of a polarized light sensor; snThe representation of the sun vector under the n system is obtained by calculation according to the astronomical calendar by using the local position and the time; delta Sm1Is Sm1The measurement error of (2);
Figure FDA00031434548300000310
for polarimetric measurement of noise vector, obey mean 0 and variance R1Of white Gaussian noise distribution, i.e. rpol～N(03×1,R1)，
Figure FDA00031434548300000311
03×1Is a 3 × 1 dimensional all 0 vector;
in the step (3), the velocity error measurement equation is established as follows:
δv＝[03×3 Hv 03×3 03×3]X+rv
wherein the content of the first and second substances,
Figure FDA00031434548300000312
outputting the calculated triaxial velocity value for the accelerometer of the inertial navigation system, 03×3Is a 3 x 3 dimensional full 0 matrix, rvFor the velocity error noise vector, obey mean 0 and variance R2Of white Gaussian noise distribution, i.e. rv～N(03×1,R2)，
Figure FDA00031434548300000313
In the step (4), the established unified inertial/polarized light combined navigation nonlinear measurement equation is as follows:
y＝h(X)+r
wherein the content of the first and second substances,
Figure FDA00031434548300000314
h (-) is the nonlinear measurement function of the inertial/polarized light integrated navigation system, and the specific form is as follows:
Figure FDA00031434548300000315
r is a measurement noise vector, and
Figure FDA0003143454830000041
06×1is a 6 x 1 dimensional all 0 vector;
in the step (5), based on the steps (1) and (4), a nonlinear state equation and a measurement equation of the inertial/polarized light integrated navigation system are established, and the established nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system are discretized to obtain a discrete state equation and a measurement equation of the inertial/polarized light integrated navigation system, wherein the established nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system are as follows:
Figure FDA0003143454830000042
y＝h(X)+r
wherein f (-) is the nonlinear state function of the inertial/polarized light combined navigation system, and the specific form is as follows:
Figure FDA0003143454830000043
q is a state noise vector obeying Gaussian white noise distribution with a mean value of 0 and a variance of Q, namely Q-N (0)12×1,Q)，Q＝E[qqT]，012×1Is a 12 x 1 dimensional all 0 vector;
discretizing the nonlinear state equation and the measurement equation of the inertial/polarized light integrated navigation system to obtain a discrete inertial/polarized light integrated navigation system state equation and a discrete measurement equation:
Figure FDA0003143454830000044
y(k)＝h(X(k))+r(k)
where k denotes the kth instant, at is the sampling time interval,
Figure FDA0003143454830000045
a first order Taylor series expansion at X (k-1) for the nonlinear state function f (·);
in the step (6), an extended kalman filter is designed according to the state equation and the measurement equation of the discrete inertial/polarized light integrated navigation system established in the step (5), the unknown state of the inertial/polarized light integrated navigation system is estimated, and the misalignment angle, the velocity error, the gyro drift and the accelerometer constant deviation of the inertial/polarized light integrated navigation system are obtained, wherein the designed extended kalman filter is as follows:
time updating:
one-step prediction equation of state:
Figure FDA0003143454830000051
wherein the content of the first and second substances,
Figure FDA0003143454830000052
is an estimated value of the time k-1,
Figure FDA0003143454830000053
a one-step predicted value at the moment k is obtained;
one-step prediction state error covariance equation:
P(k|k-1)＝Φ(k|k-1)P(k-1|k-1)ΦT(k|k-1)+Q(k-1)
wherein, P (k-1| k-1) is an error covariance matrix at the moment k-1, P (k | k-1) is a one-step prediction error covariance matrix of a state at the moment k, a state transition matrix phi (k | k-1) ═ I + A (X (k-1| k-1)) deltat, and Q (k-1) is a covariance matrix of state noise at the moment k-1;
measurement updating:
the state estimation equation:
Figure FDA0003143454830000054
wherein the content of the first and second substances,
Figure FDA0003143454830000055
for the optimal estimation of the state at the moment k, K (k) is a Kalman filtering gain matrix, and the calculation method is that
K(k)＝P(k|k-1)HT(k)(H(k)P(k|k-1)HT(k)+R(k))-1
Wherein the content of the first and second substances,
Figure FDA0003143454830000056
is a nonlinear measurement function h (-) in
Figure FDA0003143454830000057
(k) is a covariance matrix of the measured noise at time k;
estimation error covariance equation:
P(k|k)＝(I-K(k)H(k))P(k|k-1)(I-K(k)H(k))T+K(k)R(k)KT(k)
wherein I is an identity matrix with the same dimension as the covariance matrix P (k | k-1);
in the step (7), the feedback correction method is as follows:
(1) attitude correction method
First, the transformation matrix of n' and n systems is calculated
Figure FDA0003143454830000061
Wherein the content of the first and second substances,
Figure FDA0003143454830000062
and
Figure FDA0003143454830000063
for the state estimated misalignment angle, the corrected attitude matrix
Figure FDA0003143454830000064
Comprises the following steps:
Figure FDA0003143454830000065
(2) speed correction method
Setting the corrected speed of the triaxial carrier as Vx、VyAnd VzThen the velocity correction is expressed as:
Figure FDA0003143454830000066
wherein the content of the first and second substances,
Figure FDA0003143454830000067
and
Figure FDA0003143454830000068
respectively taking the three-axis speed values of the inertial navigation system under the n series;
Figure FDA0003143454830000069
and
Figure FDA00031434548300000610
is the three-axis velocity error of the state estimation under n.
CN201911131570.3A 2019-11-19 2019-11-19 EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle Active CN110672130B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201911131570.3A CN110672130B (en) 2019-11-19 2019-11-19 EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201911131570.3A CN110672130B (en) 2019-11-19 2019-11-19 EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle

Publications (2)

Publication Number Publication Date
CN110672130A CN110672130A (en) 2020-01-10
CN110672130B true CN110672130B (en) 2021-09-07

Family

ID=69087699

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201911131570.3A Active CN110672130B (en) 2019-11-19 2019-11-19 EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle

Country Status (1)

Country Link
CN (1) CN110672130B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111623771B (en) * 2020-06-08 2022-05-06 青岛智融领航科技有限公司 Polarization inertial navigation tight combination navigation method based on light intensity
CN113137977B (en) * 2021-04-21 2022-07-01 扬州大学 SINS/polarized light combined navigation initial alignment filtering method
CN114018258A (en) * 2021-11-05 2022-02-08 北京航空航天大学杭州创新研究院 Bionic combined navigation method based on polarization measurement noise variance adaptive estimation
CN113819907B (en) * 2021-11-22 2022-02-11 北京航空航天大学 Inertia/polarization navigation method based on polarization and sun dual-vector switching
CN113834484B (en) * 2021-11-26 2022-03-08 北京航空航天大学 Inertial navigation/polarization combined navigation method based on non-Rayleigh scattering model error
CN114964214B (en) * 2022-07-27 2022-11-15 立得空间信息技术股份有限公司 Extended Kalman filtering attitude calculation method of attitude heading reference system

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103217699A (en) * 2013-03-06 2013-07-24 郭雷 Integrated navigation system recursion optimizing initial-alignment method based on polarization information
JP2015524069A (en) * 2012-06-08 2015-08-20 ザ・ボード・オブ・トラスティーズ・オブ・ザ・リーランド・スタンフォード・ジュニア・ユニバーシティ Laser-driven optical gyroscope with push-pull modulation
CN105737823A (en) * 2016-02-01 2016-07-06 东南大学 GPS/SINS/CNS integrated navigation method based on five-order CKF
CN109556631A (en) * 2018-11-26 2019-04-02 北方工业大学 INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares
CN109556632A (en) * 2018-11-26 2019-04-02 北方工业大学 INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7783301B2 (en) * 2006-12-19 2010-08-24 The Boeing Company Method and device for determining a location of a communications device
CN103322856B (en) * 2013-03-06 2015-04-22 北京航空航天大学 Shooting attitude and micro-motion measuring system based on polarized light/MIMU (Micro Inertial Measurement Unit)
CN104133432B (en) * 2014-05-27 2016-08-24 北京航天万达高科技有限公司 A kind of nonopiate six bar communication in moving servosystem and control methods
CN104064869B (en) * 2014-06-13 2016-10-05 北京航天万达高科技有限公司 Biquaternion antenna for satellite communication in motion control method and system based on MEMS inertial navigation
CN108303081B (en) * 2017-12-29 2021-09-07 郭晓宇 Bionic polarization/inertia/atmospheric data combined navigation system

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2015524069A (en) * 2012-06-08 2015-08-20 ザ・ボード・オブ・トラスティーズ・オブ・ザ・リーランド・スタンフォード・ジュニア・ユニバーシティ Laser-driven optical gyroscope with push-pull modulation
CN103217699A (en) * 2013-03-06 2013-07-24 郭雷 Integrated navigation system recursion optimizing initial-alignment method based on polarization information
CN105737823A (en) * 2016-02-01 2016-07-06 东南大学 GPS/SINS/CNS integrated navigation method based on five-order CKF
CN109556631A (en) * 2018-11-26 2019-04-02 北方工业大学 INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares
CN109556632A (en) * 2018-11-26 2019-04-02 北方工业大学 INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
"SINS 大失准角初始对准的改进算法研究";杨磊等;《计算机仿真》;20120930;第29卷(第9期);57-60，108 *
"偏振光/地磁/GPS/SINS组合导航方法";卢鸿谦等;《宇航学报》;20070731;第28卷(第4期);897-902 *
"抗干扰滤波方法及在偏振组合导航系统的应用研究";杜涛;《中国博士学位论文全文数据库信息科技辑》;20170215(第2期);I136-300 *

Also Published As

Publication number Publication date
CN110672130A (en) 2020-01-10

Similar Documents

Publication Publication Date Title
CN110672131B (en) UKF (unscented Kalman Filter) alignment method for inertial/polarized light integrated navigation system under large misalignment angle
CN110672130B (en) EKF (extended Kalman filter) alignment method of inertial/polarized light integrated navigation system under large misalignment angle
CN109556632B (en) INS/GNSS/polarization/geomagnetic integrated navigation alignment method based on Kalman filtering
CN110487301B (en) Initial alignment method of radar-assisted airborne strapdown inertial navigation system
CN106289246B (en) A kind of flexible link arm measure method based on position and orientation measurement system
CN103217159B (en) A kind of SINS/GPS/ polarized light integrated navigation system modeling and initial alignment on moving base method
CN109556631B (en) INS/GNSS/polarization/geomagnetic combined navigation system alignment method based on least squares
CN103196445B (en) Based on the carrier posture measuring method of the earth magnetism supplementary inertial of matching technique
CN109506660B (en) Attitude optimization resolving method for bionic navigation
CN101571394A (en) Method for determining initial attitude of fiber strapdown inertial navigation system based on rotating mechanism
CN104374388A (en) Flight attitude determining method based on polarized light sensor
CN111102993A (en) Initial alignment method for shaking base of rotary modulation type strapdown inertial navigation system
CN103674030A (en) Dynamic measuring device and method for plumb line deviation kept on basis of astronomical attitude reference
Huang et al. Attitude estimation fusing quasi-Newton and cubature Kalman filtering for inertial navigation system aided with magnetic sensors
He et al. A combination orientation compass based on the information of polarized skylight/geomagnetic/MIMU
CN108151765B (en) Positioning and attitude measuring method for online real-time estimation and compensation of magnetometer error
CN111189442A (en) Multi-source navigation information state prediction method of unmanned aerial vehicle based on CEPF
Zhang et al. A multi-position calibration algorithm for inertial measurement units
Xiang et al. A SINS/GNSS/2D-LDV integrated navigation scheme for unmanned ground vehicles
Li et al. Airborne position and orientation system for aerial remote sensing
CN114646993A (en) Data fusion algorithm for accurate positioning based on GNSS, vision and IMU
Ben et al. DVL aided fine alignment for marine SINS
Vigrahala et al. Attitude, Position and Velocity determination using Low-cost Inertial Measurement Unit for Global Navigation Satellite System Outages
Zhang et al. An improved Kalman filter for attitude determination of multi-rotor UAVs based on low-cost MEMS sensors
Kurniawan et al. Displacement estimation and tracking of quadrotor UAV in dynamic motion

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant