CN107421543A - A kind of implicit function measurement model filtering method being augmented based on state - Google Patents

A kind of implicit function measurement model filtering method being augmented based on state Download PDF

Info

Publication number
CN107421543A
CN107421543A CN201710478805.0A CN201710478805A CN107421543A CN 107421543 A CN107421543 A CN 107421543A CN 201710478805 A CN201710478805 A CN 201710478805A CN 107421543 A CN107421543 A CN 107421543A
Authority
CN
China
Prior art keywords
mrow
msubsup
msub
mtd
mtr
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
Application number
CN201710478805.0A
Other languages
Chinese (zh)
Other versions
CN107421543B (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.)
Beihang University
Original Assignee
Beihang University
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 Beihang University filed Critical Beihang University
Priority to CN201710478805.0A priority Critical patent/CN107421543B/en
Publication of CN107421543A publication Critical patent/CN107421543A/en
Application granted granted Critical
Publication of CN107421543B publication Critical patent/CN107421543B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Indication And Recording Devices For Special Purposes And Tariff Metering Devices (AREA)
  • Length Measuring Devices With Unspecified Measuring Means (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)

Abstract

The present invention relates to a kind of implicit function measurement model filtering method being augmented based on state, IAUKF.In this method, measurement is extended in quantity of state, while null vector is considered as equivalent measurement and updated to be filtered.IAUKF obtains preferably estimation performance compared to IAEKF and IEKF.Particularly when measuring noise increase, compared to implicit UKF, performance can be very significantly improved.

Description

Implicit function measurement model filtering method based on state dimension expansion
Technical Field
The invention belongs to the field of autonomous navigation of spacecrafts, and relates to a filtering method of an implicit function measurement model of state dimension expansion.
Background
Kalman in 1960 proposed a linear optimal recursive filtering method, Kalman Filter (KF). Initially, KF is only applicable to linear systems, and as the demand for filtering nonlinear systems expands, filtering methods such as Extended Kalman Filter (EKF), Unscented Kalman Filter (uvf), Particle Filter (PF), etc. are gradually proposed and continuously developed. The measurement models in the classical kalman filtering algorithm all have explicit expressions, however, in many practical problems, constraints of state quantity and measurement quantity are often implicit, and explicit measurement models are not easy or cannot be obtained, which is an implicit measurement model filtering problem.
There are two main types of methods for solving the state estimation problem including the implicit measurement model at home and abroad. The first type is the IEKF proposed by sotto et al, obtained by linearizing an Implicit measurement equation at a reference point and taking a second order form, in combination with the conventional EKF algorithm with an explicit measurement equation, through an Implicit Extended Kalman Filter (IEKF). The second type is a filtering method of an implicit measurement model containing Iterative measurement updating, namely Iterative IEKF, which is provided by Steffen on the basis of analyzing the IEKF method. Both the methods are established on the basis of EKF, a Jacobian matrix needs to be calculated during application, linearization errors can reduce the precision of a filtering algorithm, divergence of filtering results can be caused, and the calculation of the Jacobian matrix is usually complex.
Disclosure of Invention
The technical problem to be solved by the invention is as follows: the method overcomes the defects of the prior art, provides a hidden function measurement model filtering method based on state dimension expansion, and obtains better estimation performance compared with IAEKF and IEKF. Especially when the measurement noise increases, the estimation performance can be greatly improved compared with the implicit UKF.
The invention provides a hidden function measurement model filtering method based on state dimension expansion, namely IAUKF. In the method, the quantity measurement is extended into the state quantity, and meanwhile, the zero vector is regarded as the equivalent quantity measurement to perform filtering updating, and the method specifically comprises the following steps:
first, the true quantity of k time is measuredExtending to state quantity XkIn the structure of the expanded state quantityAndestablishing a system model meeting the dimension expansion state quantity;
and secondly, initializing, solving the state quantity of the initial moment according to the system model, and substituting the state quantity of the initial moment and the covariance matrix thereof into the expanded state quantity constructed in the first stepAndin each case are denoted byAnd
thirdly, time updating, namely calculating the predicted state according to the expanded state quantity of the initial time and the covariance matrix obtained in the second stepAnd error covariance matrix thereofAt the rest of the time, in the state of calculating the predictionAnd error covariance matrix thereofThen, the state quantity obtained in the fourth step is updated according to the measurementAnd corresponding error covariance matrixTo obtain;
fourthly, measuring and updating, and solving the updated state estimation according to the state prediction value obtained by time updating and solvingAnd corresponding error covariance matrixAnd then returning to the third step, and realizing loop solution until the filtering is finished.
The first step is to construct an expanded state quantity, and establish a system model meeting the dimension expansion state quantity as follows:
wherein, XkAnd ZkRespectively the state quantity and the quantity measurement at the time k,the state quantity after the expansion is represented,indicating measurement of Z from actual quantitykAnd the measurement noise vkMeasuring the true quantity obtained by mixing;
in the formula, Fa(. a) anda state model representing the extended state quantities and their state errors, respectively, h (-) is a non-linear explicit function.
In the second step, the state quantity after the dimension expansionAnd covariance matrix thereofThe initialization is as follows:
wherein,E[x]denotes the expected value of x, Z1And N1And n and m respectively represent the dimensionality of the state vector and the measurement vector.
In the third step, the time is updated as follows:
in the formula,is satisfied with a mean value ofCovariance ofPoint of (a), wiIs the weight of the ith Sigma point,is composed ofThe covariance matrix of (a), wherein,
in the formula, naA dimension representing the state quantity after expansion, tau is a scaling parameter,represents the square root of the matrixThe ith dimensional column vector of (1).
In the fourth step, the measurement is updated as follows:
wherein,for filtering the gain matrix, Ykk-1In order to measure the predicted value of the measurement, respectively corresponding error covariance matrices.
Compared with the prior art, the invention has the advantages that:
(1) according to the invention, the state quantity and the real quantity measurement are expanded into a new state quantity, and meanwhile, the zero vector is regarded as an equivalent quantity measurement to carry out filtering updating, so that better estimation performance is obtained compared with IAEKF and IEKF, and the estimation precision is improved; especially when the measurement noise increases, the performance can be greatly improved compared with the implicit UKF.
(2) The method omits the process of solving the Jacobian matrix in the existing method, and reduces the calculated amount.
Drawings
FIG. 1 is a flow chart of a filtering method of a hidden function measurement model based on state dimension expansion according to the present invention;
Detailed Description
Fig. 1 shows a flow chart of a filtering method of an implicit function measurement model based on state dimension expansion. The following detailed description of the embodiments of the present invention:
common nonlinear systems all contain explicit metrology models, and such systems can be described as:
in the formula, the state equation f (-) and the measurement equation h (-) are both non-linear explicit functions. XkAnd wkRespectively representing the state vector at time k and its noise, Xk+1Representing the state vector at time k + 1. ZkAnd vkRespectively representing the measurement vector at time k and its noise. In practical application, it can be considered that both the state vector and the measurement vector are affected by zero-mean and uncorrelated white gaussian noise, that is, the state quantity noise and the measurement noise respectively obey:
wherein Q iskAnd NkRespectively representing the covariance matrix corresponding to the state noise and the measurement noise, and the specific value is determined by engineering experience or system parameters.
However, in many practical problems, the state quantity and the actual quantity measurement are constrained in a measurement model in an implicit function form, and an explicit measurement equation is not easy or available, and such problems are filtering problems of the implicit measurement model. Such implicit metrology models can be described as a system as follows:
wherein the state quantity XkSum quantity measurement ZkThe constituent implicit function h (·) ═ 0 constraint.
First, to deal with the nonlinear system problem with implicit metrology models, the state quantities and true quantity measurements can be expanded into a new state quantity:
in the formula, the superscript a marks the state extension,indicating measurement of Z from actual quantitykAnd the measurement noise vkThe actual amount of measurements made by the mixing is measured,indicating the expanded state quantities.
Second, note h (X) in System (3)k,Zk+vk) Zero vector equal to m dimensions, so the zero vector can be regarded as an equivalent quantity measurement YkNamely:
due to expanded state quantityIs derived from the previous state quantity XkAnd true quantity measurementThe structure is that the measurement model (5) can be rewritten as:
therefore, the system model for establishing the dimension expansion state quantity satisfaction is as follows:
in the formula, Fa(. a) andstate model representing the extended state quantities and its state errors, Fa(. a) andthe calculation formula of (a) is as follows:
wherein,the covariance matrix of (A) is defined as
The IAUKF method comprises the following concrete implementation steps:
1. initialization
Initial state estimation and corresponding error covariance matrixAnd P0Respectively setting as follows:
in the formula, E [ x ]]Expected value, state error covariance matrix Q representing xkCovariance matrix N with measurement errorskThe corresponding values at the initial time are selected respectively.
Initial extended state quantityShould be constructed according to equation (4), however, in practical applications, the true quantity is measuredNot available, the present invention measures Z with an actual quantity1Measurement in place of a true quantityAt the moment, a noise covariance matrix is introduced as N1Thereby, the state quantity after dimension expansionAnd covariance matrix thereofMay be respectively configured as:
in the formula, Z1And N1And n and m respectively represent the dimensionality of the state vector and the measurement vector.
2. Time updating
At time k, the estimation value of the expansion state quantity obtained at the last time is needed firstThe correction is performed according to equation (4). However, in practical applications, the true quantity is measuredNot available, we measured Z with the actual quantitykMeasurement in place of a true quantityAt this time, a measurement noise v is introducedkAnd its noise covariance matrix Nk. Therefore, the dimension expansion state quantityAnd covariance matrix thereofThe correction can be made as follows:
in the formula，And Pk-1Are respectively shown to be included inAndthe unexpanded state quantity estimate in (1) and its error covariance matrix.
Likewise, the IAUKF method performs probability deduction based on UT transforms. Satisfy the mean value ofCovariance of2n ofa+1 Sigma points are equivalent toThe Sigma points are propagated through the system model (7) to obtain corresponding propagated Sigma points, and the propagated Sigma points can be used for calculating the predicted stateAnd error covariance matrix thereofThis particular set of Sigma is determined according to the following formula:
in the formula, naA dimension representing the state quantity after expansion, numerically equal to n + m, τ being a scaling parameter,represents the square root of the matrixOf the ith-dimensional column vector, wiIs the weight of the ith Sigma point.
The Sigma point-by-state model passes as:
the state prediction value and its error covariance matrix can be calculated as follows:
3. measurement update
According to equation (6), the Sigma point of the estimator measurement can be calculated according to the following equation:
since the predicted value of Sigma point obtained according to equation (16)Not the true state at time k, contains errors. Therefore, equation (18) is also not equal to its true value of 0, which provides information about the predicted stateCan be used for state correction.
Thus, the predicted value of the measurement can be calculated as:
the corresponding error covariance matrix can be obtained by:
then, the gain matrix is filteredUpdated state estimationAnd corresponding error covariance matrixCan be obtained by calculation according to a UKF method respectively:
table 1 and Table 2 show the comparison of the navigation results of the IAUKF, the IEKF, the IAEKF, and the implicit UKF.
TABLE 1 navigation results of four filtering methods
TABLE 2 navigation results of four filtering methods under different measurement noises
Table 1 compares the filtering results of the four filtering methods when the equivalent measurement noise is relatively small and is 1'; table 2 compares the filtering results of the four filtering methods with different measured noise. It can be seen that: when the measurement noise is small, compared with the IAEKF and the IEKF, the estimation precision of the position of the IAUKF is greatly improved; when the measurement noise is gradually increased, the estimation performance of the IAUKF can be greatly improved compared with the implicit UKF.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.

Claims (5)

1. A hidden function measurement model filtering method based on state dimension expansion is characterized by comprising the following steps:
first, the true quantity of k time is measuredExtending to state quantity XkIn the structure of the expanded state quantityAndestablishing a system model meeting the dimension expansion state quantity;
and secondly, initializing, solving the state quantity of the initial moment according to the system model, and substituting the state quantity of the initial moment and the covariance matrix thereof into the expanded state quantity constructed in the first stepAndin each case are denoted byAnd
thirdly, time updating, namely calculating the predicted state according to the expanded state quantity of the initial time and the covariance matrix obtained in the second stepAnd error covariance matrix thereofAt the rest of the time, in the state of calculating the predictionAnd error covariance matrix thereofThen, the state quantity obtained in the fourth step is updated according to the measurementAnd corresponding errorCovariance matrixTo obtain;
fourthly, measuring and updating, and solving the updated state estimation according to the state prediction value obtained by time updating and solvingAnd corresponding error covariance matrixAnd then returning to the third step, and realizing loop solution until the filtering is finished.
2. The method of claim 1, wherein the filtering method comprises: the first step is to construct an expanded state quantity, and establish a system model meeting the dimension expansion state quantity as follows:
<mrow> <msubsup> <mi>X</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mover> <mi>Z</mi> <mo>&amp;OverBar;</mo> </mover> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
wherein, XkAnd ZkRespectively the state quantity and the quantity measurement at the time k,the state quantity after the expansion is represented,indicating measurement of Z from actual quantitykAnd the measurement noise vkMeasuring the true quantity obtained by mixing;
<mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>X</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <msup> <mi>F</mi> <mi>a</mi> </msup> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>)</mo> </mrow> <mo>+</mo> <msubsup> <mi>w</mi> <mi>k</mi> <mi>a</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>0</mn> <mo>=</mo> <mi>h</mi> <mrow> <mo>(</mo> <msubsup> <mi>X</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> </mtable> </mfenced>
in the formula, Fa(. a) anda state model representing the extended state quantities and their state errors, respectively, h (-) is a non-linear explicit function.
3. The state-based extended dimensional implicit function measurement model of claim 1A filtering method, characterized by: in the second step, the state quantity after the dimension expansionAnd covariance matrix thereofThe initialization is as follows:
<mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> <mi>a</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>Z</mi> <mn>1</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msubsup> <mi>P</mi> <mn>0</mn> <mi>a</mi> </msubsup> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>P</mi> <mn>0</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mi>n</mi> <mo>&amp;times;</mo> <mi>m</mi> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mn>0</mn> <mrow> <mi>m</mi> <mo>&amp;times;</mo> <mi>n</mi> </mrow> </msub> </mtd> <mtd> <msub> <mi>N</mi> <mn>1</mn> </msub> </mtd> </mtr> </mtable> </mfenced> </mrow>
wherein,E[x]denotes the expected value of x, Z1And N1And n and m respectively represent the dimensionality of the state vector and the measurement vector.
4. The method of claim 1, wherein the filtering method comprises: in the third step, the time is updated as follows:
<mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msub> <mi>w</mi> <mi>i</mi> </msub> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> </mrow>1
<mrow> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <munderover> <mi>&amp;Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>0</mn> </mrow> <mrow> <mn>2</mn> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow> </munderover> <msub> <mi>w</mi> <mi>i</mi> </msub> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>&amp;rsqb;</mo> <msup> <mrow> <mo>&amp;lsqb;</mo> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>-</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>&amp;rsqb;</mo> </mrow> <mi>T</mi> </msup> <mo>+</mo> <msubsup> <mi>Q</mi> <mi>k</mi> <mi>a</mi> </msubsup> </mrow>
in the formula,is satisfied with a mean value ofCovariance ofPoint of (a), wiIs the weight of the ith Sigma point,is composed ofThe covariance matrix of (a), wherein,
<mrow> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>w</mi> <mn>0</mn> </msub> <mo>=</mo> <mi>&amp;tau;</mi> <mo>/</mo> <mrow> <mo>(</mo> <mrow> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;tau;</mi> </mrow> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>+</mo> <msqrt> <mrow> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;tau;</mi> </mrow> </msqrt> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>w</mi> <mi>i</mi> </msub> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;tau;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>&amp;chi;</mi> <mrow> <mi>i</mi> <mo>+</mo> <mi>n</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>-</mo> <msqrt> <mrow> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;tau;</mi> </mrow> </msqrt> <msub> <mrow> <mo>(</mo> <msqrt> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> </msqrt> <mo>)</mo> </mrow> <mi>i</mi> </msub> <mo>,</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>w</mi> <mrow> <mi>i</mi> <mo>+</mo> <mi>n</mi> </mrow> </msub> <mo>=</mo> <mn>1</mn> <mo>/</mo> <mrow> <mo>&amp;lsqb;</mo> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <mrow> <msub> <mi>n</mi> <mi>a</mi> </msub> <mo>+</mo> <mi>&amp;tau;</mi> </mrow> <mo>)</mo> </mrow> </mrow> <mo>&amp;rsqb;</mo> </mrow> </mrow> </mtd> </mtr> </mtable> <mo>,</mo> <mi>i</mi> <mo>=</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>,</mo> <mn>...</mn> <mo>,</mo> <msub> <mi>n</mi> <mi>a</mi> </msub> </mrow>
in the formula, naA dimension representing the state quantity after expansion, tau is a scaling parameter,represents the square root of the matrixThe ith dimensional column vector of (1).
5. The method of claim 1, wherein the filtering method comprises: in the fourth step, the measurement is updated as follows:
<mrow> <msubsup> <mi>K</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>x</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>a</mi> </msubsup> <msup> <msubsup> <mi>P</mi> <mrow> <mi>y</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>a</mi> </msubsup> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msup> </mrow>
<mrow> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>a</mi> </msubsup> <msub> <mi>Y</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mrow>
<mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>=</mo> <msubsup> <mi>P</mi> <mrow> <mi>k</mi> <mo>|</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>a</mi> </msubsup> <mo>-</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>a</mi> </msubsup> <msubsup> <mi>P</mi> <mrow> <mi>y</mi> <mi>y</mi> <mo>,</mo> <mi>k</mi> </mrow> <mi>a</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msubsup> <mi>K</mi> <mi>k</mi> <mi>a</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> </mrow>
wherein,for filtering the gain matrix, Yk|k-1In order to measure the predicted value of the measurement, respectively corresponding error covariance matrices.
CN201710478805.0A 2017-06-22 2017-06-22 Implicit function measurement model filtering method based on state dimension expansion Active CN107421543B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710478805.0A CN107421543B (en) 2017-06-22 2017-06-22 Implicit function measurement model filtering method based on state dimension expansion

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710478805.0A CN107421543B (en) 2017-06-22 2017-06-22 Implicit function measurement model filtering method based on state dimension expansion

Publications (2)

Publication Number Publication Date
CN107421543A true CN107421543A (en) 2017-12-01
CN107421543B CN107421543B (en) 2020-06-05

Family

ID=60426530

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710478805.0A Active CN107421543B (en) 2017-06-22 2017-06-22 Implicit function measurement model filtering method based on state dimension expansion

Country Status (1)

Country Link
CN (1) CN107421543B (en)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109001699A (en) * 2018-01-30 2018-12-14 哈尔滨工业大学 Based on the tracking constrained with noise destination information
CN111735458A (en) * 2020-08-04 2020-10-02 西南石油大学 Navigation and positioning method of petrochemical inspection robot based on GPS, 5G and vision

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102289557A (en) * 2011-05-17 2011-12-21 杭州电子科技大学 Battery model parameter and residual battery capacity joint asynchronous online estimation method
CN103065037A (en) * 2012-11-13 2013-04-24 杭州电子科技大学 Nonlinear system target tracking method based on distributed volume information filtering
CN103294931A (en) * 2013-06-28 2013-09-11 上海交通大学 System state estimation method based on improved nonlinear robust filtering algorithm
CN104252178A (en) * 2014-09-12 2014-12-31 西安电子科技大学 Strong maneuver-based target tracking method
CN106017509A (en) * 2016-05-30 2016-10-12 北京航空航天大学 Method for determining anti-interference attitude in multi-source interference environment, and test platform
CN106352876A (en) * 2016-07-25 2017-01-25 北京航空航天大学 Airborne distributed POS transfer alignment method based on H infinity and CKF hybrid filtering

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102289557A (en) * 2011-05-17 2011-12-21 杭州电子科技大学 Battery model parameter and residual battery capacity joint asynchronous online estimation method
CN103065037A (en) * 2012-11-13 2013-04-24 杭州电子科技大学 Nonlinear system target tracking method based on distributed volume information filtering
CN103294931A (en) * 2013-06-28 2013-09-11 上海交通大学 System state estimation method based on improved nonlinear robust filtering algorithm
CN104252178A (en) * 2014-09-12 2014-12-31 西安电子科技大学 Strong maneuver-based target tracking method
CN106017509A (en) * 2016-05-30 2016-10-12 北京航空航天大学 Method for determining anti-interference attitude in multi-source interference environment, and test platform
CN106352876A (en) * 2016-07-25 2017-01-25 北京航空航天大学 Airborne distributed POS transfer alignment method based on H infinity and CKF hybrid filtering

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
周卫东等: "四元数扩维无迹卡尔曼滤波算法及其在大失准角快速传递对准中的应用", 《控制理论与应用》 *
孙媛等: "基于滤波扩维融合的传感器定位测距方法改进", 《计算机仿真》 *
赵敏华等: "基于地磁定轨和扩维卡尔曼滤波的导航算法", 《西安交通大学学报》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109001699A (en) * 2018-01-30 2018-12-14 哈尔滨工业大学 Based on the tracking constrained with noise destination information
CN111735458A (en) * 2020-08-04 2020-10-02 西南石油大学 Navigation and positioning method of petrochemical inspection robot based on GPS, 5G and vision

Also Published As

Publication number Publication date
CN107421543B (en) 2020-06-05

Similar Documents

Publication Publication Date Title
WO2018014602A1 (en) Volume kalman filtering method suitable for high-dimensional gnss/ins deep coupling
CN106767780B (en) The extension ellipsoid set-membership filtering method approached based on Chebyshev polynomial interopolations
CN110289989B (en) Distributed state estimation method based on cubature Kalman filtering algorithm
CN106441300B (en) It is a kind of with adaptive collaborative navigation filtering method
CN111181529B (en) Smooth constraint extended Kalman filtering method applied to nonlinear Gaussian model
CN103776449B (en) A kind of initial alignment on moving base method that improves robustness
CN103344260A (en) Initial alignment method of large azimuth misalignment angle of strapdown inertial navigation system based on RBCKF (rao-black-wellised cubature kalman filter)
CN108562290B (en) Navigation data filtering method and device, computer equipment and storage medium
KR102119254B1 (en) Method for designing Information Fusion Integrated Navigation of Inertial Navigation System, Global Navigation Satellite System and Terrain Referenced Navigation based Federated Filter and Computer readable medium having the same
CN107565931A (en) A kind of self calibration Unscented kalman filtering method
US20170292351A1 (en) Method and apparatus for estimating down-hole process variables of gas lift system
CN107421543B (en) Implicit function measurement model filtering method based on state dimension expansion
CN104035110B (en) The Fast Kalman filtering localization method being applied in multimodal satellite navigation system
CN103792562A (en) Strong tracking UKF filter method based on sampling point changing
CN116595897B (en) Nonlinear dynamic system state estimation method and device based on message passing
CN109840517A (en) A kind of estimation of MEMS gyro noise and filtering method
CN113432608B (en) Generalized high-order CKF method based on maximum correlation entropy for INS/CNS integrated navigation system
CN114139109A (en) Target tracking method, system, equipment, medium and data processing terminal
CN106871905B (en) Gaussian filtering substitution framework combined navigation method under non-ideal condition
CN103268403B (en) A kind of method for tracking target based on the strong trace information wave filter of volume
CN110532517A (en) Gas pipeline method for parameter estimation based on improved ARUKF
CN112632454B (en) MEMS gyroscope filtering method based on self-adaptive Kalman filtering algorithm
CN110912535A (en) Novel pilot-free Kalman filtering method
CN116667815A (en) Filtering method of non-Gaussian multiplicative noise system based on maximum entropy Gaussian sum
Mourad et al. An assumed‐gradient finite element method for the level set equation

Legal Events

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