CN108225373A - A kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman - Google Patents
A kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman Download PDFInfo
- Publication number
- CN108225373A CN108225373A CN201711401702.0A CN201711401702A CN108225373A CN 108225373 A CN108225373 A CN 108225373A CN 201711401702 A CN201711401702 A CN 201711401702A CN 108225373 A CN108225373 A CN 108225373A
- Authority
- CN
- China
- Prior art keywords
- moment
- covariance matrix
- misalignment angle
- improved
- value
- 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.)
- Granted
Links
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
Abstract
The invention discloses a kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman, this method is according to the measuring value z at k momentkThe recursion for calculating the k moment measures covariance matrix AkWith measurement covariance matrix Bk, and then obtain newly ceasing feedback factor αk, use αkUpdate the one-step prediction state covariance matrix P ' of next filtering cyclek|k‑1, so as to calculate the estimated value at k moment；It repeats the above steps, obtains the state estimation at each moment.This method introduces feedback supervision mechanism, and then improve the precision and stability of alignment under the premise of new breath feedback efficiency is improved.
Description
Technical field
The invention belongs to field of navigation technology, and in particular to a kind of alignment methods of misalignment.
Background technology
Initial alignment is the key technology of navigation and the premise of navigation calculation, and the accuracy of alignment determines to a certain extent
The precision of navigation.Initial alignment is generally divided into coarse alignment and fine alignment, and so-called coarse alignment is by analytic method, nonlinear filtering
The methods of large misalignment angle converged to low-angle, be then aligned again with compass, nonlinear filtering the methods of installation error, device
More accurately initial attitude angle is obtained after error and modeling equal error COMPREHENSIVE CALCULATING.
Nonlinear filtering is widely used as a kind of effective state estimation algorithm in initial alignment field.Hold
G-card Kalman Filtering (Cubature Kalman Filter, CKF) overcomes traditional extended Kalman filter (Extended
Kalman filter, EKF) and Unscented kalman (Unscented Kalman Filter, UKF) truncated error and hyper parameter
The shortcomings of, there is better numerical stability and filtering accuracy.Traditional CKF can accurately estimate preceding the two of third-order non-linear function
Rank square, higher exponent number then have truncated error.In order to improve precision of the algorithm under strong nonlinearity occasion, 5 rank CKF are sent out
Exhibition.In practical engineering application, Filtering Model, system noise and measurement noise often all cannot be modeled accurately, at this time
There is also filtering accuracy is low or the problem of filtering divergence by even 5 rank CKF.In order to improve the robustness of algorithm, need
It is improved on the basis of traditional CKF.When measurement equation is non-linear, can the feedback efficiency newly ceased be improved by iterative method；
When measurement equation is linear, iterative method failure.The measurement being initially aligned is often using rate error, site error as observed quantity
, measurement equation is linear at this time.In order to improve performance of the nonlinear filtering in initial alignment, can be calculated by introducing innovation
Method, for example new breath, new breath error and gain are improved, and then improve effect of the new breath for state estimation.But the feedback newly ceased
Intensity is needed to track, be supervised, and the mistake convergence or diverging of filtering may be caused when feedback noise is larger.There has been no effective at present
Supervision mechanism control the feedback newly ceased.
Invention content
Goal of the invention：For problems of the prior art, the present invention provides one kind to be based on improved 5 rank volume card
The large misalignment angle alignment methods of Germania, this method introduce feedback supervision mechanism under the premise of new breath feedback efficiency is improved, and then
Improve the precision and stability of alignment.
Technical solution：The present invention adopts the following technical scheme that：
A kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman, include the following steps：
(1) according to the measuring value z at k momentkThe recursion for calculating the k moment measures covariance matrix Ak, step is：
Calculate the new breath γ at k momentk：γk=zk-HPk|k-1；
Calculate the new breath covariance matrix Z at k momentk：
The recursion for calculating the k moment measures covariance matrix Ak：Ak=Zk-R；
Wherein H is is directed at measurement equation, Pk|k-1For the one-step prediction covariance matrix in current filter period, b for fade because
Son, R is measures noise matrix；T is the transposed operator of vector or matrix；
(2) the measurement covariance matrix B at k moment is calculatedkWith new breath feedback factor αk, update a step of next filtering cycle
Predicted state covariance matrix P 'k|k-1, calculating formula is：
B=HPk|k-1(H)T
P′k|k-1=αk·Pk|k-1
Wherein tr () is the mark operation for seeking matrix；
(3) estimated value at k moment is calculated；
(4) it repeats the above steps, obtains the state estimation at each moment.
It is preferred that further include following steps after the completion of step (4)：
Calculate the gradient at the azimuthal misalignment angle of each filtering cycle
Find out the filtering cycle value T where the maximum value of azimuthal misalignment angle gradientmax：
In t=1 ..., TmaxStep 1-4 is performed in filtering cycle to IMU data again to be filtered；From t=Tmax+
1,...,TendFiltering cycle is filtered using 5 rank CKF of tradition, and wherein diff () is gradient operator, and abs () is asks exhausted
To being worth operator, TendRepresent the last one filtering cycle.
The measuring value is velocity error or site error.
[0.5,1) value range of fading factor b is.
Step (3) specifically comprises the following steps：
Wherein KkFor filtering gain,For state, cross covariance battle array is measured,To measure covariance matrix, Pk|kFor state
Posteriority covariance matrix,For state posterior value,To measure estimated value, zkMeasuring value for the k moment.
Advantageous effect：Compared with prior art, the large misalignment angle disclosed by the invention based on improved 5 rank volume Kalman
The error of covariance of the measurement in short-term coefficient feedback of alignment methods combined innovation and iteration fading factor method to 5 rank CKF algorithms into
Row improves, and improves alignment precision and stability of the nonlinear filtering in the environment of the non-accurate modeling of model error.
Description of the drawings
Fig. 1 is the flow chart of large misalignment angle alignment methods in the embodiment of the present invention.
Specific embodiment
The invention discloses a kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman, with reference to attached
The present invention is further explained for figure.
As shown in Figure 1, a kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman, include the following steps：
(1) according to the measuring value z at k momentkThe recursion for calculating the k moment measures covariance matrix Ak；
Assuming that H is alignment measurement equation, Pk|k-1For the one-step prediction covariance matrix in current filter period, b for fade because
Son, the value range of b be generally [0.5,1), R is velocity error or site error to measure noise matrix, measuring value, is calculated such as
Under：
Calculate the new breath γ at k momentk：γk=zk-HPk|k-1；
Calculate the new breath covariance matrix Z at k momentk：
The recursion for calculating the k moment measures covariance matrix Ak：Ak=Zk-R；
Wherein T is the transposed operator of vector or matrix；
(2) the measurement covariance matrix B at k moment is calculatedkWith new breath feedback factor αk, update a step of next filtering cycle
Predicted state covariance matrix P 'k|k-1, calculating formula is：
B=HPk|k-1(H)T
P′k|k-1=αk·Pk|k-1
Wherein tr () is the mark operation for seeking matrix；
(3) estimated value at k moment is calculated；
Wherein KkFor filtering gain,For state, cross covariance battle array is measured,To measure covariance matrix, Pk|kFor state
Posteriority covariance matrix,For state posterior value,To measure estimated value, zkMeasuring value for the k moment.
(4) it repeats the above steps, obtains the state estimation at each moment, complete the improvement filtering of full-time feedback.
The above method is a kind of feedback filtering method in short-term, and the new breath of measurement, the side of making can be effectively utilized at alignment initial stage
Position large misalignment angle can restrain as soon as possible；It, may be due to mistake if being continuing with feedback filtering when misalignment converges to small angle
It feeds back and causes to vibrate or dissipate, feedback filtering is no longer necessary at this time.As a result, as an improvement scheme, in step
Suddenly following steps are further included after the completion of (4)：
Calculate the gradient at the azimuthal misalignment angle of each filtering cycle
Find out the filtering cycle value T where the maximum value of azimuthal misalignment angle gradientmax：
In t=1 ..., TmaxIn filtering cycle again to inertial measuring unit (Inertial Measurement Unit,
IMU) data perform step 1-4 and are filtered；From t=Tmax+1,...,TendFiltering cycle is filtered using 5 rank CKF of tradition,
Wherein diff () is gradient operator, and abs () is asks absolute value operators, TendRepresent the last one filtering cycle.
Claims (5)
1. a kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman, which is characterized in that include the following steps：
(1) according to the measuring value z at k momentkThe recursion for calculating the k moment measures covariance matrix Ak, step is：
Calculate the new breath γ at k momentk：γk=zk-HPk|k-1；
Calculate the new breath covariance matrix Z at k momentk：
The recursion for calculating the k moment measures covariance matrix Ak：Ak=Zk-R；
Wherein H is is directed at measurement equation, Pk|k-1For the one-step prediction covariance matrix in current filter period, b is fading factor, R
To measure noise matrix；T is the transposed operator of vector or matrix；
(2) the measurement covariance matrix B at k moment is calculatedkWith new breath feedback factor αk, update the one-step prediction of next filtering cycle
State covariance matrix Pk′|k-1, calculating formula is：
B=HPk|k-1(H)T
Pk′|k-1=αk·Pk|k-1
Wherein tr () is the mark operation for seeking matrix；
(3) estimated value at k moment is calculated；
(4) it repeats the above steps, obtains the state estimation at each moment.
2. the large misalignment angle alignment methods according to claim 1 based on improved 5 rank volume Kalman, feature exist
In further including following steps after the completion of the step (4)：
Calculate the gradient at the azimuthal misalignment angle of each filtering cycle
Find out the filtering cycle value T where the maximum value of azimuthal misalignment angle gradientmax：
In t=1 ..., TmaxStep 1-4 is performed in filtering cycle to IMU data again to be filtered；From t=Tmax+1,...,
TendFiltering cycle is filtered using 5 rank CKF of tradition, and wherein diff () is gradient operator, and abs () is asks absolute value to calculate
Son, TendRepresent the last one filtering cycle.
3. the large misalignment angle alignment methods according to claim 1 based on improved 5 rank volume Kalman, feature exist
In the measuring value is velocity error or site error.
4. the large misalignment angle alignment methods according to claim 1 based on improved 5 rank volume Kalman, feature exist
In, fading factor b value range for [0.5,1).
5. the large misalignment angle alignment methods according to claim 1 based on improved 5 rank volume Kalman, feature exist
In step (3) specifically comprises the following steps：
Wherein KkFor filtering gain,For state, cross covariance battle array is measured,To measure covariance matrix, Pk|kFor state posteriority
Covariance matrix,For state posterior value,To measure estimated value, zkMeasuring value for the k moment.
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 true CN108225373A (en) | 2018-06-29 |
| CN108225373B 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) |
Cited By (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 |
Citations (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 |
| CN103471616A (en) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | Initial alignment method of SINS (strapdown inertial navigation system) with moving base and at large azimuth misalignment angle |
| CN104655131A (en) * | 2015-02-06 | 2015-05-27 | 东南大学 | Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) |
| CN104777426A (en) * | 2015-04-17 | 2015-07-15 | 河海大学 | Power generator dynamic state estimation method based on unscented transformation strong tracking filtering |
| US20150317838A1 (en) * | 2014-05-02 | 2015-11-05 | Thales Visionix, Inc. | Registration for vehicular augmented reality using auto-harmonization |
| CN105737823A (en) * | 2016-02-01 | 2016-07-06 | 东南大学 | GPS/SINS/CNS integrated navigation method based on five-order CKF |
| CN106646356A (en) * | 2016-11-23 | 2017-05-10 | 西安电子科技大学 | Nonlinear system state 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 |
| CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
-
2017
- 2017-12-22 CN CN201711401702.0A patent/CN108225373B/en active Active
Patent Citations (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 |
| CN103471616A (en) * | 2013-09-04 | 2013-12-25 | 哈尔滨工程大学 | Initial alignment method of SINS (strapdown inertial navigation system) with moving base and at large azimuth misalignment angle |
| US20150317838A1 (en) * | 2014-05-02 | 2015-11-05 | Thales Visionix, Inc. | Registration for vehicular augmented reality using auto-harmonization |
| CN104655131A (en) * | 2015-02-06 | 2015-05-27 | 东南大学 | Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF) |
| CN104777426A (en) * | 2015-04-17 | 2015-07-15 | 河海大学 | Power generator dynamic state estimation method based on unscented transformation strong tracking filtering |
| CN105737823A (en) * | 2016-02-01 | 2016-07-06 | 东南大学 | GPS/SINS/CNS integrated navigation method based on five-order CKF |
| CN106646356A (en) * | 2016-11-23 | 2017-05-10 | 西安电子科技大学 | Nonlinear system state 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 |
| CN107063245A (en) * | 2017-04-19 | 2017-08-18 | 东南大学 | A kind of SINS/DVL integrated navigation filtering methods based on 5 rank SSRCKF |
Non-Patent Citations (2)
| Title |
|---|
| YONGGANG ZHANG 等: "SINS Initial Alignment Based on Fifth-degree Cubature Kalman Filter", 《PROCEEDINGS OF 2013 IEEE INTERNATIONAL CONFERENCE ON MECHATRONICS AND AUTOMATION》 * |
| 黄湘远 等: "5阶CKF在捷联惯导非线性对准中的应用研究", 《系统工程与电子技术》 * |
Cited By (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 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108225373B (en) | 2020-04-24 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109163720A (en) | Kalman filter tracking method based on fading memory exponent | |
| CN104567871B (en) | A kind of quaternary number Kalman filtering Attitude estimation method based on earth magnetism gradient tensor | |
| CN110441694B (en) | Lithium battery state-of-charge estimation method based on multiple fading factors Kalman filtering | |
| CN109459019A (en) | A kind of vehicle mounted guidance calculation method based on cascade adaptive robust federated filter | |
| CN102654404B (en) | Method for improving resolving precision and anti-jamming capability of attitude heading reference system | |
| CN108520107A (en) | System state estimation method based on maximum-likelihood criterion Robust Kalman Filter | |
| US10041736B2 (en) | Dynamic monitoring, diagnosis, and control of cooling tower systems | |
| CN104462015B (en) | Process the fractional order linear discrete system state updating method of non-gaussian L é vy noises | |
| CN103033186A (en) | High-precision integrated navigation positioning method for underwater glider | |
| US10370945B2 (en) | Method and apparatus for estimating down-hole process variables of gas lift system | |
| CN108761512A (en) | A kind of adaptive CKF filtering methods of missile-borne BDS/SINS deep combinations | |
| WO2017088328A1 (en) | Surface water depth information based ground irrigation control method | |
| CN104112079A (en) | Fuzzy adaptive variational Bayesian unscented Kalman filter method | |
| CN104075711B (en) | A kind of IMU/Wi Fi signal tight integration indoor navigation methods based on CKF | |
| Madoński et al. | Towards a turnkey solution of industrial control under the active disturbance rejection paradigm | |
| CN109737958A (en) | A kind of acoustics tests the speed the polar region grid inertial navigation error inhibition method of auxiliary | |
| CN103312297B (en) | A kind of iteration expansion increment kalman filter method | |
| CN108225373A (en) | A kind of large misalignment angle alignment methods based on improved 5 rank volume Kalman | |
| CN109840517A (en) | A kind of estimation of MEMS gyro noise and filtering method | |
| CN111190207A (en) | Unmanned aerial vehicle INS BDS integrated navigation method based on PSTCSDREF algorithm | |
| Knauth | Smartphone PDR positioning in large environments employing WiFi, particle filter, and backward optimization | |
| CN103968839B (en) | Single-point gravity matching method for improving CKF on basis of bee colony algorithm | |
| CN107421543B (en) | Implicit function measurement model filtering method based on state dimension expansion | |
| Bayat et al. | An augmented strapdown inertial navigation system using jerk and jounce of motion for a flying robot | |
| CN109211266A (en) | A kind of grid inertial navigation system synthesis correction method peculiar to vessel |
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 |