CN107290742B - Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system - Google Patents

Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system Download PDF

Info

Publication number
CN107290742B
CN107290742B CN201710471328.5A CN201710471328A CN107290742B CN 107290742 B CN107290742 B CN 107290742B CN 201710471328 A CN201710471328 A CN 201710471328A CN 107290742 B CN107290742 B CN 107290742B
Authority
CN
China
Prior art keywords
matrix
algorithm
time
noise
sampling instant
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201710471328.5A
Other languages
Chinese (zh)
Other versions
CN107290742A (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.)
Wuhan University of Technology WUT
Original Assignee
Wuhan University of Technology WUT
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 Wuhan University of Technology WUT filed Critical Wuhan University of Technology WUT
Priority to CN201710471328.5A priority Critical patent/CN107290742B/en
Publication of CN107290742A publication Critical patent/CN107290742A/en
Application granted granted Critical
Publication of CN107290742B publication Critical patent/CN107290742B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/66—Radar-tracking systems; Analogous systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/28—Details of pulse systems
    • G01S7/285—Receivers
    • G01S7/292—Extracting wanted echo-signals
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
    • G01S7/35—Details of non-pulse systems
    • G01S7/352—Receivers
    • G01S7/354—Extracting wanted echo-signals
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
    • G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
    • G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
    • G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance

Abstract

The invention discloses square root volume kalman filter methods in a kind of Nonlinear Parameter tracking system to improve time update link and Sage-Husa noise estimator link, form ASCKFNS algorithm for adaptive square root volume Kalman filtering algorithm ASCKF；The present invention is suitable for nonlinear discrete systems, under the premise of guaranteeing the stability of algorithm, further improves algorithm accuracy, and largely reduce calculation amount, improves real-time.

Description

Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system
Technical field
The invention belongs to the technical fields such as data fusion, filtering algorithm, are related to a kind of square root volume Kalman filtering side Method, in particular to square root volume kalman filter method in a kind of Nonlinear Parameter tracking system.
Background technique
Sensor will receive random noise disturbance when being monitored to Target Tracking System, and Kalman filtering algorithm can be with Optimal estimation is carried out to system using the state equation and measurement equation of system discrete system, caused by reducing random noise disturbance It influences (document 1).Conventional Kalman filtering algorithm is only applicable to linear discrete system, Arasaratnam and Haykin two Scholar proposes square root volume Kalman filtering (the Square-root Cubature suitable for nonlinear discrete systems Kalman Filter, SCKF) algorithm, improve filter stability and accuracy (document 2), but noise statistics it is unknown, When in the environment of the mathematical model inaccuracy of system dynamics, this algorithm can be due to model and the measuring value of acquisition mismatch Lead to filtering divergence.Li Ning et al. proposes a kind of adaptive square root volume Kalman filtering algorithm (Adaptive Square-root Cubature Kalman Filter, ASCKF) (document 3), by Sage-Husa time varying noise estimator It applies in SCKF algorithm, by time varying noise estimator, can be filtered every time when carrying out Recursive Filtering using metric data When all simultaneously estimation and update the system noise covariance matrix QkWith measurement noise covariance matrix RkStatistical property, certain The shortcomings that overcoming SCKF algorithm filtering accuracy when noise statistics are unknown and uncertain lower in degree or even dissipating (document 4), but in fact, Sage-Husa time varying noise estimator filters needs in known Q every timekIn the case where to RkEstimated, and It cannot be in QkWith RkThey are estimated (document 5) simultaneously when all unknown, so that with the increase of filter times, the filter of ASCKF algorithm Wave accuracy may reduce and the complexity increase filtered causes real-time to reduce.Shandong equality people proposes one based on covariance The filtering divergence criterion of matching technique, only when filtering abnormal to RkEstimated, in the premise for guaranteeing precision with holding back scattered property Under, the complexity of calculating reduces, and can be improved the real-time (document 6) of system.But the flat filtering divergence criterion proposed in Shandong is only fitted For linear system, the filtering accuracy drop for solving Nonlinear Parameter tracking system can not be directly combined with ASCKF The problem of low and real-time reduces.In the adaptive strong tracking Kalman filter algorithm that [document 7] is introduced, propose to utilize Jacobi For matrix by nonlinear system Approximate Equivalent in the thought of linear system, effect is preferable, but strong tracking algorithm calculates complicated, real-time Difference.
In Target Tracking System practical application, mostly nonlinear system, main problem include two aspects:
(1) under normal conditions, the factors such as environmental change can make system be difficult to obtain accurate mathematical model and make an uproar Sound statistical property, for example, the factors such as wind, wave interfere the fortune that can all make hull when Ship Dynamic Positioning Systems Based is run across the sea Dynamic state is it is not possible that complete stability, and system motion mathematical model and noise statistics are difficult to accurately given at this time.
(2) when practical application, Target Tracking System needs to complete sensor in a sampling period to survey parameter, processor The operation such as filtering, it is necessary to reduce filtering as far as possible and calculate the time, guarantee that completing all navigation in a sampling period calculates.Cause This modified hydrothermal process improves the stability and real-time of system, is highly beneficial to practical application.
In conclusion needing a kind of filter of noise estimator performance for having superperformance that can apply to nonlinear system Wave algorithm is guaranteeing that filtering is accurate it is also desirable to algorithm is further simplified, to improve algorithm real-time.
The basic principle and application [J] software theory and method of [document 1] Peng Dingcong Kalman filtering, 2009, (11): 1-2.
[document 2] Arasaratnam I, Haykin S.Cubature Kalman filters [J] .IEEE Trans.on Automatic Control,2009,54(6):1254-1269.
Adaptive square root CKF target following of [document 3] Li Ning, Zhu Ruihui, Zhang Yonggang based on Sage_Husa algorithm Method [J] system engineering and electronic technology, 2014,36 (10)
[document 4] Lichtfuse E.Non-GPS navigation using vision-aiding and active radio range measurements[D].Department of the Air Force Air University,Air Force Institute of Technology,Wright-Pattersion Air Force Base,Ohio,USA,2011.
[document 5] Zhang Changyun adaptive filter method studies [J] aviation journal, 1998,19 (7): 96-99.
The improved Sage-Husa adaptive-filtering of [document 6] Lu Ping, Zhao Long, Chen Zhe and its application [J] system emulation Report, 2007,19 (15): 3503-3505
[document 7] Zhenjie Pan, Liang Gao, Shesheng Gao and Bingbing Gao.Adaptive cubature Kalman filter for ultra-tightly coupled BDS/INS integration[J] .Proceedings of 2016 IEEE 6th International Conference on Electronics Information and Emergency Communication,2016,269-272.
Summary of the invention
In order to solve the above-mentioned technical problems, the present invention provides a kind of adaptive square root volume Kalmans of nonlinear system Filtering algorithm (Adaptive Square-root Cubature Kalman Filter of Nonlinear System, ASCKFNS), the method for the linear system of nonlinear system approximate transform filtering divergence is improved using Jacobian matrix to sentence According to, it can be with ASCKF algorithm R. concomitans into nonlinear system target tracking algorism, and suitably simplify calculation amount, it mentions The accuracy and real-time of high algorithm.
The technical scheme adopted by the invention is that: square root volume Kalman filtering in a kind of Nonlinear Parameter tracking system Method, it is characterised in that: be directed to adaptive square root volume Kalman filtering algorithm ASCKF, improve the time update link with Sage-Husa noise estimator link forms ASCKFNS algorithm；
Link is updated in the time, by nonlinear state transfer function f according to asking the principle of single order local derviation to seek Jacobian matrixSubstitute into the state vector posterior estimate of -1 sampling instant of kthTo Jacobian matrixIn, it obtains kth time and adopts The sample moment is by the equivalent state matrix F of nonlinear system approximate linearizationk, simplify the state transition function of nonlinear system, from And simplify the time and update link, improve system real time；
In Sage-Husa noise estimator link, Nonlinear Parameter tracking system is measured into function h according to seeking single order local derviation Principle seek Jacobian matrixSubstitute into the prior state vector predictors x at k momentk|k-1To Jacobian matrixIn, obtain K sampling instant is by the equivalent measurement matrix H of nonlinear system approximate linearizationk, and then obtain relatively simple noise hold back it is scattered Judgement allows noise to hold back and dissipates judgement while acting on system noise covariance matrix Q when carrying out kth time filteringkWith measurement noise Covariance matrix Rk, check whether to need to correct QkWith Rk, do not have to filtering every time so all according to system noise covariance matrix Qk To measurement noise covariance matrix RkIt is modified, prevents long-term while estimating QkWith RkCause filtering abnormal.
The present invention is suitable for nonlinear discrete systems, under the premise of guaranteeing the stability of algorithm, further improves calculation Method accuracy, and calculation amount is largely reduced, improve real-time.
Detailed description of the invention
Fig. 1 is the flow chart of the embodiment of the present invention；
Fig. 2 is ASCKFNS algorithm, ASCKF algorithm and the real motion track comparison diagram of the embodiment of the present invention；
Fig. 3 is the comparison diagram of the state vector ASCKFNS algorithm of the embodiment of the present invention, ASCKF algorithm and true value；
Fig. 4 is ASCKFNS algorithm, the ASCKF algorithm state vector root-mean-square error comparison diagram of the embodiment of the present invention.
Specific embodiment
Understand for the ease of those of ordinary skill in the art and implement the present invention, with reference to the accompanying drawings and embodiments to this hair It is bright to be described in further detail, it should be understood that implementation example described herein is merely to illustrate and explain the present invention, not For limiting the present invention.
It is analyzed for general Nonlinear Parameter tracking system, mathematical model can be described as:
xk=f (xk-1)+wk-1(1)；
zk=h (xk)+vk(2)；
In formula (1), xkFor kth time sampling instant system mode vector, f (x) is system transfer function, wk-1It is kth -1 time Sampling instant system noise vector.
In formula (2), zkFor kth time sampling instant system measurements vector, h (x) is system measurements function, vkFor kth time sampling Moment measures noise vector.
Relationship between system noise vector and measurement noise vector are as follows:
In formula (3), indicate that system noise and measurement noise are irrelevant, the two desired value is 0, system noise covariance square Battle array is Q, and measurement noise covariance matrix is R.
Based on above-mentioned Nonlinear Parameter tracking system, referring to Fig.1, a kind of Nonlinear Parameter tracking system provided by the invention Square root volume kalman filter method in system improves the time for adaptive square root volume Kalman filtering algorithm ASCKF Link and Sage-Husa noise estimator link are updated, ASCKFNS algorithm is formed；Specifically includes the following steps:
(1) time updates；
By nonlinear function f () in pointCarry out approximate linearization:
Calculating state priori prediction value:
Calculate error co-variance matrix priori prediction value:
Calculate the square root of error co-variance matrix priori prediction value:
Sk|k-1=chol (Pk|k-1) (7)
In formula (7), chol () is to carry out factorization to matrix.
(2) it measures and updates；
Calculate volume point weight:
In formula (8), m is the dimension of state vector；I is unit matrix；[I,-I]jExpression takes the jth of matrix [I ,-I] to arrange； ωjFor volume point weight.
Calculate volume point:
Xj,k|k=Sk|k-1ξj+xk|k-1(9)；
Propagate volume point:
Zj,k|k-1=h (Xj,k|k) (10)；
It calculates and measures priori prediction value:
(3) it holds back and dissipates judgement calculating；
Calculate residual values:
vk=zk-zk|k-1(12)；
By nonlinear function h () in point xk|k-1Place carries out approximate linearization:
It carries out measurement noise and holds back scattered judgement:
(4) it holds back to dissipate to adjudicate and measurement noise is estimated；
If formula (14) is set up,Value are as follows:
Calculate forgetting factor parameter value:
In formula, the normal value 0.95~0.995 of b
If formula (14) is invalid,Value respectively are as follows:
(5) it measures and updates；
Calculate the square root of new breath covariance matrix:
In formula (20),To measure weighted center matrix；Tria () is to carry out QR transformation to matrix.
Calculate filtering gain value:
In formula (23), χk|k-1For system weighted center matrix；Pxz,k|k-1For Cross-covariance.
Calculating state posterior estimate:
Calculate error co-variance matrix posteriority predicted value:
Pk|k=Sk|kSk|k T(26)；
(6) it holds back to dissipate to adjudicate and system noise is estimated；
If formula (14) is set up,Value are as follows:
If formula (14) is invalid,Value are as follows:
(7) algorithm terminates to adjudicate
k≤N (29)；
In formula, N is the algorithm maximum sampling number of setting
If formula (29) is set up, continue successively to be calculated by step (1)~(7) sequence.
If formula (29) is invalid, terminate to calculate.
The present invention improves on the basis of ASCKF algorithm, for nonlinear discrete systems, mainly updates link in the time Made improvements with Sage-Husa noise estimator link, formed ASCKFNS algorithm, with improve system accuracy and in real time Property.ASCKFNS algorithm includes three links: the time updates link, measures update link, Sage-Husa noise estimator link, ASCKFNS algorithm mainly improves thought are as follows:
(1) link is updated in the time, it may be considered that by nonlinear state transfer function f according to asking the principle of single order local derviation to ask Jacobian matrixSubstitute into the state vector posterior estimate of -1 sampling instant of kthTo Jacobian matrixIn, Kth time sampling instant can be obtained by the equivalent state matrix F of nonlinear system approximate linearizationk, thus can will be non-linear The state transition function of system is simplified, so that simplifying the time updates link, improves system real time.
Sk|k-1=chol (Pk|k-1)；
In formula,System noise covariance matrix Q after the filtering obtained for -1 sampling instant of kthk-1Estimated value； Pk-1|k-1The error co-variance matrix posteriority predicted value obtained for -1 sampling instant of kth；Pk|k-1It is obtained for kth time sampling instant Error co-variance matrix priori prediction value；Chol () indicates to carry out Cholesky decomposition to matrix, symmetric positive definite Matrix is expressed as the product of a lower triangular matrix and its transposition, if such as chol (Pk|k-1)=Sk|k-1, then meet Sk|k- 1Sk|k-1 T=Pk|k-1；
The time of ASCKFNS algorithm updates link and updates link compared to the time of ASCKF algorithm, without volume point It seeks, reduces calculation amount, the specific calculation amount comparison of the two is specifically introduced below.
(2) in Sage-Husa noise estimator link, nonlinear system is measured into function h according to the original for seeking single order local derviation Reason seeks Jacobian matrixSubstitute into the prior state vector predictors x at k momentk|k-1To Jacobian matrixIn, when k can be obtained It carves the equivalent measurement matrix H of nonlinear system approximate linearizationk, and then obtain relatively simple noise and hold back scattered judgement, into When row kth time filtering, allows noise to hold back and dissipate judgement while acting on system noise covariance matrix QkWith measurement noise covariance square Battle array Rk, check whether to need to correct QkWith Rk, do not have to filtering every time so all according to system noise covariance matrix QkIt makes an uproar to measurement Sound covariance matrix RkIt is modified, prevents long-term while estimating QkWith RkCause filtering abnormal.It holds back and dissipates judgement derivation process are as follows:
vk≈zk-Hkxk|k-1；
It can must hold back scattered judgement are as follows:
In formula, vkFor the new breath of kth time sampling instant, zkFor kth time sampling instant system measurements vector；For kth- System noise Q after the filtering that 1 sampling instant obtainsk-1Estimated value；Tr [] is to Matrix Calculating mark.
Due to system noise covariance matrix QkAs the increase of filter times can gradually tend towards stability, the present invention revaluation Noise covariance matrix R is surveyed in meteringk, allow noise to hold back and dissipate judgement while acting on QkWith Rk, can simplify calculation amount.And in Sage- Negative term is suitably saved in the recurrence formula of Husa noise estimator, can further prevent the non-positive definite of error co-variance matrix in this way The case where can not decomposing, is further reduced calculation amount, improves algorithm real-time.
If above-mentioned judgement is set up, show that the actual error of state estimation caused by measuring noise will be more than theoretical estimated Value is needed to measurement noise covariance matrix RkIt is modified, thenWithValue is respectively as follows:
In formula,Noise covariance matrix R is measured after the filtering obtained for kth time sampling instantkEstimated value；It is System noise covariance matrix Q after the filtering that k sampling instant obtainskEstimated value；dk-1For forgetting factor, reinforce new data To the influence power of parameter estimation；KkFor the Kalman filtering gain of kth time sampling instant.
If above-mentioned judgement is invalid, show actual error not above theoretical predicted value, does not need to measurement noise covariance Matrix RkIt is modified, thenWithValue is respectively as follows:
It is further to effect of the invention below to be verified；
It selects ship kinematic system to verify The effect of invention as the typical example of Target Tracking System, ship is moved System, it is first determined then Target Tracking System mathematical model carries out Matlab software emulation with the method in the present invention, more Aspect compares the accuracy and real-time of ASCKFNS algorithm Yu ASCKF algorithm.
(1) ship motion mathematical model:
Select system mode vector are as follows:
xk=[x, x ', y, y ']T；
In formula, x is the displacement of ship x-axis；X ' is ship x-axis speed；Y is the displacement of ship y-axis；Y ' is ship y-axis speed.
Also there is preferable control performance due to needing to verify the ASCKFNS algorithm system inaccurate to mathematical model, therefore sets It has set two kinds of bigger motion states of difference to be converted, ship is selected to move in two-dimensional surface first as constant angular speed Circular motion is linear uniform motion again:
In formula, ω is angular speed；T is the discrete system sampling time；K is present sample number, K1For motion switch state The sampling number that moment has already passed through, N are the algorithm maximum sampling number of setting.
Selected amount direction finding amount are as follows:
zk=[l, θ]T；
In formula, l is using radar site as the polar diameter of vessel position in the polar coordinate system of origin, and it is former that θ, which is with radar site, The polar angle of vessel position in the polar coordinate system of point.
Select radar for the common origin position of plane right-angle coordinate and polar coordinate system:
(2) matlab software emulation is carried out for verification algorithm accuracy；
Select system noise initial value are as follows: Q0=diag [0.25 1]；
Measure noise initial value are as follows: R0=diag [1 2.25]；
Select covariance matrix initial value are as follows: P0=diag [1 11 1]；
Select vessel position initial value are as follows: x0=[410 15 200 10]T；
Enable sampling period T=0.2；Angular velocity omega=1；Forgetting factor parameter value b=0.98；The motion switch state moment is Sampling number K through passing through1=30；The algorithm maximum sampling number N=40 of setting selects state vector xkFirst element Displacement x, as ordinate, carries out MATLAB software emulation, can draw rectangular coordinate system as abscissa, third elements displacement y Interior target trajectory figure obtains ASCKFNS algorithm, ASCKF algorithm and real motion trajectory diagram.
In Fig. 2, stain indicates real motion track, and full curve indicates ASCKFNS algorithm motion profile, unbroken curve table Show ASCKF algorithm motion profile.As shown in Figure 2, stain is substantially all on full curve, show ASCKFNS algorithm accuracy compared with It is good, but it does not appear that ASCKFNS algorithm, ASCKF algorithm accuracy difference.To compare the two performance, this hair comprehensively It is bright to draw state vector x respectivelykThe comparison diagram of the ASCKFNS algorithms of four elements, ASCKF algorithm and true value, such as Fig. 3 It is shown
As can be seen that full curve fluctuation is more slightly smaller than unbroken curve from (a) and (c) of Fig. 3, show ASCKFNS algorithm Than ASCKF algorithm, in the displacement x and the filtering of displacement y for state vector, accuracy is higher, but (b) connects with (d) Continuous curve is overlapped with unbroken curve, can not find out the difference of speed x ' Yu speed y ', for more acurrate difference ASCKFNS algorithm and The effect of ASCKF algorithm, invention defines root-mean-square error σ:
Indicate the estimated value of jth time sampling instant i-th of element of state vector, m is state vector dimension, is this time emulated In, m=4.Root-mean-square error σ, which can be measured, samples interior state vector x at first k timeskThe estimated values of four elements and true value it is comprehensive Close error.MATLAB software emulation is carried out, is the effect of track algorithm long-play, the algorithm maximum sampling number of setting increases N=200 is added as, also has a preferable control performance to the system of noise statistics inaccuracy for verifying ASCKFNS algorithm, therefore System noise can also be changed with noise is measured in emulation, selection will in the 100th sampling instantWithExpand respectively GreatlyWithOther parameters are constant, can draw ASCKFNS algorithm, the comparison of ASCKF algorithm state vector root-mean-square error Figure, as shown in Figure 4；
In Fig. 4, full curve indicates the state vector root-mean-square error of ASCKFNS algorithm, and unbroken curve indicates that ASCKF is calculated The state vector root-mean-square error of method, most of sampled point of full curve show ASCKFNS algorithm all below unbroken curve Accuracy effect is preferable.In the 30th sampling, Equation of Motion mutates, but root-mean-square error is still smaller, Ke Yishi For in the uncertain system of mathematical model.And in the 100th sampling instant, although system noise, measure noise uprush for Originally 2 times, ASCKFNS algorithm is also very stable, this is because containing forgetting factor in its noise estimator, can be directed to noise Environment generates adaptive behavior, and the situation of noise statistics inaccuracy can also be applied.
For verification algorithm real-time, the present invention updates link in the time with ASCKF algorithm to ASCKFNS algorithm and estimates with noise The computation complexity of the formula changed in gauge link is counted.
1. updating link in the time, it is assumed that state vector is m dimension, measures vector as n dimension, ignores substitution volume point and ask non-thread The time of property functional value asks the time of Jacobian matrix, asks the time of transposed matrix.
The ASCKF algorithm time updates link process are as follows:
Sk-1|k-1=chol (Pk-1|k-1)；
XJ, k | k-1=f (XJ, k-1 | k-1)；
Calculation amount needed for ASCKF algorithm seeks state vector priori prediction value is 1 submatrix decomposition operation, 2m3+ m multiplication Operation, 2m3+2m2- m sub-addition operation；Calculation amount needed for ASCKF algorithm seeks the square root of error co-variance matrix priori prediction value For 3 submatrix decomposition operations, 2m3+2m2+ m multiplyings, 2m3+4m2- m sub-addition operation.
The ASCKFNS algorithm time updates link process are as follows:
Sk|k-1=chol (Pk|k-1)；
Calculation amount needed for ASCKFNS algorithm seeks state vector priori prediction value is m2Secondary multiplying, m2- m sub-addition fortune It calculates；Calculation amount needed for ASCKF algorithm seeks the square root of error co-variance matrix priori prediction value is 1 submatrix decomposition operation, 2m3 Secondary multiplying, 2m3-m2Sub-addition operation.
The computation complexity of ASCKF algorithm is greater than ASCKFNS algorithm, this is because ASCKFNS algorithm is by nonlinear system After the linear system of system function Approximate Equivalent of uniting, it is possible to reduce the number of matrix decomposition, and need to use in ASCKF algorithm 2m volume point carrys out approximate calculation Gauss weighted integral, and update operation time time and state dimension have close ties.
2. in noise estimator link, it is assumed that state vector is m dimension, measures vector as n dimension, ignores and seek Jacobian matrix Time, the time for seeking transposed matrix.
ASCKF algorithm noise estimator link process are as follows:
ASCKFNS algorithm noise estimator link process are as follows:
When holding back scattered judgement establishment, then:
If hold back dissipate judgement it is invalid when:
Compare the noise estimator link of above two algorithm, it can be seen that no matter hold back dissipate decide whether to set up, ASCKF algorithm Computation complexity be greater than ASCKFNS algorithm, this is because ASCKFNS algorithm has carried out deduction item processing.
Generally speaking, the real-time of ASCKFNS algorithm is better than the real-time of ASCKF algorithm.
It should be understood that the part that this specification does not elaborate belongs to the prior art.
It should be understood that the above-mentioned description for preferred embodiment is more detailed, can not therefore be considered to this The limitation of invention patent protection range, those skilled in the art under the inspiration of the present invention, are not departing from power of the present invention Benefit requires to make replacement or deformation under protected ambit, fall within the scope of protection of the present invention, this hair It is bright range is claimed to be determined by the appended claims.

Claims (1)

1. square root volume kalman filter method in a kind of Nonlinear Parameter tracking system, it is characterised in that: for adaptive Square root volume Kalman filtering algorithm ASCKF improves time update link and Sage-Husa noise estimator link, is formed ASCKFNS algorithm；
Link is updated in the time, by nonlinear state transfer function f according to asking the principle of single order local derviation to seek Jacobian matrixGeneration Enter the state vector posterior estimate of -1 sampling instant of kthTo Jacobian matrixIn, when obtaining kth time sampling It carves the equivalent state matrix F of nonlinear system approximate linearizationk, simplify the state transition function of nonlinear system, thus simple Change time update link, improves system real time；
In Sage-Husa noise estimator link, Nonlinear Parameter tracking system is measured into function h according to the original for seeking single order local derviation Reason seeks Jacobian matrixSubstitute into the prior state vector predictors x of kth time sampling instantk|k-1To Jacobian matrixIn, Kth time sampling instant is obtained by the equivalent measurement matrix H of nonlinear system approximate linearizationk, and then obtain relatively simple make an uproar Sound holds back scattered judgement, when carrying out kth time filtering, allows noise to hold back and dissipates judgement while acting on system noise covariance matrix QkWith amount Survey noise covariance matrix Rk, check whether to need to correct QkWith Rk, do not have to filtering every time so all according to system noise covariance Matrix QkTo measurement noise covariance matrix RkIt is modified, prevents long-term while estimating QkWith RkCause filtering abnormal；
The improvement time updates link, implements formula are as follows:
Sk|k-1=chol (Pk|k-1)；
In formula,System noise covariance matrix Q after the filtering obtained for -1 sampling instant of kthk-1Estimated value； Pk-1|k-1The error co-variance matrix posteriority predicted value obtained for -1 sampling instant of kth；Pk|k-1It is obtained for kth time sampling instant Error co-variance matrix priori prediction value；Chol () indicates to carry out Cholesky decomposition to matrix, symmetric positive definite Matrix is expressed as the form an of lower triangular matrix and the product of its transposition；
In the Sage-Husa noise estimator link, holds back and dissipates judgement derivation process are as follows:
vk≈zk-Hkxk|k-1；
It can obtain and hold back scattered judgement are as follows:
In formula, vkFor the new breath of kth time sampling instant, zkFor kth time sampling instant system measurements vector；It is kth -1 time System noise Q after sampling instant filteringk-1Estimated value；Tr [] is to Matrix Calculating mark；
If above-mentioned judgement is set up, shows that the actual error of state estimation caused by measuring noise will be more than theoretical predicted value, need It to be modified to noise covariance matrix Rk is measured, thenWithValue is respectively as follows:
In formula,Noise covariance matrix R is measured after the filtering obtained for kth time sampling instantkEstimated value；For kth time System noise covariance matrix Q after the filtering that sampling instant obtainskEstimated value；dk-1For forgetting factor, reinforce new data to ginseng The influence power of number estimation；KkFor the Kalman filtering gain of kth time sampling instant;
If above-mentioned judgement is invalid, show actual error not above theoretical predicted value, does not need to measurement noise covariance matrix Rk is modified, thenWithValue is respectively as follows:
CN201710471328.5A 2017-06-20 2017-06-20 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system Active CN107290742B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201710471328.5A CN107290742B (en) 2017-06-20 2017-06-20 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201710471328.5A CN107290742B (en) 2017-06-20 2017-06-20 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system

Publications (2)

Publication Number Publication Date
CN107290742A CN107290742A (en) 2017-10-24
CN107290742B true CN107290742B (en) 2019-06-11

Family

ID=60097599

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201710471328.5A Active CN107290742B (en) 2017-06-20 2017-06-20 Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system

Country Status (1)

Country Link
CN (1) CN107290742B (en)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109061579A (en) * 2018-08-03 2018-12-21 中国航空工业集团公司雷华电子技术研究所 A kind of robust volume filtering method of band thickness tail noise
CN109443342A (en) * 2018-09-05 2019-03-08 中原工学院 NEW ADAPTIVE Kalman's UAV Attitude calculation method
CN112415412A (en) * 2019-08-23 2021-02-26 比亚迪股份有限公司 Method and device for estimating SOC value of battery, vehicle and storage medium
CN110726887A (en) * 2019-09-09 2020-01-24 国网浙江省电力有限公司电力科学研究院 Voltage sag detection method for in-situ power system
CN112764345B (en) * 2020-12-21 2022-08-02 杭州电子科技大学 Strong nonlinear system Kalman filter design method based on target state tracking
CN113639744B (en) * 2021-07-07 2023-10-20 武汉工程大学 Navigation positioning method and system for bionic robot fish

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005127977A (en) * 2003-10-27 2005-05-19 Mitsubishi Electric Corp Target tracking system
CN104463214A (en) * 2014-12-11 2015-03-25 衢州学院 SVR parameter optimization method based on DRVB-ASCKF
CN104868876A (en) * 2015-05-12 2015-08-26 北京理工大学 Kalman filtering method under the condition of unknown process noise covariance matrix Q
CN106372646A (en) * 2016-08-30 2017-02-01 上海交通大学 Multi-target tracking method based on SRCK-GMCPHD filtering
CN106786561A (en) * 2017-02-20 2017-05-31 河海大学 A kind of Low-frequency Oscillation Modal Parameters discrimination method based on adaptive Kalman filter

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005127977A (en) * 2003-10-27 2005-05-19 Mitsubishi Electric Corp Target tracking system
CN104463214A (en) * 2014-12-11 2015-03-25 衢州学院 SVR parameter optimization method based on DRVB-ASCKF
CN104868876A (en) * 2015-05-12 2015-08-26 北京理工大学 Kalman filtering method under the condition of unknown process noise covariance matrix Q
CN106372646A (en) * 2016-08-30 2017-02-01 上海交通大学 Multi-target tracking method based on SRCK-GMCPHD filtering
CN106786561A (en) * 2017-02-20 2017-05-31 河海大学 A kind of Low-frequency Oscillation Modal Parameters discrimination method based on adaptive Kalman filter

Also Published As

Publication number Publication date
CN107290742A (en) 2017-10-24

Similar Documents

Publication Publication Date Title
CN107290742B (en) Square root volume kalman filter method in a kind of Nonlinear Parameter tracking system
Perala et al. Robust extended Kalman filtering in hybrid positioning applications
CN105740203A (en) Multi-sensor passive synergic direction finding and positioning method
Ricker et al. Adaptive tracking filter for maneuvering targets
CN104019817B (en) A kind of norm constraint strong tracking volume kalman filter method for Satellite Attitude Estimation
Xu et al. A novel DVL calibration method based on robust invariant extended Kalman filter
Li et al. A calibration method of DVL in integrated navigation system based on particle swarm optimization
CN105676217B (en) A kind of improved ML folded Clutter in Skywave Radars maneuvering target method for parameter estimation
CN111913175A (en) Water surface target tracking method with compensation mechanism under transient failure of sensor
CN103776449A (en) Moving base initial alignment method for improving robustness
Guo et al. A robust SINS/USBL integrated navigation algorithm based on earth frame and right group error definition
EP2869026B1 (en) Systems and methods for off-line and on-line sensor calibration
CN110231620B (en) Noise-related system tracking filtering method
Zha et al. An improved nonlinear filter based on adaptive fading factor applied in alignment of SINS
CN109856623B (en) Target state estimation method for multi-radar linear flight path line
Wang et al. A robust backtracking CKF based on Krein space theory for in-motion alignment process
CN110501686A (en) A kind of method for estimating state based on NEW ADAPTIVE high-order Unscented kalman filtering
Fan et al. A modified adaptive Kalman filtering method for maneuvering target tracking of unmanned surface vehicles
CN107621632A (en) Adaptive filter method and system for NSHV tracking filters
Lou et al. Robust partially strong tracking extended consider Kalman filtering for INS/GNSS integrated navigation
CN109471192A (en) A kind of full-automatic gravity tester Dynamic High-accuracy data processing method
CN108731702A (en) A kind of large misalignment angle Transfer Alignment based on Huber methods
Li et al. A new adaptive unscented Kalman filter based on covariance matching technique
Mallick et al. Comparison of measures of nonlinearity for bearing-only and GMTI filtering
Wang et al. An adaptive Kalman filtering algorithm based on maximum likelihood estimation

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