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 PDFInfo
- 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
- 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.)
- Granted
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)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Indication And Recording Devices For Special Purposes And Tariff Metering Devices (AREA)
- Length Measuring Devices With Unspecified Measuring 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
Technical field
The invention belongs to Spacecraft Autonomous Navigation field, is related to the implicit function measurement model filtering side that a kind of state is augmented
Method.
Background technology
R.E.Kalman proposes a kind of filtering method of linear optimal recursion, i.e. Kalman filtering in nineteen sixty
(Kalman Filter,KF).Initial KF is only applicable to linear system, with expansion of the people to nonlinear system filtering demands
Greatly, EKF (Extended Kalman Filter, EKF), Unscented Kalman filterings (Unscented
Kalman Filter, UKF), the filtering method such as particle filter (Particle Filter, PF) progressively proposed and obtained constantly
Development.Measurement model all has explicit expression in classical Kalman filtering algorithm, but in many practical problems, shape
The constraint of state amount and measurement is often implicit, is not easy or can not obtain explicit measurement model, such issues that be exactly implicit
Measurement model filtering problem.
The solution method on the state estimation problem containing implicit measurement model mainly has two class methods both at home and abroad.The first kind
Be by the propositions such as Soatto by implicit extended Kalman filter (Implicit ExtendedKalman Filter,
IEKF), by being linearized implicit measurement equation at reference point and taking second order form, with reference to traditional with explicit measurement side
The EKF algorithms of journey, have obtained IEKF.Second class is that one kind of proposition contains by Steffen on the basis of IEKF methods are analyzed
The filtering method of the iterative implicit measurement model for measuring renewal, i.e. Iterative IEKF.Above-mentioned two classes method is established
On the basis of EKF, it is required for calculating Jacobian matrixes in application, linearized stability can reduce the precision of filtering algorithm, may
Cause the diverging of filter result, and the calculating of Jacobian matrixes is usually relatively complex.
The content of the invention
The technical problem to be solved in the present invention is:Overcome the deficiencies in the prior art, there is provided it is a kind of be augmented based on state it is hidden
Function measurement model filtering method, compared to IAEKF and IEKF, obtain preferably estimation performance.Particularly when measurement noise increases
Added-time, compared to implicit UKF, estimation performance can be very significantly improved.
The present invention proposes 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 to be filtered renewal, specifically include with
Lower step:
The first step, first, the substantial amount at k moment is measuredExpand to quantity of state XkIn, the quantity of state after construction extensionWithEstablish the system model for being augmented quantity of state satisfaction;
Second step, initialization, according to system model, solve the quantity of state of initial time, by the quantity of state of initial time and
The quantity of state that its covariance matrix is substituted into the first step after the extension constructedWithIn, it is denoted as respectivelyWith
3rd step, time renewal, according to the quantity of state and covariance matrix of the initial time after the extension tried to achieve in second step,
Calculate the state of predictionAnd its error covariance matrixRemaining at the time of, calculate prediction stateAnd its by mistake
Poor covariance matrixWhen, then the quantity of state tried to achieve in renewal step is measured according to the 4th stepAnd corresponding error association
Variance matrixTo try to achieve;
4th step, renewal is measured, obtained status predication value is solved according to time renewal, solves the state estimation after renewalAnd corresponding error covariance matrixThe 3rd step is then return to, realizes that circulation solves, until filtering terminates.
Quantity of state after the first step construction extension, the system model that foundation is augmented quantity of state satisfaction are as follows:
Wherein, XkAnd ZkThe respectively quantity of state and measurement at k moment,The quantity of state after extension is represented,Represent by
Actual measurement ZkWith measurement noise vkThe real measurement being mixed to get;
In formula, Fa() andThe state model and its state error of extended mode amount are represented respectively, and h () is non-linear
Explicit function.
In the second step, the quantity of state after being augmentedAnd its covariance matrixIt is initialized as:
Wherein,E [x] represents x desired value, Z1And N1
The measurement and its covariance matrix of initial time are represented respectively, and n and m represent state vector and measure the dimension of vector respectively.
3rd step, time renewal are as follows:
In formula,It is to meet that average isCovariance isPoint, wiIt is i-th of Sigma point
Weights,ForCovariance matrix, wherein,
In formula, naThe dimension of the quantity of state after extension is represented, τ is scale parameter,Representing matrix square
RootI-th dimension column vector.
4th step, it is as follows to measure renewal:
Wherein,For filtering gain matrix, Ykk-1For the predicted value of measurement, Respectively corresponding error covariance matrix.
The present invention compared with prior art the advantages of be:
(1) quantity of state and real measurement are extended to a new quantity of state by the present invention, while null vector is considered as
Equivalent measurement updates to be filtered, and compared to IAEKF and IEKF, obtains preferably estimation performance so that estimated accuracy
It is improved;Particularly when measuring noise increase, compared to implicit UKF, performance can be very significantly improved.
(2) this hair eliminates the process that Jacobian matrix is solved in existing method, reduces amount of calculation.
Brief description of the drawings
Fig. 1 is the flow chart for the implicit function measurement model filtering method being augmented in the present invention based on state;
Embodiment
Fig. 1 gives the flow chart for the implicit function measurement model filtering method being augmented based on state.The following detailed description of this
The specific implementation process of invention:
Common nonlinear system contains explicit measurement model, and such system can be described as:
In formula, state equation f () and measurement equation h () are non-linear explicit functions.XkAnd wkWhen representing k respectively
The state vector and its noise at quarter, Xk+1Represent the state vector at k+1 moment.ZkAnd vkRespectively represent the k moment measurement vector and
Its noise.In practical application, it is believed that state vector and measurement vector are by zero-mean, incoherent white Gaussian noise shadow
Ring, i.e. quantity of state noise and measurement noise is obeyed respectively:
Wherein, QkAnd NkState-noise is represented respectively and measures the covariance matrix corresponding to noise, and its occurrence is passed through by engineering
Test or systematic parameter determines.
But in many practical problems, quantity of state and actual measurement are the amounts for being restrained to an implicit function form
Survey model in, be not easy or explicit measurement equation can not be obtained, such issues that be exactly implicit measurement model filtering problem.It is such
Implicit measurement model can be described as following system:
Wherein, quantity of state XkWith measurement ZkImplicit function h ()=0 is formed to constrain.
First, can be quantity of state and real amount in order to handle the nonlinear system problem containing implicit measurement model
Measurement is extended to a new quantity of state:
In formula, the extension of subscript a flag states,Represent by actual measurement ZkWith measurement noise vkWhat is be mixed to get is true
Measurement,Represent the quantity of state after extension.
Second, pay attention to the h (X in system (3)k,Zk+vk) it is equal to the null vector of m dimensions, therefore null vector can be regarded as
Equivalent measurement Yk, i.e.,:
Due to the quantity of state after extensionIt is by quantity of state X beforekMeasured with substantial amountForm, therefore measure mould
Type (5) can be rewritten as:
Therefore, establish be augmented quantity of state satisfaction system model be:
In formula, Fa() andThe state model and its state error of extended mode amount, F are represented respectivelya() and's
Calculation formula is as follows:
Wherein,Covariance matrix be defined as
The specific implementation step of IAUKF methods is as follows:
1. initialization
Initial state estimation and corresponding error covariance matrixAnd P0It is respectively set as:
In formula, E [x] represents x desired value, state error covariance matrix QkWith error in measurement covariance matrix NkSelect respectively
Initial time is worth accordingly.
Initial extended mode amountIt should be constructed according to formula (4), but in practical application, substantial amount measuresNothing
Method obtains, the actual measurement Z of the present invention1Measured instead of substantial amountIt is N now to introduce noise covariance battle array1Measurement
Noise, therefore, the quantity of state after being augmentedAnd its covariance matrixIt can be respectively configured to:
In formula, Z1And N1Represent the measurement and its covariance matrix of initial time respectively, n and m represent respectively state vector and
Measure the dimension of vector.
2. the time updates
At the k moment, it is necessary first to the extended mode amount estimate obtained to last momentIt is modified according to formula (4).
However, in practical application, substantial amount measuresIt can not obtain, we use actual measurement ZkMeasured instead of substantial amount
Now, introduce and measure noise vkAnd its noise covariance battle array Nk.Therefore, it is augmented quantity of stateAnd its covariance matrixCan be with
It is modified as the following formula:
In formula,And Pk-1Represent to be contained in respectivelyWithIn do not extend quantity of state estimation and its error association
Variance matrix.
Similarly, IAUKF methods are based on UT conversion and carry out probability deduction.Meet that average isCovariance is2na
+ 1 Sigma point is equivalent toDistribution, these Sigma points are propagated by system model (7) after can obtaining corresponding propagate
Sigma points, these propagation after Sigma points can be used for calculate prediction stateAnd its error covariance matrixThis
The specific Sigma of group is determined according to the following formula:
In formula, naThe dimension of the quantity of state after extension is represented, numerically equal to n+m, τ are scale parameters,Representing matrix square rootI-th dimension column vector, wiIt is the weights of i-th of Sigma point.
Sigma presses state model transmission:
Then status predication value and its error covariance matrix can be calculated as follows:
3. measure renewal
According to formula (6), estimating the Sigma points of measurement can be calculated according to the following formula:
Due to the predicted value of Sigma points obtained according to formula (16)It is not the time of day at k moment, containing wrong
Difference.Therefore, formula (18) is also not equal to its actual value 0, and this is provided on predicted stateControl information, available for state
Amendment.
Then, the predicted value of measurement can be calculated as:
Corresponding error covariance matrix can be obtained by following formula:
Then, filtering gain matrixState estimation after renewalAnd corresponding error covariance matrixIt can distinguish
Calculated and obtained according to UKF methods:
Table 1, table 2 is IAUKF, IEKF, IAEKF, and implicitly UKF navigation results compare.
The navigation results of 1 four kinds of filtering methods of table
The different navigation results for measuring the lower four kinds of filtering methods of noise of table 2
What table 1 was compared is smaller when measuring noise, is 1 " when, the filter result of four kinds of filtering methods;Table 2 is compared
, different to measure under noise, the filter result of four kinds of filtering methods.It can be seen that:When measurement noise is smaller, IAUKF is compared
In IAEKF and IEKF, the estimated accuracy of position is greatly improved;When measurement noise gradually increases, IAUKF is compared to implicit
UKF, estimation performance can be very significantly improved.
The content not being described in detail in description of the invention belongs to prior art known to professional and technical personnel in the field.
Claims (5)
1. a kind of implicit function measurement model filtering method being augmented based on state, it is characterised in that comprise the following steps:
The first step, first, the substantial amount at k moment is measuredExpand to quantity of state XkIn, the quantity of state after construction extensionWithEstablish the system model for being augmented quantity of state satisfaction;
Second step, initialization, according to system model, the quantity of state of initial time is solved, by the quantity of state of initial time and its association
The quantity of state that variance matrix is substituted into the first step after the extension constructedWithIn, it is denoted as respectivelyWith
3rd step, time renewal, according to the quantity of state and covariance matrix of the initial time after the extension tried to achieve in second step, calculate
The state of predictionAnd its error covariance matrixRemaining at the time of, calculate prediction stateAnd its error association
Variance matrixWhen, then the quantity of state tried to achieve in renewal step is measured according to the 4th stepAnd corresponding error covariance
Battle arrayTo try to achieve;
4th step, renewal is measured, obtained status predication value is solved according to time renewal, solves the state estimation after renewalWith
And corresponding error covariance matrixThe 3rd step is then return to, realizes that circulation solves, until filtering terminates.
2. the implicit function measurement model filtering method being augmented based on state described in as requested 1, it is characterised in that:Described first
Quantity of state after step construction extension, the system model that foundation is augmented quantity of state satisfaction are 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>&OverBar;</mo>
</mover>
<mi>k</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
Wherein, XkAnd ZkThe respectively quantity of state and measurement at k moment,The quantity of state after extension is represented,Represent by reality
Measurement ZkWith measurement noise vkThe real measurement being mixed to get;
<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 formula, Fa() andThe state model and its state error of extended mode amount are represented respectively, and h () is Nonlinear Explicit
Function.
3. the implicit function measurement model filtering method being augmented based on state described in as requested 1, it is characterised in that:Described second
In step, the quantity of state after being augmentedAnd its covariance matrixIt is initialized as:
<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>&times;</mo>
<mi>m</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mi>m</mi>
<mo>&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] represents x desired value, Z1And N1Respectively
The measurement and its covariance matrix of initial time are represented, n and m represent state vector and measure the dimension of vector respectively.
4. the implicit function measurement model filtering method being augmented based on state described in as requested 1, it is characterised in that:Described 3rd
Step, time renewal are 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>&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>&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>&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>&lsqb;</mo>
<msubsup>
<mi>&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>&rsqb;</mo>
<msup>
<mrow>
<mo>&lsqb;</mo>
<msubsup>
<mi>&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>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>+</mo>
<msubsup>
<mi>Q</mi>
<mi>k</mi>
<mi>a</mi>
</msubsup>
</mrow>
In formula,It is to meet that average isCovariance isPoint, wiIt is the power of i-th of Sigma point
Value,ForCovariance matrix, wherein,
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&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>&tau;</mi>
<mo>/</mo>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>n</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<mi>&tau;</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&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>&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>&lsqb;</mo>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>n</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<mi>&tau;</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>&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>&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>&lsqb;</mo>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>n</mi>
<mi>a</mi>
</msub>
<mo>+</mo>
<mi>&tau;</mi>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>&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 formula, naThe dimension of the quantity of state after extension is represented, τ is scale parameter,Representing matrix square rootI-th dimension column vector.
5. the implicit function measurement model filtering method being augmented based on state described in as requested 1, it is characterised in that:Described 4th
Step, it is as follows to measure renewal:
<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 gain matrix, Yk|k-1For the predicted value of measurement, Respectively corresponding error covariance matrix.
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)
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)
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 |
-
2017
- 2017-06-22 CN CN201710478805.0A patent/CN107421543B/en active Active
Patent Citations (6)
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)
Title |
---|
周卫东等: "四元数扩维无迹卡尔曼滤波算法及其在大失准角快速传递对准中的应用", 《控制理论与应用》 * |
孙媛等: "基于滤波扩维融合的传感器定位测距方法改进", 《计算机仿真》 * |
赵敏华等: "基于地磁定轨和扩维卡尔曼滤波的导航算法", 《西安交通大学学报》 * |
Cited By (2)
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 |
---|---|---|
Kolås et al. | Constrained nonlinear state estimation based on the UKF approach | |
WO2021063136A1 (en) | Data-driven high-precision integrated navigation data fusion method | |
CN106683122A (en) | Particle filtering method based on Gaussian mixture model and variational Bayes | |
CN106767780B (en) | The extension ellipsoid set-membership filtering method approached based on Chebyshev polynomial interopolations | |
CN105321345B (en) | A kind of road traffic flow prediction method filtered based on ARIMA models and kalman | |
Giles et al. | Multilevel Monte Carlo methods for applications in finance | |
CN103902812B (en) | A kind of particle filter method, device and method for tracking target, device | |
CN104252178A (en) | Strong maneuver-based target tracking method | |
CN107783944A (en) | A kind of multi-model self calibration Unscented kalman filtering method | |
CN106772524A (en) | A kind of agricultural robot integrated navigation information fusion method based on order filtering | |
Shi et al. | Adaptive dynamic surface asymptotic tracking control of uncertain strict‐feedback systems with guaranteed transient performance and accurate parameter estimation | |
CN107421543A (en) | A kind of implicit function measurement model filtering method being augmented based on state | |
CN107547067A (en) | A kind of multi-model self calibration EKF method | |
Takei et al. | An efficient algorithm for a visibility-based surveillance-evasion game | |
CN112113146A (en) | Synchronous self-adaptive check method for roughness coefficient and node water demand of water supply pipe network pipeline | |
CN103296995B (en) | Any dimension high-order (>=4 rank) tasteless conversion and Unscented Kalman Filter method | |
Cavagnari | REGULARITY RESULTS FOR A TIME-OPTIMAL CONTROL PROBLEM IN THE SPACE OF PROBABILITY MEASURES. | |
CN114139109A (en) | Target tracking method, system, equipment, medium and data processing terminal | |
Peng et al. | Nonlinear dynamic systems and optimal control problems on time scales | |
CN104539265A (en) | Self-adaptive UKF (Unscented Kalman Filter) algorithm | |
CN111310110A (en) | Mixed state estimation method for high-dimensional coupling uncertain system | |
Kohno et al. | A new method for the level set equation using a hierarchical-gradient truncation and remapping technique | |
CN103345569A (en) | Noise coefficient computing method used for Allan variance analysis technology | |
Zhang et al. | Necessary conditions of exponential stability for a class of linear uncertain systems with a single constant delay | |
CN115580261A (en) | State estimation method based on combination of robust strategy and dynamic enhanced CKF |
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 |