CN108225373B - Large misalignment angle alignment method based on improved 5-order cubature Kalman - Google Patents
Large misalignment angle alignment method based on improved 5-order cubature Kalman Download PDFInfo
- Publication number
- CN108225373B CN108225373B CN201711401702.0A CN201711401702A CN108225373B CN 108225373 B CN108225373 B CN 108225373B CN 201711401702 A CN201711401702 A CN 201711401702A CN 108225373 B CN108225373 B CN 108225373B
- Authority
- CN
- China
- Prior art keywords
- covariance matrix
- calculating
- order
- time
- measurement
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
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)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
The invention discloses a large misalignment angle alignment method based on an improved 5-order cubature Kalman, which is based on a measured value z at the moment kkRecursive measurement covariance matrix A for calculating k timekAnd the measured covariance matrix BkAnd then obtains an innovation feedback coefficient αkUsing αkUpdating one-step prediction state covariance matrix P 'of next filtering period'k|k‑1Thereby calculating an estimated value at the k moment; and repeating the steps to obtain the state estimation of each moment. According to the method, a feedback supervision mechanism is introduced on the premise of improving the innovation feedback efficiency, so that the alignment precision and stability are improved.
Description
Technical Field
The invention belongs to the technical field of navigation, and particularly relates to a misalignment angle alignment method.
Background
The initial alignment is a key technology of navigation and a precondition of navigation solution, and the accuracy of the alignment determines the accuracy of the navigation to a certain extent. The initial alignment is generally divided into coarse alignment and fine alignment, wherein the coarse alignment is to converge a large misalignment angle to a small angle by an analytical method, a nonlinear filtering method and other methods, and then to comprehensively calculate installation errors, device errors, modeling errors and other errors by a compass alignment method, a nonlinear filtering method and other methods to obtain a more accurate initial attitude angle.
Nonlinear filtering has been widely used in the field of initial alignment as an effective state estimation algorithm. The Cubature Kalman Filter (CKF) overcomes the shortcomings of truncation error and over-parameter of the traditional Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF), and has better numerical stability and filtering precision. The traditional CKF can accurately estimate the first two moments of a third-order nonlinear function, and the higher orders have truncation errors. In order to improve the accuracy of the algorithm in strong non-linear situations, 5 th order CKF is developed. In practical engineering application, a filtering model, system noise and measurement noise cannot be accurately modeled, and even 5-order CKF has the problems of low filtering precision or filtering divergence. In order to improve the robustness of the algorithm, improvement is needed on the basis of the traditional CKF. When the measurement equation is nonlinear, the feedback efficiency of innovation can be improved by an iteration method; when the equivalence measurement equation is linear, the iterative method fails. The measurement of the initial alignment is usually observed as a rate error and a position error, and the measurement equation is linear at this time. To improve the performance of the non-linear filtering in the initial alignment, the innovation on the state estimation can be improved by introducing innovative algorithms, such as improving innovation, innovation error and gain. However, the intensity of the newly-inspired feedback needs to be tracked and supervised, and when the feedback noise is large, the wrong convergence or divergence of the filtering can be caused. Currently there is no effective supervision mechanism to control the feedback of information.
Disclosure of Invention
The purpose of the invention is as follows: aiming at the problems in the prior art, the invention provides a large misalignment angle alignment method based on an improved 5-order cubature Kalman, and the method introduces a feedback supervision mechanism on the premise of improving the innovation feedback efficiency so as to improve the alignment precision and stability.
The technical scheme is as follows: the invention adopts the following technical scheme:
a large misalignment angle alignment method based on improved 5 th order cubature Kalman comprises the following steps:
(1) according to the measured value z at time kkCalculating a recursive measurement covariance matrix A at time kkThe method comprises the following steps:
calculating innovation gamma at time kk：γk＝zk-HPk|k-1；
Calculating an innovation covariance matrix Z at time kk：
Calculating a recursive measurement covariance matrix A at time kk：Ak＝Zk-R；
Wherein H is the alignment measurement equation, Pk|k-1Predicting a covariance matrix for one step of the current filtering period, wherein b is an fading factor, and R is a measurement noise matrix; t is a transpose operator of a vector or a matrix;
(2) calculating a measurement covariance matrix B at time kkAnd innovation feedback coefficient αkUpdating one-step prediction state covariance matrix P 'of next filtering period'k|k-1The calculation formula is:
B＝HPk|k-1(H)T
P′k|k-1＝αk·Pk|k-1
wherein tr (-) is the trace operation of the matrix;
(3) calculating an estimated value at the k moment;
(4) and repeating the steps to obtain the state estimation of each moment.
Preferably, the method further comprises the following steps after the step (4) is completed:
calculating a gradient of azimuthal misalignment angle for each filter period
Finding the value of the filter period T at which the maximum value of the azimuthal misalignment angular gradient liesmax：
At T1maxExecuting the steps 1-4 to the IMU data again in the filtering period for filtering; from T to Tmax+1,...,TendThe filtering period uses the traditional 5-order CKF to carry out filtering, wherein diff (·) is a gradient operator, abs (·) is an absolute value calculation operator, and TendRepresenting the last filtering period.
The measurement value is a speed error or a position error.
The value range of the fading factor b is [0.5,1 ].
The step (3) specifically comprises the following steps:
wherein KkIn order to filter the gain of the filter,
as a state, measure inter-covariance matrix,
for measuring covariance matrix, Pk|kIs a state posterior covariance matrix,
in order to be a state a-posteriori value,
for measurement of the estimated value, zkMeasured values at time k.
Has the advantages that: compared with the prior art, the 5-order CKF algorithm is improved by combining the improved 5-order cubature Kalman-based large misalignment angle alignment method with the innovative short-time measurement covariance error coefficient feedback and the iterative fading factor method, and the alignment precision and stability of the nonlinear filter in the environment of inaccurate modeling of model errors are improved.
Drawings
FIG. 1 is a flow chart of a large misalignment angle alignment method according to an embodiment of the present invention.
Detailed Description
The invention discloses a large misalignment angle alignment method based on an improved 5-order cubature Kalman, which is further explained by combining the accompanying drawing.
As shown in fig. 1, a large misalignment angle alignment method based on improved 5 th order cubature kalman includes the following steps:
(1) according to the measured value z at time kkCalculating a recursive measurement covariance matrix A at time kk；
Let H be the alignment measurement equation, Pk|k-1For one-step prediction covariance matrix of the current filtering period, b is an extinction factor, the value range of b is generally [0.5,1 ], R is a measurement noise matrix, and the measurement value is a speed error or a position error, which is calculated as follows:
calculating innovation gamma at time kk：γk＝zk-HPk|k-1；
Calculating an innovation covariance matrix Z at time kk：
Calculating a recursive measurement covariance matrix A at time kk：Ak＝Zk-R；
Wherein T is a transpose operator of a vector or a matrix;
(2) calculating a measurement covariance matrix B at time kkAnd innovation feedback coefficient αkUpdating one-step prediction state covariance matrix P 'of next filtering period'k|k-1The calculation formula is:
B＝HPk|k-1(H)T
P′k|k-1＝αk·Pk|k-1
wherein tr (-) is the trace operation of the matrix;
(3) calculating an estimated value at the k moment;
wherein KkIn order to filter the gain of the filter,
as a state, measure inter-covariance matrix,
for measuring covariance matrix, Pk|kIs a state posterior covariance matrix,
in order to be a state a-posteriori value,
for measurement of the estimated value, zkMeasured values at time k.
(4) And repeating the steps to obtain the state estimation of each moment, and finishing the improved filtering of full-time feedback.
The method is a short-time feedback filtering method, and can effectively utilize measurement information at the initial stage of alignment, so that a large azimuth misalignment angle can be converged as soon as possible; if feedback filtering is continued to be used until the misalignment angle converges to a small angle, oscillation or divergence due to overfeedback may occur, at which time feedback filtering is no longer necessary. Therefore, as an improved scheme, the method further comprises the following steps after the step (4) is completed:
calculating a gradient of azimuthal misalignment angle for each filter period
Finding the value of the filter period T at which the maximum value of the azimuthal misalignment angular gradient liesmax：
At T1maxFiltering the Inertial Measurement Unit (IMU) data in the filtering period by executing the steps 1-4; from T to Tmax+1,...,TendThe filtering period uses the traditional 5-order CKF to carry out filtering, wherein diff (·) is a gradient operator, abs (·) is an absolute value calculation operator, and TendRepresenting the last filtering period.
Claims (4)
1. A large misalignment angle alignment method based on improved 5 th order cubature Kalman is characterized by comprising the following steps:
(1) according to the measured value z at time kkCalculating a recursive measurement covariance matrix A at time kkThe method comprises the following steps:
calculating innovation gamma at time kk：γk＝zk-HPk|k-1；
Calculating an innovation covariance matrix Z at time kk：
Calculating a recursive measurement covariance matrix A at time kk：Ak＝Zk-R；
Wherein H is the alignment measurement equation, Pk|k-1Predicting a covariance matrix for one step of the current filtering period, wherein b is an fading factor, and R is a measurement noise matrix; t is a transpose operator of a vector or a matrix;
(2) calculating a measurement covariance matrix B at time kkAnd innovation feedback coefficient αkUpdating one-step prediction state covariance matrix P 'of next filtering period'k|k-1The calculation formula is:
B＝HPk|k-1(H)T
P′k|k-1＝αk·Pk|k-1
wherein tr (-) is the trace operation of the matrix;
(3) calculating a state posterior estimate at time k
Wherein KkIn order to filter the gain of the filter,
as a state, measure inter-covariance matrix,
measuring covariance matrix;
for measurement of the estimated value, zkIs the measured value at the k moment;
(4) and repeating the steps to obtain the state estimation of each moment.
2. The improved 5 th order volumetric Kalman based large misalignment angle alignment method of claim 1, further comprising the following steps after step (4) is completed:
calculating a gradient of azimuthal misalignment angle for each filter period
Finding the value of the filter period T at which the maximum value of the azimuthal misalignment angular gradient liesmax：
At T1maxExecuting the steps 1-4 to the IMU data again in the filtering period for filtering; from T to Tmax+1,...,TendThe filtering period uses the traditional 5-order CKF to carry out filtering, wherein diff (·) is a gradient operator, abs (·) is an absolute value calculation operator, and TendRepresenting the last filtering period.
3. The improved 5 th order volumetric kalman based large misalignment angle alignment method of claim 1, wherein the measurement value is a velocity error or a position error.
4. The improved 5 th order cubature kalman based large misalignment angle alignment method according to claim 1, characterized in that the value range of the extinction factor b is [0.5,1 ].
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711401702.0A CN108225373B (en) | 2017-12-22 | 2017-12-22 | Large misalignment angle alignment method based on improved 5-order cubature Kalman |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711401702.0A CN108225373B (en) | 2017-12-22 | 2017-12-22 | Large misalignment angle alignment method based on improved 5-order cubature Kalman |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108225373A CN108225373A (en) | 2018-06-29 |
| CN108225373B true CN108225373B (en) | 2020-04-24 |
Family
ID=62648570
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711401702.0A Active CN108225373B (en) | 2017-12-22 | 2017-12-22 | Large misalignment angle alignment method based on improved 5-order cubature Kalman |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108225373B (en) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112859004A (en) * | 2021-01-04 | 2021-05-28 | 南京理工大学 | Outlier rejection method based on improved Kalman filtering |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101915579A (en) * | 2010-07-15 | 2010-12-15 | 哈尔滨工程大学 | Novel CKF(Crankshaft Fluctuation Sensor)-based SINS (Ship Inertial Navigation System) large misalignment angle initially-aligning method |
| CN103471616B (en) * | 2013-09-04 | 2016-01-27 | 哈尔滨工程大学 | Initial Alignment Method under a kind of moving base SINS Large azimuth angle condition |
| US9766075B2 (en) * | 2014-05-02 | 2017-09-19 | Thales Visionix, Inc. | Registration for vehicular augmented reality using auto-harmonization |
| CN104655131B (en) * | 2015-02-06 | 2017-07-18 | 东南大学 | Inertial navigation Initial Alignment Method based on ISTSSRCKF |
| CN104777426B (en) * | 2015-04-17 | 2017-11-24 | 河海大学 | A kind of generator dynamic state estimator method based on Unscented transform strong tracking |
| CN105737823B (en) * | 2016-02-01 | 2018-09-21 | 东南大学 | A kind of GPS/SINS/CNS Combinated navigation methods based on five rank CKF |
| CN106646356B (en) * | 2016-11-23 | 2019-07-26 | 西安电子科技大学 | A kind of non-linear system status estimation method based on Kalman filtering positioning |
| CN106874935A (en) * | 2017-01-16 | 2017-06-20 | 衢州学院 | SVMs parameter selection method based on the fusion of multi-kernel function self adaptation |
| CN107063245B (en) * | 2017-04-19 | 2020-12-25 | 东南大学 | SINS/DVL combined navigation filtering method based on 5-order SSRCKF |
-
2017
- 2017-12-22 CN CN201711401702.0A patent/CN108225373B/en active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN108225373A (en) | 2018-06-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2018014602A1 (en) | Volume kalman filtering method suitable for high-dimensional gnss/ins deep coupling | |
| CN109163720A (en) | Kalman filter tracking method based on fading memory exponent | |
| WO2020087845A1 (en) | Initial alignment method for sins based on gpr and improved srckf | |
| CN104567871B (en) | A kind of quaternary number Kalman filtering Attitude estimation method based on earth magnetism gradient tensor | |
| CN104020480B (en) | A kind of satellite navigation method of the interactive multi-model UKF with adaptive factor | |
| CN105136145A (en) | Kalman filtering based quadrotor unmanned aerial vehicle attitude data fusion method | |
| CN108981696B (en) | Sins random misalignment angle non-singular rapid transfer alignment method | |
| CN104121907A (en) | Square root cubature Kalman filter-based aircraft attitude estimation method | |
| CN105043388A (en) | Vector search iterative matching method based on inertia/gravity matching integrated navigation | |
| CN109945895B (en) | Inertial navigation initial alignment method based on fading smooth variable structure filtering | |
| CN108761512A (en) | A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations | |
| CN112798021B (en) | Inertial navigation system inter-travelling initial alignment method based on laser Doppler velocimeter | |
| CN103604430A (en) | Marginalized cubature Kalman filter (CKF)-based gravity aided navigation method | |
| Zhao et al. | Adaptive two-stage Kalman filter for SINS/odometer integrated navigation systems | |
| CN113203429B (en) | Online estimation and compensation method for temperature drift error of gyroscope | |
| CN103776449A (en) | Moving base initial alignment method for improving robustness | |
| CN109840517A (en) | A kind of estimation of MEMS gyro noise and filtering method | |
| CN111190207B (en) | PSTCSDREF algorithm-based unmanned aerial vehicle INS BDS integrated navigation method | |
| CN103312297B (en) | A kind of iteration expansion increment kalman filter method | |
| CN108225373B (en) | Large misalignment angle alignment method based on improved 5-order cubature Kalman | |
| CN108731702B (en) | Large misalignment angle transfer alignment method based on Huber method | |
| CN103968839A (en) | Single-point gravity matching method for improving CKF on basis of bee colony algorithm | |
| CN113432608B (en) | Generalized high-order CKF method based on maximum correlation entropy for INS/CNS integrated navigation system | |
| CN110912535A (en) | Novel pilot-free Kalman filtering method | |
| CN112649022B (en) | Large misalignment angle transfer alignment method considering flexural deformation and lever arm effect |
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 |