CN103528587B - Independent combined navigation system - Google Patents

Abstract

The present invention relates to a kind of independent combined navigation system, belong to navigation system technical field.Using SINS as principle navigation system, SAR, CNS are as secondary navigation system, constituting SINS/SAR/CNS integrated navigation system, step is as follows: first design SINS/SAR and SINS/CNS navigation subfilter, calculates the two groups of local optimum estimated values obtaining integrated navigation system stateWith local Optimal error covariance matrixUse federated filter technology that two groups of local optimum estimated values feeding senior filters are carried out fusion again and obtain global optimum's estimated valueWith global optimum's error covariance matrixFinally utilize global optimum's estimated value

Description

Independent combined navigation system

Technical field

The present invention relates to a kind of independent combined navigation system, belong to navigation system technical field.

Background technology

Independent navigation refers under conditions of being independent of ground staff's observing and controlling, it is possible to the autonomous position accurately determining aircraft Put and speed.That is when aircraft be in one unknown, complicated, in dynamic non-structure environment, do nobody Under conditions of Yu, pass through environment sensing, it is possible to arrive desired destination, the consumption of time or energy can be reduced simultaneously as far as possible Deng.Independent navigation sensor possesses 4 features: autonomous, real-time, passive and not against terrestrial information.

Independent navigation is to propose towards China's military spacecraft (military satellite, military aircraft and Cruise Missile etc.) development reality New, there is strong challenging research topic.Military spacecraft airmanship develops towards the direction of independent combined navigation It it is an inexorable trend.

Autonomous navigation technology include inertial navigation, celestial navigation, radionavigation, satellite navigation, earth-magnetism navigation, landform/ Image and the navigation of various visual information.Inertial navigation system is to utilize the inertance element sensitivity vehicles such as accelerometer in motion During acceleration, then by integral and calculating, obtain the navigational parameter such as Position And Velocity of carrier.Inertial navigation system Advantage is entirely autonomous formula, strong security, is susceptible to the interference of external condition and anthropic factor, round-the-clock, is not limited by weather System, but disadvantage is error can As time goes on accumulate.Celestial navigation system utilizes optics or radio telescope to connect Receiving the next electromagnetic wave of celestial body transmitting to go to follow the tracks of celestial body, attitude measurement accuracy is high, for high-altitude and the spacecraft of rarefaction of air It it is ideal navigation mode.Radio navigation system utilizes radiotechnics measure and navigation parameter, including Doppler effect Test the speed, with radar range finding and interception, with guidance station location etc., if its output information spinner carrier positions, to precision navigation system From the point of view of Tong, its positioning precision is the highest, and working range is limited by earth station overlay area.Satellite navigation system is that astronomy is led Boat with radionavigational conjugate, classical satellite navigation system include the U.S. GPS system, Europe " Galileo " system, And the Beidou system of China, there is navigation accuracy height, use the advantages such as simple, but star seen from positioning precision heavy dependence Quantity and geometric distribution, region poor for observing environment, because visible star number amount causes precision to be greatly reduced less, and signal is easy Disturbed or cover.Earth-magnetism navigation technology, as a kind of passive autonomous navigation method, has capacity of resisting disturbance strong, without accumulated error Etc. advantage, shortcoming is that precision is poor, is more suitable in cruise missile, surface vessel, water the auxiliary navigation methods such as latent device.

Obviously, existing any single navigation system is all difficult to high-precision independent navigation is fully achieved.High-precision from leading Boat needs the integrated navigation system being made up of multiple airborne sensors to complete, and its key technology realized is multi-sensor information Merge.Multi-sensor information fusion is as the frontier nature recently emerged, research field that prospect is the most wide, widely It is applied to the integrated disposal processing of multi-source information.It fully utilizes the different characteristics of polytype sensor, can be multi-faceted Overall Acquisition target different attribute information, improves autonomous navigation system coverage temporally and spatially, improves The service efficiency of navigation sensor information also adds the credibility of information.The appearance of the most various filtering algorithms, for combination Navigation system provides theoretical basis and mathematical tool.

At present, filtering method engineering combining guiding systems employing is mainly Kalman filtering (Kalman Filter, KF) With EKF (Extended Kalman Filter, EKF).KF requires that system mathematic model is necessary for linear, when When integrated navigation system model has nonlinear characteristic, linear model is still used to describe integrated navigation system and use KF to carry out Filtering, it will cause linear model error of approximation.

Although EKF is widely applied in integrated navigation system nonlinear filtering, but it still has theoretical limitation Property, it is in particular in: (1), when mission nonlinear degree is more serious, the higher order term ignoring Taylor expansion will cause linearisation Error increases, and causes the filtering error of EKF to increase and even dissipates；(2) Jacobian matrix ask for complicated, computationally intensive, in reality Border application is difficult to carry out, the most even hardly results in the Jacobian matrix of nonlinear function；(3) EKF is by state equation Model error processes as process noise, and is assumed to be white Gaussian noise, this and the actual noise situation of integrated navigation system And do not correspond；Meanwhile, EKF is derived by based on KF, and the statistical property of system initial state is required strict by it.Cause This EKF is very poor about the probabilistic robustness of system model.

The non-thread such as model prediction filtering (MPF), particle filter (PF) and the Unscented kalman filtering (UKF) that are occurred in the recent period Property filtering method and interacting multiple model algorithm, Nonlinear Filtering Problem and overcome model not in processing integrated navigation system The aspect such as determine, have the most original advantage.Although they achieve certain theory in integrated navigation system application aspect Achievement, but there is problems with and merit attention: the selection of (1) importance function directly affects the performance height of PF, carries out PF weight The Common Criteria that the property wanted function selects studies great meaning；Currently for many practical problems, occur in that many selection importances The innovatory algorithm of function, but the importance function system of selection being applied in integrated navigation system is less, needs the most theoretical Analyze；(2) though classical method for resampling can effectively overcome particle degeneracy, but amount of calculation forms progression with population increase passs Increasing, system real time is deteriorated, and how solving particle filter realizability in integrated navigation system becomes subject matter；(3) There is presently no the problem that PF can restrain by any mathematical theory in which kind of condition and make answer, therefore evaluate and analyze PF and exist Performance in integrated navigation system application the most just becomes the most difficult.

For UT conversion and UKF, although the precision that can obtain UT conversion at present proves, but UKF algorithm still can not be as EKF Provide stability analysis like that, have impact on its application in integrated navigation system.The sampling policy of UKF is existing a variety of, its Precision is low, it is impossible to be accurately obtained nonlinear system higher order term information, the best to the filter effect of non-Gaussian filtering.Essence Degree height, but calculate excessively complicated and poor real, cause UKF algorithm to realize difficulty in integrated navigation system.

As can be seen here, existing filtering technique can not fully meet the requirement of high-precision independent navigation, needs from leading Flying height precision non-linear algorithm is furtherd investigate with generating date technology, for solving the base of military spacecraft independent navigation This theory and a basic technology difficult problem seek new way, to improve the fight capability of military spacecraft further.

Summary of the invention

It is an object of the invention to a kind of SINS/SAR/CNS independent combined navigation system based on the most optimum correction, profit Defeated to inertial navigation system, synthetic aperture radar and celestial navigation system by highly-precise filtering technology and multisource information fusion technology The attitude, the positional information that go out carry out information processing and fusion, and then carry out navigation error optimally estimating comprehensively, correcting, with Improve the precision of integrated navigation system.

For achieving the above object, technical solution of the present invention is as follows:

A kind of independent combined navigation system, in SINS/SAR/CNS Design of Integrated Navigation System, owing to SINS can be complete Weather, offer 3 d pose, speed and the positional information of round-the-clock, and good concealment, capacity of resisting disturbance is strong, therefore, by SINS As principle navigation system, SAR, CNS, as secondary navigation system, constitute SINS/SAR/CNS integrated navigation system.The steps include:

First, design SINS/SAR and SINS/CNS navigation subfilter, obtain integrated navigation system state by calculating Two groups of local optimum estimated valuesWith local Optimal error covariance matrixThen, use federated filter technology, Two groups of local optimum estimated values are sent in senior filter and carries out information fusion, obtain global optimum's estimated value of system mode With global optimum's error covariance matrix；Finally, global optimum's estimated value of the state obtained is utilizedTo inertial navigation system The error of system carries out real time correction.All can not export the elevation information of carrier due to CNS and SAR, therefore, native system uses air pressure SINS altitude channel is corrected, with the problem suppressing SINS altitude channel to dissipate by the elevation information of altimeter output carrier. SINS/SAR/CNS integrated navigation system optimal estimation fusion algorithm is

$\left\{\begin{array}{c}{\mathrm{\Σ}}_{\hat{X},g}={({\mathrm{\Σ}}_{\hat{X},1}^{-1}+{\mathrm{\Σ}}_{\hat{X},2}^{-1})}^{-1}\\ {\hat{X}}_g={\mathrm{\Σ}}_{\hat{X},g}({\mathrm{\Σ}}_{\hat{X},1}^{-1}{\hat{X}}_1+{\mathrm{\Σ}}_{\hat{X},2}^{-1}{\hat{X}}_2)\end{array}---(1-1)\right..$

Independent combined navigation system, arranges SINS/SAR/CNS integrated navigation system mathematical model as follows:

(1) state equation: in SINS/SAR/CNS integrated navigation system, owing to the positioning precision of SAR and CNS is high, by mistake The difference error much smaller than SINS, and accumulate the most in time.Therefore, in order to reduce system dimension, by the navigation error of SAR and CNS It is thought of as white Gaussian noise, and is not classified as the quantity of state of integrated navigation system, only the systematic error of SINS is thought of as SINS/ The system state amount of SAR/CNS integrated navigation system.

Choose sky, northeast (E-N-U) geographic coordinate system g, as navigational coordinate system n of integrated navigation system, to lead according to inertia The error equation of boat system, state x (t) of SINS/SAR/CNS integrated navigation system is chosen for

$x\left(t\right)={[{\left(\mathrm{\δq}\right)}^T,{\left(\mathrm{\δV}\right)}^T,{\left(\mathrm{\δP}\right)}^T,{\mathrm{\ϵ}}^T,{\▿}^T]}^T---\left(1\right)$

In formula, δ q=[δ_q0、δ_q1、δ_q2、δ_q3]^TAttitude error quaternary number for SINS；δV=[δV_E、δV_N、δV_U]^TFor SINS East, north, the velocity error in direction, sky；δP=[δL、δλ、δh]^TLatitude, longitude and altitude error for SINS；ε=[ε_x、ε_y、ε_x]^T Represent the random drift of gyro；It it is the constant value biasing of accelerometer.

The state equation that can obtain SINS/SAR/CNS integrated navigation system according to formula (1) is

$\stackrel{\·}{x}=f(x,t)+G\left(t\right)w\left(t\right);---\left(2\right)$

In formula, (x t) is the state array of system to f；w(t)=[w_gx, w_gy, w_gz, w_ax, w_ay, w_az]^TRepresent system noise, [w_gx、w_gy、w_gz] it is the white noise of gyro, [w_ax、w_ay、w_az] it is the white noise of accelerometer；G (t) is that the noise of system drives Matrix, and system mode battle array and noise driving battle array are respectively

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\ω}}_{\mathrm{in}}^n-{\mathrm{BC}}_b^c{\mathrm{\ϵ}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×{\mathrm{\δV}}^n-(2\mathrm{\δ}{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\δ\ω}}_{\mathrm{en}}^n)\×V^n+C_b^c{\▿}^b\\ \mathrm{M\δV}+\mathrm{N\δL}\\ 0_{3\×1}\\ 0_{3\×1}\end{array}\right]---\left(3\right)$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\×3}\\ 0_{3\×3}& C_b^c\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\\ 0_{3\×3}& 0_{3\×3}\end{array}\right]---\left(4\right)$

(2) measurement equation:

1. SINS/SAR subsystem measurement equation: synthetic aperture radar passes through images match, it is possible to obtain the level of carrier Positional information and course angle information, strapdown inertial navigation system is by the angular movement information exporting gyroscope and acceleration and line Movable information resolves, it is possible to obtain the attitude of carrier, speed and positional information.Owing to SAR can not obtain the height of carrier Information, in order to suppress SINS altitude channel to dissipate, uses pressure altimeter to measure the elevation information of carrier.Therefore, may be used With the course angle information that inertial navigation is exported and positional information and the course angle information of synthetic aperture radar output, horizontal level The difference of the carrier height information of information and pressure altimeter output is as measurement, the measurement of SINS/SAR integrated navigation system Equation is

z₁(t)=h₁(x, t)+v₁(t) (9)

In formula, h₁(x t) is the nonlinear function of measurement equation, v (t)=[δ ψ_S, δ L_S, δ λ_S, δ h_e]^TFor measuring white noise, Average is zero.

2. SINS/CNS subsystem measurement equation: in SINS/CNS algorithm of combined navigation subsystem, inertial navigation system can Obtain attitude quaternion and the positional information of carrier.Celestial body is observed obtaining by celestial navigation system by star sensor The attitude quaternion of carrier and the horizontal position information (longitude and latitude) of carrier, but the elevation information of carrier can not be obtained, Being observed the elevation information of carrier it is thus desirable to introduce height air gauge, suppression SINS altitude channel dissipates.Choose inertia The attitude of carrier quaternary number of navigation system output and the attitude of carrier quaternary number of positional information and celestial navigation system output and water Mean place information, and the difference of the carrier height information of height air gauge output is as measurement, SINS/CNS integrated navigation The measurement equation of system is

z₂(t)=h₂(t) x (t)+v,₂(t) (11)

In formula, h₂T () is measurement matrix, v₂(t)=[δq_C0, δ q_C1,δq_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TFor measuring white noise, Average is zero.

(3) independent navigation high precision nonlinear filtering algorithm: devise a set of SINS/CNS/SAR of being suitable for and independently combine The high accuracy of navigation system, nonlinear filtering algorithm, this set algorithm includes:

1. robust self adaptation Unscented particle filter algorithm；2. self adaptation Unscented that fades particle filter；3. mould Stick with paste robust adaptive particle filter；4. self adaptation SVD-UKF filtering algorithm；5. self adaptation square root Unscented particle filter. Specific as follows:

1. robust self adaptation Unscented particle filter algorithm:

Robust self adaptation Unscented particle filter, has fully absorbed Robust filter, robust adaptive-filtering and particle filter The advantage of ripple, is combined Robust filter principle with UPF by the equivalence weight factor and adaptive factor, selects suitable power letter Number and adaptive factor controlled state model information and measurement model information, the impact of the abnormal interference of suppression.

The step of robust self adaptation Unscented particle filter algorithm is as follows:

A () initializes, extract N number of particle according to initial mean value and mean square deviation, in the k=0 moment,I=1, 2 ..., N, arranging weights is

(b) at k=1,2 ..., n-hour, calculate according to following order:

(b1) equivalence weight is calculatedWith adaptive factor α.Selecting IGG scheme structure Modified Equivalent Weight Function, IGG is owned by France in fall power Function, i.e. does robust and limits measuring value, if taking its inverse, is then defined as variance inflation factor function.

If equivalent weight matrix is $\stackrel{\‾}{P}=\mathrm{diag}({\stackrel{\‾}{P}}_1,{\stackrel{\‾}{P}}_2,...,{\stackrel{\‾}{P}}_k),$

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.---\left(12\right)$

Another kind of expression formula can also be used as required

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(13\right)$

Wherein k₀∈ (1,1.5), k₁∈ (3,8), V_kFor observation l_kResidual vector, For time current Carve state estimation value.Adaptive factor is chosen as follows

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.---\left(14\right)$

Wherein c₀∈ (1,1.5), c₁∈ (3,8),The mark of tr () representing matrix,For forecast Value, i.e.Can be seen that weight function and adaptive factor structural form base This is identical, and both of which is important regulatory factor.The former is by choosing the judgement of residual error, and the latter is according to state estimation Difference with predicted valueChoose.

(b2) Sigma point is calculated, by UKF algorithm more new particleObtainAndMeetIf new samples is2N+1 Sigma point sampling point is

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(15\right)$

λ=α in formula²(n+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines sampled point To prediction average degree of scatter, β be typically based on priori come value (value optimal for Gauss distribution be (b2), W_jTable Show the weights of jth Sigma point, meet ∑ W_j=1, j=0,1 ... 2N.

C () calculates weights $w_k^i=w_{k-1}^i\frac{p\left(y_k\right|{\stackrel{\&OverBar;}{x}}_k^{i^{*}})p\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}})}{q\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}},l_k)},$ And be normalized to ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

D () calculates estimator ${\hat{N}}_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\tilde{w}}_k^i\right)}^2.$

Acquired results is compared with set threshold, it is judged that the order of severity of sample degeneracy,The least, show to degenerate The most serious.In this case, previously obtained posterior density can be carried out resampling, retrieve M new particle, and compose Give each particle identical weights 1/M.

E () calculates nonlinear state amount estimated value.Repeat previous step (b).

Above-mentioned steps, when choosing the importance density function, make use of two important regulatory factors, i.e. the equivalence weight factor And adaptive factor.After being converted UT by both, gained particle sampler point more reasonably distributes useful information, adopts for importance Sample process provides more preferable sample distribution function.

2. self adaptation Unscented that fades particle filter:

The self adaptation Unscented particle filter that fades improved is with particle filter as basic framework, and fusion fades adaptive Answer filtering principle and UT conversion process, absorb each single algorithm advantage, set up parameter and can the importance density of Automatic adjusument divide Cloth function, utilizes up-to-date measurement information the most efficiently so that it is closer to real distribution function, so that filtering algorithm has Preferably adaptivity and robustness.

Unscented particle filter algorithm mainly utilizes UT conversion to obtain sampled point, it is achieved to state vector Posterior distrbutionp Approximation.Different from DSMC, Unscented particle filter is not random sampling from given distribution, is Take Sigma point that minority determines as sampled point.Sigma sampled point is

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(20\right)$

λ=α in formula²(N+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines sampled point Degree of scatter to prediction average.For different system modeies and noise it is assumed that UT mapping algorithm has different forms, certainly Determine UT mapping algorithm expression formula it is crucial that determine Sigma point sampling strategy, i.e. Sigma point number, position and weights.

Self adaptation Unscented that fades particle filter algorithm uses the memory span of fading factor restriction filter, fully Current observation value is utilized constantly to revise predictive value, and to the unknown or inapt system model parameter, noise statistics parameter etc. Carry out estimating and revising.Algorithm mainly comprises the following steps:

A () initializes, during k=0,Wherein k represents the moment.Unification arranges weights and isWherein k Representing the moment, N represents particle number.

B () calculates Sigma point, if new samples isCalculate 2N+1 Sigma sampled point, utilize UKF algorithm that particle is entered Row prediction and renewal,

Ibid, in following formula, β is typically based on priori and carrys out value (value optimal for Gauss distribution the most each symbolic significance It is 2), W_jRepresent the weights of jth Sigma point, meet ∑ W_j=1, j=0,1 ... 2N, and carry out time renewal and measurement updaue. Utilization fade adaptive extended kalman filtering thought calculate fading factor, utilize formula α_k, and calculate

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i)---\left(31\right)$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T---\left(32\right)$

ObtainAs the importance density function of particle sampler, carry out importance and adopt Sample.

C () is from importance density letterIn sample after, and to each particle calculate weights

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(33\right)$

And calculate normalization weights.

${\hat{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i---\left(34\right)$

D () utilizes estimatorJudge that sample degeneracy degree is the most serious, the most obtained from the above Posterior density carries out resampling and retrieves M new particle, again gives each particle identical weights

E () calculates nonlinear state amount estimated value.

${\tilde{x}}_k=\underset{i=0}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i{x}_k^{i**}---\left(35\right)$

Return (2nd) step, by the state estimation of new observed quantity recursive calculation subsequent time.

3. robust adaptive particle filter is obscured:

(I) equivalence weight is constructed based on fuzzy theory: have again estimating of higher adaptivity to obtain existing stronger Robustness least squares Value, can be divided into power according to data residual error: observation (is made robust limit by Bao Quan district (keep former observation constant), Jiang Quan district System) and region of rejection (Quan Weiling).Designing one-dimensional fuzzy controller, the step of structure weight factor γ is as follows:

(a) obfuscation.The effect of obfuscation is that the precise volume of input is converted into obfuscation amount, will input quantity(accurately Value) obfuscation becomes fuzzy variable (whereinFor standardized residual), the input determined is converted into the mould described by degree of membership Stick with paste collection.

Detailed process: by input variableFuzzy subset be divided into { too big, greatly, more normal }, be abbreviated as { B_e, B_c, B_n, After input quantifies,Domain be X={0,1,2,3,4}；The fuzzy subset of output γ be { minimum, less, normally }, is abbreviated as {S_e, S_c, S_n, its domain is that Y size is divided into 5 grades, to represent different value, i.e. and Y={0,1,2,3,4}.Respectively to inputFuzzy quantization is carried out with output γ.

(b) according to the intuitive thinking reasoning of people and practical experience, by input quantityWith the relation of output weight factor γ, if Meter fuzzy control rule.IfToo big, then γ is minimum；IfRelatively big, then γ is less；IfNormally, then γ is normal.

May determine that fuzzy relation is according to fuzzy rule

R=(B_e×S_e)+(B_c×S_c)+(B_n×S_n)

Wherein, "×" represents the cartesian product of fuzzy vector.It is computed

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right]$

(c) according to fuzzy control principle, by input variableFuzzy subset and fuzzy relationship matrix r pass through fuzzy reasoning Obtain the fuzzy set that weight factor is γ, draw final fuzzy control quantity.

D this fuzzy control quantity is carried out ambiguity solution and is exported controlled quentity controlled variable, i.e. weight factor γ accurately by ().Fuzzy reasoning is tied Fruit is converted into the process of exact value and is referred to as anti fuzzy method.When anti fuzzy method processing procedure, use maximum membership grade principle.

(II) fuzzy robust adaptive particle filter algorithm

Fuzzy robust adaptive particle filter (Fuzzy Robust Adaptively Particle Filter, FRAPF) The step of algorithm is as follows:

A () initializes.In the k=0 moment, sample out N number of particle according to emphasis density, it is assumed that each particle sampled out is usedRepresent, make k=1；

(b) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and adaptive factor.Residual to predict Difference for the differentiation statistic of structure's variable state model error is

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(40\right)$

Wherein,For k moment i-th status prediction information, pass throughObtain；Represent that prediction is residual DifferenceCovariance matrix；Tr () is Matrix Calculating trace operator.The adaptive factor function of statistic is differentiated based on prediction residual For

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 < c < 2.5.

C () updates:

$x_k^i=f(x_{k-1}^i,v_{k-1})---\left(42\right)$

ByThe particle in k moment is updated according to formula (42)

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)})---\left(43\right)$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})}---\left(44\right)$

Renewal weights and normalization weights, i=1,2 ..., N.

D () resampling: be ranked up the weights of all particles according to descending, arranging thresholding sample points is N_th(generally It is chosen as N/2 or N/3), effective sample is counted as N_eff, work as N_eff<N_thTime, to particle collectionResampling, obtains new Particle collectionAnd reset weights and be

E () filters

$p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i)---\left(45\right)$

Calculate the filtering density in k moment and carry out re-sampling, making k=k+1, returning (b).

The adaptive factor α of robust adaptive-filtering_kNeither individual processing model error covariance matrixThe most not single Reason of the staying alone covariance matrix that previous epoch, state parameter vector was estimatedBut act on the state parameter forecast of entirety The equivalent covariance matrix of vectorOwing to robust adaptive-filtering have employed Robust filter principle to observation information, work as sight When surveying existence exception, using dynamic model information as an entirety, unified adaptive factor motivation of adjustment model is used to believe The breath overall contribution to state parameter, thus obtain reliable filter result.

4. self adaptation SVD-UKF filtering algorithm:

(I) singular value decomposition: singular value decomposition (SVD) is preferably a kind of square of stability and precision during numerical algebra calculates Battle array decomposition method, it is easy to realize on computers.It is defined as follows.

Assuming that A ∈ R^m×n(m >=n), then the singular value decomposition of matrix A is represented by

$A={\mathrm{UAV}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T---\left(46\right)$

In formula (46), U ∈ R^m×m, Λ ∈ R^m×n, V ∈ R^n×n, S=diag (s₁,s₂..., s_r)。s₁≥s₂≥…≥s_r≥0 Being referred to as the singular value of matrix A, the column vector of U and V is called the left and right singular vector of matrix A.

If A^TA positive definite, then formula (46) can be reduced to

$A=U\left[\begin{array}{c}S\\ 0\end{array}\right]V^T---\left(47\right)$

If A symmetric positive definite, then A=USU^T, the most left singular vector is equal with right singular vector, calculates such that it is able to reduce Amount.

(II) statistic and the determination of adaptive factor: with prediction residual as variable, the Error subtraction scheme of structural regime model Statistic and adaptive factor.Prediction residual (newly breath), containing the state without observation information correction, more can reflect dynamical system The disturbance being subject to.Prediction residualIt is expressed as

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{x}}_k\right)-y_k---\left(48\right)$

In formula (48),For k moment status prediction information.Use prediction residualStructural regime model error differentiates statistic As follows.

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

In formula (49),Represent prediction residualCovariance matrix, tr () is Matrix Calculating trace operator.

Adaptive factor function is elected as

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.---\left(50\right)$

In formula (50), α_kRepresenting adaptive factor, c is empirical, 1 < c < 2.5 under normal circumstances.

(III) self adaptation SVD-UKF algorithm steps:

Self adaptation SVD-UKF algorithm steps is as follows:

A () initializes: to the parameter initialization in state equation, and calculate the weight system of Sigma point average and covariance Number w^(m)、w^(c)。

(b) singular value decomposition, calculating Sigma point vector:

C () time updates:

D () measures and updates；

5. self adaptation square root Unscented particle filter

If system dynamics equation is

X_k=Φ_k,k-1X_k-1+W_k (67)

In formula, X_kIt is that k moment n ties up state vector, Φ_{K, k-1}For n × n-state transfer matrix, W_kVectorial for system noise, its Covariance matrix is

Observational equation is

Y_k=H_kX_k+e_k (68)

In formula, Y_kIt is that k moment m ties up observation vector, H_kDesign matrix, e is tieed up for m × n_kFor observation noise vector, its covariance Battle array is ∑_k。W_k, W_j, e_k, e_j(j ≠ k) is orthogonal.

The probability density function of known state is p (X₀|Y₀)=p(X₀), then theoretical according to Bayesian Estimation, non-linear The status predication equation of time-varying system and state renewal equation are respectively

p(X_k|Y_1:k-1)=∫p(X_k|X_k-1)p(X_k-1|Y_1:k-1)dX_k-1 (69)

$p\left(X_k\right|Y_{1:k})=\frac{p\left(Y_k\right|X_k)p\left(X_k\right|Y_{1:k-1})}{p\left(Y_k\right|Y_{1:k-1})}---\left(70\right)$

In formula, p (X_k|X_k-1) it is state transfering density, p (X_k-1|Y_1:k-1) it is the posterior density in k-1 moment；p(X_k| Y_1:k-1) it is prior distribution, p (Y_k|X_k) it is likelihood density, p (Y_k|Y_1:k-1) it is normaliztion constant, can be tried to achieve by following formula

p(Y_k|Y_1:k-1)=∫p(Y_k|X_k)p(X_k|Y_1:k-1)dX_k (71)

Formula (69)～(71) constitute recursion Bayesian Estimation.Formula (71) only can obtain analytic solutions to some dynamical system. Integral operation can be converted into the summation operation of finite sample point by Monte Carlo method based on stochastical sampling, after Test the approximate expression form of probability density function.Posterior density p (X in reality_k|Y_1:k) it is probably multivariate, non-standard probability Distribution, needs to sample it by importance sampling algorithm, so that structure importance function.Select appropriate importance letter Number can effectively solve the sample degeneracy problem of particle filter.

This project uses UKF algorithm to produce the importance density function of particle filter, and this algorithm makes full use of up-to-date sight Survey data, the error that correction kinetic model and noise statistics parameter cause in real time.The step of self adaptation square root UPF is as follows.

A () initializes (k=0): randomly draw N number of primary(i=1,2 ..., N).Assume ${\hat{X}}_0^i=E\left[X_0^i\right],S_0^i=\mathrm{chol}\left\{E\right[(X_0^i-{\hat{X}}_0^i){(X_0^i-{\hat{X}}_0^i)}^T\left]\right\},w_0^i=p\left(Y_0\right|X_0^i).$ Wherein,WithRepresent respectively Initial time i-th particle and estimated value thereof,Represent initial time i-th Cholesky factoring,Represent i-th grain The initialization weights of son, chol{ } the Cholesky decomposition operator of representing matrix.

B () uses self adaptation Deep space algorithm to update each particleObtain Represent the covariance of k moment i-th particle.

(b1) Sigma point and weights are calculated；

(b2) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and adaptive factor.

Prediction residual (or newly breath), containing the state without observation information correction, more can reflect the disturbance of dynamical system.During k Carve i-th prediction residual vectorIt is expressed as

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(74\right)$

With the differentiation statistic that prediction residual is structure's variable state model error it is

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(75\right)$

Wherein,For k moment i-th status prediction information, pass throughObtain,Represent prediction Residual errorCovariance matrix, tr () is Matrix Calculating trace operator.

Adaptive factor function based on prediction residual differentiation statistic is

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(76\right)$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 < c < 2.5.

(b3) time updates (status predication)；

(b4) measurement updaue (state estimation)；

Employ the QR decomposition in linear algebra in this step, Cholesky decomposes, with the shape of Cholesky factoring State covariance matrix is directly propagated and updated to formula, thus enhances the numerical stability in state covariance matrix renewal process Property, it is ensured that the orthotropicity of covariance matrix.Wherein, qr{ } representing matrix QR decompose.

(c) importance sampling weight computing: make significance distribution functionSampling ParticleN () represents normal distribution.Pass through respectively $w_k^i=w_{k-1}^i\frac{p(Y_k/X_k^i)p(X_k^i/X_{k-1}^i)}{q\left({\stackrel{\&OverBar;}{X}}_k^i\right|{\stackrel{\&OverBar;}{X}}_{0:k-1}^i,Y_{1:k})},$ Renewal weights and normalization weights, i=1 ..., N.

(d) resampling: arranging thresholding sample points is N_th(being generally chosen as N/2 or N/3), effective sample is counted as N_eff, Work as N_eff<N_thTime, to particle collectionResampling, obtains new particle collectionAnd reset Weights are

E () state updates:

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{X}}_k^i{\tilde{w}}_k^i---\left(93\right)$

${\widehat{\mathrm{\&Sigma;}}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i({\hat{X}}_k-{\tilde{X}}_k^i){({\hat{X}}_k-{\tilde{X}}_k^i)}^T---\left(94\right)$

In SINS/SAR navigation subsystem, strapdown inertial navigation system utilizes gyro and accelerometer to transport the line of carrier Dynamic and angular movement measures, it is thus achieved that the angle increment of carrier and specific force increment, then utilizes 3 d pose update algorithm, speed more New algorithm and location updating algorithm carry out navigation and calculate, it is possible to obtain 3 d pose information, velocity information and the position that carrier is real-time Confidence ceases.Synthetic aperture radar passes through Range-Doppler Imaging principle, can obtain the high-resolution SAR in target area figure in real time Picture, then uses the image matching algorithm of feature based to carry out images match with the reference map in airborne digital map storehouse, permissible Obtain course angle and the horizontal position information of carrier.Owing to synthetic aperture radar cannot obtain the elevation information of carrier, and strapdown The altitude channel of inertial navigation system dissipates, and therefore uses pressure altimeter to measure the real-time height of carrier, to solve strapdown The problem that inertial navigation altitude channel is the most considerable.The method using filtering indirectly, the course angle that SINS is exported and position The difference of the elevation information of information and the course angle of synthetic aperture radar output and horizontal position information and pressure altimeter output Value, as measurement, is then fed in wave filter carrying out nonlinear filtering, it is thus achieved that the optimal estimation value of integrated navigation system state, Recycle this estimated value the error of strapdown inertial navigation system is modified.Utilize revised SINS navigation information pair simultaneously Synthetic aperture radar carries out motion compensation, to improve the image quality of SAR.

In SINS/CNS Design of Integrated Navigation System, inertia device can export carrier angular movement information and line motion letter Breath, utilizes the navigation computation of inertial navigation to resolve these information, it is possible to obtain the 3 d pose letter that carrier is real-time Breath, velocity information and positional information.Star sensor in celestial navigation system can export and be observed the right ascension of star, declination and rotation These information are carried out resolving horizontal position information and the attitude information that can obtain carrier by angle.Due to celestial navigation system not The elevation information of carrier can be obtained, and the altitude channel of strapdown inertial navigation system dissipates, therefore, use pressure altimeter Measure the height of carrier, the altitude channel of strapdown inertial navigation system is corrected.

Owing to the star sensor of celestial navigation system is connected on carrier, ignore alignment error, then star sensor coordinate system Overlap with carrier coordinate.Altitude of the heavenly body angle that star sensor observes and azimuth, by calculating the starlight that can obtain celestial body Unit direction vector, utilizes attitude algorithm algorithm can calculate the carrier system attitude matrix relative to inertial systemFurther according toCarrier can be tried to achieve and be tied to the pose transformation matrix of navigation system, thus obtain celestial navigation system and be calculated Carrier system b be the attitude quaternion of n to navigation.The mathematics that the unit direction vector that star sensor obtains is provided by SINS is put down Stage fiducialCarry out coordinate transform, then utilize the altitude difference method of celestial navigation system, the longitude λ of carrier can be calculated_cnsWith Latitude L_cnsInformation.The navigation information q that inertial navigation system is exported_sins, L_sins, λ_sins, h_sinsExport with celestial navigation system Q_cns, L_cns, λ_cnsCarrier height information h that information and pressure altimeter record_eDiffer from, deliver in SINS/CNS wave filter It is filtered calculating, it is possible to obtain the optimal estimation value of state.Finally, by the optimal estimation value of state to SINS Navigational parameter error is corrected, and makes SINS provide more accurate mathematical platform benchmark for celestial navigation system.

The beneficial effects of the present invention is: it is autonomous that the present invention proposes a kind of SINS/SAR/CNS based on the most optimum correction Integrated navigation system, can apply to be unsatisfactory for the Altitude Long Endurance Unmanned Air Vehicle of dynamics of orbits characteristic, it is achieved that roll over based on starlight The parsing astrofix penetrated；Utilize highly-precise filtering algorithm and information fusion technology to inertial navigation system, synthetic aperture radar Carry out information processing and fusion with attitude, the positional information of celestial navigation system output, and then navigation error is carried out Estimate excellently, correct, improve the precision of integrated navigation system；Have that amount of calculation is little, high reliability, and also permissible It is applied to the aircraft such as near space vehicle, empty sky shuttle vehicle, ballistic missile, change rail spacecraft, there is wide answering Use prospect.

Accompanying drawing explanation

Fig. 1 is SINS/SAR integrated navigation system schematic diagram in this invention.

Fig. 2 is SINS/CNS algorithm of combined navigation subsystem principle in this invention.

Fig. 3 is SINS/SAR/CNS integrated navigation schematic diagram in this invention.

Detailed description of the invention

The detailed description of the invention of this invention is expanded on further below in conjunction with the accompanying drawings.

Embodiment

SINS/SAR integrated navigation schematic diagram is as shown in Figure 1.Fig. 2 is that in this invention, SINS/CNS algorithm of combined navigation subsystem is former Reason.Fig. 3 is SINS/SAR/CNS integrated navigation schematic diagram in this invention.

SINS/SAR/CNS integrated navigation system mathematical model is as follows:

(1) state equation: in SINS/SAR/CNS integrated navigation system, owing to the positioning precision of SAR and CNS is high, by mistake The difference error much smaller than SINS, and accumulate the most in time.Therefore, in order to reduce system dimension, by the navigation error of SAR and CNS It is thought of as white Gaussian noise, and is not classified as the quantity of state of integrated navigation system, only the systematic error of SINS is thought of as SINS/ The system state amount of SAR/CNS integrated navigation system.

Choose sky, northeast (E-N-U) geographic coordinate system g, as navigational coordinate system n of integrated navigation system, to lead according to inertia The error equation of boat system, state x (t) of SINS/SAR/CNS integrated navigation system is chosen for

$x\left(t\right)={[{\left(\mathrm{\&delta;q}\right)}^T,{\left(\mathrm{\&delta;V}\right)}^T,{\left(\mathrm{\&delta;P}\right)}^T,{\mathrm{\&epsiv;}}^T,{\&dtri;}^T]}^T---\left(1\right)$

In formula, δ q=[δ q₀、δq₁、δq₂、δq₃]^TAttitude error quaternary number for SINS；δV=[δV_E、δV_N、δV_U]^TFor SINS east, north, the velocity error in direction, sky；δP=[δL、δλ、δh]^TLatitude, longitude and altitude error for SINS；ε=[ε_x、ε_y、 ε_x]^TRepresent the random drift of gyro；It it is the constant value biasing of accelerometer.

The state equation that can obtain SINS/SAR/CNS integrated navigation system according to formula (1) is

$\stackrel{\&CenterDot;}{x}=f(x,t)+G\left(t\right)w\left(t\right)---\left(2\right)$

In formula, (x t) is the state array of system to f；W (t)=[w_gx, w_gy, w_gz, w_ax, w_ay, w_az]^TRepresent system noise, [w_gx、w_gy、w_gz] it is the white noise of gyro, [w_ax、w_ay、w_az] it is the white noise of accelerometer；G (t) is that the noise of system drives Matrix, and system mode battle array and noise driving battle array are respectively

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\&omega;}}_{\mathrm{in}}^n-{\mathrm{BC}}_b^c{\mathrm{\&epsiv;}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&omega;}}_{\mathrm{en}}^n)\&times;{\mathrm{\&delta;V}}^n-(2\mathrm{\&delta;}{\mathrm{\&omega;}}_{\mathrm{ie}}^n+{\mathrm{\&delta;\&omega;}}_{\mathrm{en}}^n)\&times;V^n+C_b^c{\&dtri;}^b\\ \mathrm{M\&delta;V}+\mathrm{N\&delta;L}\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right]---\left(3\right)$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^c\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right]---\left(4\right)$

(2) measurement equation:

1. SINS/SAR subsystem measurement equation: synthetic aperture radar passes through images match, it is possible to obtain the level of carrier Positional information and course angle information, strapdown inertial navigation system is by the angular movement information exporting gyroscope and acceleration and line Movable information resolves, it is possible to obtain the attitude of carrier, speed and positional information.Owing to SAR can not obtain the height of carrier Information, in order to suppress SINS altitude channel to dissipate, uses pressure altimeter to measure the elevation information of carrier.Therefore, may be used With the course angle information that inertial navigation is exported and positional information and the course angle information of synthetic aperture radar output, horizontal level The difference of the carrier height information of information and pressure altimeter output is as measurement, then measurement can be expressed as

$z_1\left(t\right)=\left[\begin{array}{c}{\mathrm{\&psi;}}_I-{\mathrm{\&psi;}}_S\\ L_I-L_S\\ {\mathrm{\&lambda;}}_I-{\mathrm{\&lambda;}}_S\\ h_I-h_e\end{array}\right]=\left[\begin{array}{c}(\mathrm{\&psi;}+{\mathrm{\&delta;\&psi;}}_I)-(\mathrm{\&psi;}+{\mathrm{\&delta;\&psi;}}_S)\\ (L+{\mathrm{\&delta;L}}_I)-(L+{\mathrm{\&delta;L}}_S)\\ (\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_I)-(\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_S)\\ (h+{\mathrm{\&delta;h}}_I)-(h+{\mathrm{\&delta;h}}_e)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;\&psi;}}_I\\ {\mathrm{\&delta;L}}_I\\ {\mathrm{\&delta;\&lambda;}}_I\\ {\mathrm{\&delta;h}}_I\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&delta;\&psi;}}_S\\ {\mathrm{\&delta;L}}_S\\ {\mathrm{\&delta;\&lambda;}}_S\\ {\mathrm{\&delta;h}}_e\end{array}\right]---\left(5\right)$

In formula, ψ_I、L_I、λ_IAnd h_IIt is respectively the course angle of SINS output, latitude, longitude and altitude information, ψ_S、L_SAnd λ_SPoint Not for course angle, latitude and the longitude information of SAR output, h_eFor the elevation information of pressure altimeter output, δ represents every Corresponding error.

Attitude error angle (the course error angle δ ψ of Eulerian angles form_I, pitch error angle δ θ_IWith roll error angle δ γ_I) retouch State be preferably navigation be the navigation that n (i.e. preferably platform system T) calculates to strapdown inertial navigation system navigational computer It it is the error between c (i.e. actual platform system P).The navigation calculated by navigational computer is that c passes through to rotate, and can obtain c It is tied to the transition matrix of n systemFor

$\begin{array}{c}{C}_{c1}^n=\left[\begin{array}{cc}\cos \mathrm{\&delta;\&lambda;}\cos \mathrm{\&delta;\&psi;}+\sin \mathrm{\&delta;\&lambda;}\sin \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}& \sin \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\\ -\cos \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}+\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}& \cos \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\\ -\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&theta;}& \sin \mathrm{\&delta;\&theta;}\end{array}\right.\\ \begin{array}{c}\sin \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}-\cos \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}\\ -\sin \mathrm{\&delta;\&gamma;}\sin \mathrm{\&delta;\&psi;}-\cos \mathrm{\&delta;\&gamma;}\cos \mathrm{\&delta;\&psi;}\sin \mathrm{\&delta;\&theta;}\\ \cos \mathrm{\&delta;\&psi;}\cos \mathrm{\&delta;\&theta;}\end{array}\end{array}---\left(6\right)$

When use attitude quaternion error angle represent preferable navigation be n and calculate navigation be the error angle between c time, if appearance State error quaternion is δ q=[δ q₀, δ q₁, δ q₂, δ q₃]^T, then the c represented with attitude error quaternary number is tied to the transition matrix of n systemFor

$C_{c2}^n=\left[\begin{array}{ccc}{\mathrm{\&delta;q}}_0^2+{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2& 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)& 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2)\\ 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)& {\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2+{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2& 2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)\\ 2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2)& 2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)& {\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2+{\mathrm{\&delta;q}}_3^2\end{array}\right]---\left(7\right)$

Due to matrixWith matrixAll describe is that the calculated navigation of navigational computer is c and preferable navigation is n Between transition matrix, therefore formula (6) is equal with formula (7), and peer-to-peer carries out can be calculated the attitude error angle of Eulerian angles form And the transformational relation between the attitude error angle of quaternary number form formula is

$\left\{\begin{array}{c}\mathrm{\&delta;\&psi;}=\arctan \left(\frac{2({\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_3)}{{\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2+{\mathrm{\&delta;q}}_2^2-{\mathrm{\&delta;q}}_3^2}\right)\\ \mathrm{\&delta;\&theta;}=\arcsin \left(2({\mathrm{\&delta;q}}_2{\mathrm{\&delta;q}}_3+{\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_1)\right)\\ \mathrm{\&delta;\&gamma;}=\arctan \left(\frac{2({\mathrm{\&delta;q}}_0{\mathrm{\&delta;q}}_2-{\mathrm{\&delta;q}}_1{\mathrm{\&delta;q}}_3)}{{\mathrm{\&delta;q}}_0^2-{\mathrm{\&delta;q}}_1^2-{\mathrm{\&delta;q}}_2^2+{\mathrm{\&delta;q}}_3^2}\right)\end{array}\right.---\left(8\right)$

According to formula (5) and formula (8), the measurement equation that can obtain SINS/SAR integrated navigation system is

z₁(t)=h₁(x, t)+v₁(t) (9)

In formula, h₁(x t) is the nonlinear function of measurement equation, v (t)=[δ ψ_S, δ L_S, δ λ_S, δ h_e]^TFor measuring white noise, Average is zero.

2. SINS/CNS subsystem measurement equation: in SINS/CNS algorithm of combined navigation subsystem, inertial navigation system can Obtain attitude quaternion and the positional information of carrier.Celestial body is observed obtaining by celestial navigation system by star sensor To attitude quaternion and the horizontal position information (longitude and latitude) of carrier of carrier, but the height letter of carrier can not be obtained Breath, it is therefore desirable to introducing height air gauge and be observed the elevation information of carrier, suppression SINS altitude channel dissipates.Choose used Property the attitude of carrier quaternary number of navigation system output and the attitude of carrier quaternary number of positional information and celestial navigation system output and Horizontal position information, and the difference of the carrier height information of height air gauge output is as measurement, then measurement can represent For

$z_2\left(t\right)=\left[\begin{array}{cc}{q}_{I0}& q_{C0}\\ q_{I1}& q_{C1}\\ q_{I2}& q_{C2}\\ q_{I3}& -q_{C3}\\ L_I& L_C\\ {\mathrm{\&lambda;}}_I& {\mathrm{\&lambda;}}_C\\ h_I& h_e\end{array}\right]=\left[\begin{array}{c}(q_0+{\mathrm{\&delta;q}}_{I0})-(q_0+{\mathrm{\&delta;q}}_{C0})\\ (q_1+{\mathrm{\&delta;q}}_{I1})-(q_1+{\mathrm{\&delta;q}}_{C1})\\ (q_2+{\mathrm{\&delta;q}}_{I2})-(q_2+{\mathrm{\&delta;q}}_{C2})\\ (q_3+{\mathrm{\&delta;q}}_{I3})-(q_3+{\mathrm{\&delta;q}}_{C3})\\ (L+{\mathrm{\&delta;L}}_1)-(L+{\mathrm{\&delta;L}}_C)\\ (\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_I)-(\mathrm{\&lambda;}+{\mathrm{\&delta;\&lambda;}}_C)\\ (h+{\mathrm{\&delta;h}}_I)-(h+{\mathrm{\&delta;h}}_e)\end{array}\right]=\left[\begin{array}{c}{\mathrm{\&delta;q}}_{I0}\\ {\mathrm{\&delta;q}}_{I1}\\ {\mathrm{\&delta;q}}_{I2}\\ {\mathrm{\&delta;q}}_{I3}\\ {\mathrm{\&delta;L}}_I\\ \mathrm{\&delta;}{\mathrm{\&lambda;}}_I\\ {\mathrm{\&delta;h}}_I\end{array}\right]-\left[\begin{array}{c}{\mathrm{\&delta;q}}_{C0}\\ {\mathrm{\&delta;q}}_{C1}\\ {\mathrm{\&delta;q}}_{C2}\\ {\mathrm{\&delta;q}}_{C3}\\ {\mathrm{\&delta;L}}_C\\ {\mathrm{\&delta;\&lambda;}}_C\\ {\mathrm{\&delta;h}}_e\end{array}\right]---\left(10\right)$

In formula, q_I0、q_I1、q_I2、q_I3For the attitude quaternion of inertial navigation system output, L_I、λ_I、h_IIt is respectively inertial navigation system output Latitude, longitude and altitude information；q_C0、q_C1、q_C2、q_C3For the attitude quaternion of celestial navigation system output, L_C、λ_CIt is respectively The latitude of celestial navigation output and longitude information；h_eElevation information for pressure altimeter output；δ represents the mistake of every correspondence Difference.

The measurement equation that can obtain SINS/CNS algorithm of combined navigation subsystem according to formula (1) and formula (10) is

z₂(t)=h₂(t)x(t)+v₂(t) (11)

In formula, h₂T () is measurement matrix, v₂(t)=[δq_C0, δ q_C1,δq_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TFor measuring white noise, Average is zero.

(3) independent navigation high precision nonlinear filtering algorithm: devise a set of SINS/CNS/SAR of being suitable for and independently combine The high accuracy of navigation system, nonlinear filtering algorithm, this set algorithm includes:

1. robust self adaptation Unscented particle filter algorithm；2. self adaptation Unscented that fades particle filter；3. mould Stick with paste robust adaptive particle filter；4. self adaptation SVD-UKF filtering algorithm；5. self adaptation square root Unscented particle filter. Specific as follows:

1. robust self adaptation Unscented particle filter algorithm:

Robust self adaptation Unscented particle filter, has fully absorbed Robust filter, robust adaptive-filtering and particle filter The advantage of ripple, is combined Robust filter principle with UPF by the equivalence weight factor and adaptive factor, selects suitable power letter Number and adaptive factor controlled state model information and measurement model information, the impact of the abnormal interference of suppression.

The step of robust self adaptation Unscented particle filter algorithm is as follows:

A () initializes, extract N number of particle according to initial mean value and mean square deviation, in the k=0 moment,I=1, 2 ..., N, arranging weights is

(b) at k=1,2 ..., n-hour, calculate according to following order:

1) equivalence weight is calculatedWith adaptive factor α.Selecting IGG scheme structure Modified Equivalent Weight Function, IGG is owned by France in fall power letter Number, i.e. does robust and limits measuring value, if taking its inverse, is then defined as variance inflation factor function.

If equivalent weight matrix is $\stackrel{\&OverBar;}{P}=\mathrm{diag}({\stackrel{\&OverBar;}{P}}_1,{\stackrel{\&OverBar;}{P}}_2,...,{\stackrel{\&OverBar;}{P}}_k),$

$\stackrel{\&OverBar;}{P_k}=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.---\left(12\right)$

Another kind of expression formula can also be used as required

$\stackrel{\&OverBar;}{P_k}=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0{(k_1-|V_k\left|\right)}^2}{\left|V_k\right|{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(13\right)$

Wherein k₀∈ (1,1.5), k₁∈ (3,8), V_kFor observation l_kResidual vector, For time current Carve state estimation value.Adaptive factor is chosen as follows

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.---\left(14\right)$

Wherein c₀∈ (1,1.5), c₁∈ (3,8),The mark of tr () representing matrix,For forecast Value, i.e.Can be seen that weight function and adaptive factor structural form are basic Identical, both of which is important regulatory factor.The former is by choosing the judgement of residual error, and the latter according to state estimation with The difference of predicted valueChoose.

2) Sigma point is calculated, by UKF algorithm more new particleObtainAndMeetIf new samples is2N+1 Sigma point sampling point is

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(15\right)$

λ=α in formula²(n+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines sampled point Degree of scatter to prediction average, β is typically based on priori and comes value (value optimal for Gauss distribution is 2), W_jRepresent The weights of jth Sigma point, meet ∑ W_j=1, j=0,1 ... 2N.

UKF algorithm is utilized particle to be predicted and updates:

$W_0^m=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})},W_0^c=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})---\left(16\right)$

$W_j^c=W_j^m=W_j=(N+\mathrm{\&lambda;}),j=\mathrm{1,2},...,2N---\left(17\right)$

${\mathrm{\&chi;}}_{k|k-1}^i=f\left({\mathrm{\&chi;}}_{k-1}^i\right)---\left(18\right)$

${\stackrel{\&OverBar;}{x}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{\mathrm{\&chi;}}_{j,k|k-1}^i---\left(19\right)$

$L_{k|k-1}^i=h\left({\mathrm{\&chi;}}_{k|k-1}^i\right)---\left(20\right)$

${\stackrel{\&OverBar;}{l}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{L}_{j,k|k-1}^i---\left(21\right)$

$P_{l_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c[{(L}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]\&CenterDot;{[L_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]}^T+Q_k---\left(22\right)$

$P_{x_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[L_{j,k|k-1}^i-{\stackrel{\&OverBar;}{l}}_{k|k-1}^i]}^T+R_k---\left(23\right)$

$P_{x_k{x}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[{\mathrm{\&chi;}}_{j,k|k-1}^i,-x_{k|k-1}^i]}^T---\left(24\right)$

$K_k=P_{x_k{l}_k}{P}_{l_k{l}_k}^{-1}---\left(25\right)$

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(l_k-{\stackrel{\&OverBar;}{L}}_{k|k-1}^i)---\left(26\right)$

$P_k^i=P_{x_k{x}_k}^i-K_k{P}_{l_k{l}_k}{K}_k^T---\left(27\right)$

Can be obtained by formula (26)～(27)

Equivalence weight and adaptive factor is utilized to calculate sampling particle valuation and variance

${\stackrel{\&OverBar;}{x}}_k^{i^{*}}={\stackrel{\&OverBar;}{x}}_{k|k-1}^i,+K_k^{*}(l_k-{\stackrel{\&OverBar;}{L}}_{k|k-1}^i)---\left(28\right)$

$P_k^{i^{*}}={\mathrm{\&alpha;}}_k{P}_{x_k{x}_k}^i-K_k^{*}{\stackrel{\&OverBar;}{P}}_{l_k{l}_k}{K}_k^{*T}---\left(29\right)$

$K_k^{*}=P_{x_k{l}_k}{\stackrel{\&OverBar;}{P}}_{l_k{l}_k}^{-1}---\left(30\right)$

Above formula shows by adaptive factor α_kWithCan affect and regulateMake the importance density function closer to Actual distribution.Obtain according to formula (16)～(19)As the importance density function of particle sampler, then carry out important Property sampling.From formula (18) it can be seen that when measurement model exists abnormal, equivalent weight matrixElement reduces, during parameter estimation Utilization measurement information rate reduces, and reduces the abnormal information impact on valuation.Otherwise, parameter estimation increases useful measurement information Utilization rate；In like manner, when state model exists abnormal, adaptive factor α_kReduce, utilization state information of forecasting during parameter estimation Rate reduces, and weakens the interference that model is abnormal, otherwise also sets up.If equivalence weightAnd during α=0,WithIt is UKF to calculate Sample average that method obtains and variance.

C () calculates weights $w_k^i=w_{k-1}^i\frac{p\left(y_k\right|{\stackrel{\&OverBar;}{x}}_k^{i^{*}})p\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}})}{q\left({\stackrel{\&OverBar;}{x}}_k^{i^{*}}\right|{\stackrel{\&OverBar;}{x}}_{k-1}^{i^{*}},l_k)},$ And be normalized to ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

D () calculates estimator ${\hat{N}}_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\left({\tilde{w}}_k^i\right)}^2.$

Acquired results is compared with set threshold, it is judged that the order of severity of sample degeneracy,The least, show to degenerate The most serious.In this case, previously obtained posterior density can be carried out resampling, retrieve M new particle, and compose Give each particle identical weights 1/M.

E () calculates nonlinear state amount estimated value.Repeat previous step (b).

Above-mentioned steps, when choosing the importance density function, make use of two important regulatory factors, i.e. the equivalence weight factor And adaptive factor.After being converted UT by both, gained particle sampler point more reasonably distributes useful information, adopts for importance Sample process provides more preferable sample distribution function.

2. self adaptation Unscented that fades particle filter: the self adaptation Unscented particle filter that fades of improvement is with grain Son is filtered into basic framework, merges fade adaptive filtration theory and UT conversion process, absorbs each single algorithm advantage, sets up ginseng Number can the importance density fonction of Automatic adjusument, utilize up-to-date measurement information the most efficiently so that it is closer to truly Distribution function so that filtering algorithm has more preferable adaptivity and robustness.

Unscented particle filter algorithm mainly utilizes UT conversion to obtain sampled point, it is achieved to state vector Posterior distrbutionp Approximation.Different from DSMC, Unscented particle filter is not random sampling from given distribution, is Take Sigma point that minority determines as sampled point.Sigma sampled point is

${\mathrm{\&chi;}}_{k-1}^i=[{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}]---\left(20\right)$

λ=α in formula²(N+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines sampled point Degree of scatter to prediction average.For different system modeies and noise it is assumed that UT mapping algorithm has different forms, certainly Determine UT mapping algorithm expression formula it is crucial that determine Sigma point sampling strategy, i.e. Sigma point number, position and weights.

Self adaptation Unscented that fades particle filter algorithm uses the memory span of fading factor restriction filter, fully Current observation value is utilized constantly to revise predictive value, and to the unknown or inapt system model parameter, noise statistics parameter etc. Carry out estimating and revising.Algorithm mainly comprises the following steps:

A () initializes, during k=0,Wherein k represents the moment.Unification arranges weights and isWherein k Representing the moment, N represents particle number.

B () calculates Sigma point, if new samples isCalculate 2N+1 Sigma sampled point, utilize UKF algorithm that particle is entered Row prediction and renewal,

Ibid, in following formula, β is typically based on priori and carrys out value (value optimal for Gauss distribution the most each symbolic significance It is 2), W_jRepresent the weights of jth Sigma point, meet ∑ W_j=1, j=0,1 ... 2N.

Time updates

$W_0^m=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})},W_0^c=\frac{\mathrm{\&lambda;}}{(N+\mathrm{\&lambda;})}+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;}),j=0---\left(21\right)$

$W_j^c=W_j^m=W_j=1/2(N+\mathrm{\&lambda;}),j=\mathrm{1,2},...,2N---\left(22\right)$

${\mathrm{\&chi;}}_{k|k-1}^i=f\left({\mathrm{\&chi;}}_{k-1}^i\right)---\left(23\right)$

${\stackrel{\&OverBar;}{x}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{\mathrm{\&chi;}}_{j,k|k-1}^i---\left(24\right)$

$Y_{k|k-1}^i=h\left({\mathrm{\&chi;}}_{k|k-1}^i\right)---\left(25\right)$

${\stackrel{\&OverBar;}{y}}_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m{Y}_{j,k|k-1}^i---\left(26\right)$

$P_{k|k-1}^i=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^m\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-x_{k|k-1}^i\right]\&CenterDot;{[{\mathrm{\&chi;}}_{j,k|k-1}^i,-x_{k|k-1}^i]}^T---\left(27\right)$

Measure and update

$P_{x_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c\left[({\mathrm{\&chi;}}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{x}}_{k|k-1}^i\right]\&CenterDot;{[Y_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]}^T---\left(28\right)$

$P_{l_k{l}_k}=\underset{j=0}{\overset{2N}{\mathrm{\&Sigma;}}}{W}_j^c[{(Y}_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]\&CenterDot;{[Y_{j,k|k-1}^i-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i]}^T---\left(29\right)$

$K_k=P_{x_k{l}_k}{P}_{l_k{l}_k}^{-1}---\left(30\right)$

Now utilize the adaptive extended kalman filtering thought that fades to calculate fading factor, utilize formula α_k, and calculate

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k/k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i)---\left(31\right)$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T---\left(32\right)$

ObtainAs the importance density function of particle sampler, carry out importance and adopt Sample.

C () is from importance density letterIn sample after, and to each particle calculate weights

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)}---\left(33\right)$

And calculate normalization weights.

${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i---\left(34\right)$

D () utilizes estimatorJudge that sample degeneracy degree is the most serious, the most obtained from the above Posterior density carries out resampling and retrieves M new particle, again gives each particle identical weights

E () calculates nonlinear state amount estimated value.

${\hat{x}}_k=\underset{i=0}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i{x}_k^{i**}---\left(35\right)$

Return (2nd) step, by the state estimation of new observed quantity recursive calculation subsequent time.

3. robust adaptive particle filter is obscured:

(I) based on fuzzy theory structure equivalence weight:

There is again the valuation of higher adaptivity in order to obtain existing stronger Robustness least squares, according to data residual error, power can be divided For: Bao Quan district (keep former observation constant), Jiang Quan district (observation being made robust limit) and region of rejection (Quan Weiling).Design One-dimensional fuzzy controller, the step of structure weight factor γ is as follows:

(a) obfuscation.The effect of obfuscation is that the precise volume of input is converted into obfuscation amount, will input quantity(accurately Value) obfuscation becomes fuzzy variable (whereinFor standardized residual), the input determined is converted into the mould described by degree of membership Stick with paste collection.

Detailed process: by input variableFuzzy subset be divided into { too big, greatly, more normal }, be abbreviated as { B_e, B_c, B_n, After input quantifies,Domain be X={0,1,2,3,4}；The fuzzy subset of output γ be { minimum, less, normal }, is abbreviated For { S_e, S_c, S_n, its domain is that Y size is divided into 5 grades, to represent different value, i.e. and Y={0,1,2,3,4}.Respectively to defeated EnterFuzzy quantization is carried out with output γ.

(b) according to the intuitive thinking reasoning of people and practical experience, by input quantityWith the relation of output weight factor γ, if Meter fuzzy control rule.IfToo big, then γ is minimum；IfRelatively big, then γ is less；IfNormally, then γ is normal.

May determine that fuzzy relation is according to fuzzy rule

R=(B_e×S_e)+(B_c×S_c)+(B_n×S_n)

Wherein, "×" represents the cartesian product of fuzzy vector.It is computed

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right]$

(c) according to fuzzy control principle, by input variableFuzzy subset and fuzzy relationship matrix r pass through fuzzy reasoning Obtain the fuzzy set that weight factor is γ, draw final fuzzy control quantity.

D this fuzzy control quantity is carried out ambiguity solution and is exported controlled quentity controlled variable, i.e. weight factor γ accurately by ().Fuzzy reasoning is tied Fruit is converted into the process of exact value and is referred to as anti fuzzy method.When anti fuzzy method processing procedure, use maximum membership grade principle.

(II) fuzzy robust adaptive particle filter algorithm

Fuzzy robust adaptive particle filter (Fuzzy Robust Adaptively Particle Filter, FRAPF) The step of algorithm is as follows: (a) initializes.In the k=0 moment, sample out N number of particle according to emphasis density, it is assumed that samples out is every Individual particle is usedRepresent, make k=1；(b) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and from Adaptation factor.

Error equation is:

$V_k=A_k{\hat{X}}_k-Z_k---\left(36\right)$

In formula, A_kFor coefficient matrix；Z_kFor observation vector, its weight matrix is P_k；V_kFor residual vector；For state parameter Vector.Taking risk function is

${V_k}^T{\stackrel{\&OverBar;}{P}}_k{V}_k+{({\stackrel{\&OverBar;}{X}}_k-{\hat{X}}_k)}^T{\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{X}}_k}^{-1}({\stackrel{\&OverBar;}{X}}_k-{\hat{X}}_k)=\min ---\left(37\right)$

Wherein,For Z_kEquivalent weight matrix, i.e.γ is weight factor.After seeking extreme value,

If equivalent weight matrix is $\stackrel{\&OverBar;}{P}=\mathrm{diang}({\stackrel{\&OverBar;}{P}}_1,{\stackrel{\&OverBar;}{P}}_{2,}...,{\stackrel{\&OverBar;}{P}}_k),$

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(k_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.---\left(38\right)$

Prediction residual contains the state without observation information correction, more can reflect the disturbance of dynamical system.K moment i-th Prediction residual vectorIt is expressed as

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(39\right)$

With the differentiation statistic that prediction residual is structure's variable state model error it is

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(40\right)$

Wherein,For k moment i-th status prediction information, pass throughObtain；Represent that prediction is residual DifferenceCovariance matrix；Tr () is Matrix Calculating trace operator.The adaptive factor function of statistic is differentiated based on prediction residual For

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(41\right)$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 < c < 2.5.

C () updates:

$x_k^i=f(x_{k-1}^i,v_{k-1})---\left(42\right)$

ByThe particle in k moment is updated according to formula (42)

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)})---\left(43\right)$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})}---\left(44\right)$

Renewal weights and normalization weights, i=1,2 ..., N.

D () resampling: be ranked up the weights of all particles according to descending, arranging thresholding sample points is N_th(generally It is chosen as N/2 or N/3), effective sample is counted as N_eff, work as N_eff<N_thTime, to particle collectionResampling, obtains new Particle collectionAnd reset weights and be

E () filters: $p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i)---\left(45\right)$

Calculate the filtering density in k moment and carry out re-sampling, making k=k+1, returning (b).

The adaptive factor α of robust adaptive-filtering_kNeither individual processing model error covariance matrixThe most not single Reason of the staying alone covariance matrix that previous epoch, state parameter vector was estimatedBut act on the state parameter forecast of entirety The equivalent covariance matrix of vectorOwing to robust adaptive-filtering have employed Robust filter principle to observation information, work as sight When surveying existence exception, using dynamic model information as an entirety, unified adaptive factor motivation of adjustment model is used to believe The breath overall contribution to state parameter, thus obtain reliable filter result.

4. self adaptation SVD-UKF filtering algorithm:

(I) singular value decomposition: singular value decomposition (SVD) is preferably a kind of square of stability and precision during numerical algebra calculates Battle array decomposition method, it is easy to realize on computers.It is defined as follows.

Assuming that A ∈ R^m×n(m >=n), then the singular value decomposition of matrix A is represented by

$A={\mathrm{U\&Lambda;V}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T---\left(46\right)$

In formula (46), U ∈ R^m×m, Λ ∈ R^m×n, V ∈ R^n×n, S=diag (s₁, s₂..., s_r)。s₁≥s₂≥…≥s_r≥0 Being referred to as the singular value of matrix A, the column vector of U and V is called the left and right singular vector of matrix A.

If A^TA positive definite, then formula (46) can be reduced to

$A=U\left[\begin{array}{c}S\\ 0\end{array}\right]V^T---\left(47\right)$

If A symmetric positive definite, then A=USU^T, the most left singular vector is equal with right singular vector, calculates such that it is able to reduce Amount.

(II) statistic and the determination of adaptive factor: with prediction residual as variable, the Error subtraction scheme of structural regime model Statistic and adaptive factor.Prediction residual (newly breath), containing the state without observation information correction, more can reflect dynamical system The disturbance being subject to.Prediction residualIt is expressed as

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{x}}_k\right)-y_k---\left(48\right)$

In formula (48),For k moment status prediction information.Use prediction residualStructural regime model error differentiates statistic As follows.

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

In formula (49),Represent prediction residualCovariance matrix, tr () is Matrix Calculating trace operator.

Adaptive factor function is elected as

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.---\left(50\right)$

In formula (50), α_kRepresenting adaptive factor, c is empirical, 1 < c < 2.5 under normal circumstances.

(III) self adaptation SVD-UKF algorithm steps: self adaptation SVD-UKF algorithm steps is as follows:

A () initializes: to the parameter initialization in state equation, and calculate the weight system of Sigma point average and covariance Number w^(m)、w^(c)。

${\hat{X}}_0=E\left(X_0\right),P_0=E\left[(X_0-{\hat{X}}_0){(X_0-{\hat{X}}_0)}^T\right],$

$w_0^{\left(m\right)}=\frac{1}{l+\mathrm{\&lambda;}},w_0^{\left(c\right)}=\frac{1}{l+\mathrm{\&lambda;}}+(1+{\mathrm{\&alpha;}}^2+\mathrm{\&beta;}),$

$w_i^{\left(m\right)}=w_i^{\left(c\right)}=\frac{1}{2(l+\mathrm{\&lambda;})},i=\mathrm{1,2},...,2l$

Wherein, α represent Sigma point relative to the diffusion of average (usual 1e-4≤α≤1), β be about system priori believe The parameter of breath.To Gauss distribution, β=2 are optimal, P₀Being original state covariance matrix, l is system mode dimension.

(b) singular value decomposition, calculating Sigma point vector:

Calculate characteristic point covariance matrix and 2l+1 Sigma point vector χ_{I, k}, hereinafter k ∈ 1,2 ..., ∞.

$P_{k-1}=U_{k-1}{S}_{k-1}{V}_{k-1}^T---\left(51\right)$

${\mathrm{\&chi;}}_{i,k-1}=\left[\begin{array}{ccc}{\hat{X}}_{k-1}& {\hat{X}}_{k-1}+\mathrm{\&rho;}{U}_{i,k}\sqrt{S_{i,k}}& {\hat{X}}_{k-1}\end{array},-\mathrm{\&rho;}{U}_{i,k}\sqrt{S_{i,k}}\right]---\left(52\right)$

In formula (52), ρ is scaling factor, and the most appropriate value isS_{I, k}For singular value decomposition because of Son.

C () time updates:

χ_{I, k | k-1}=f (χ_i,k-1)+W_kI=0,1 ..., 2l (53)

${\hat{X}}_{k|k-1}={\mathrm{\&chi;}}_{0,k|k-1}+\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(m\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\mathrm{\&chi;}}_{0,k|k-1})---\left(54\right)$

$S_{i,k|k-1}=\mathrm{svd}\left\{\right[\sqrt{w_i^{\left(c\right)}}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})\left]\right\}---\left(55\right)$

${\stackrel{\&OverBar;}{S}}_{i,k|k-1}=S_{i,k|k-1}/\sqrt{{\mathrm{\&alpha;}}_k}--\left(56\right)$

In formula (55), svd{ } it is SVD decomposition operator, the adaptive factor α in formula (56)_kDetermined by formula (50), use α_k Revise S_{I, k | k-1}。

${\mathrm{\&chi;}}_{i,k|k-1}=\left[\begin{array}{ccc}{\mathrm{\&chi;}}_{i,k-1}& {\mathrm{\&chi;}}_{i,k-1}+\mathrm{\&rho;}{U}_{i,k}{\stackrel{\&OverBar;}{S}}_{i,k|k-1}& {\mathrm{\&chi;}}_{i,k-1}-\mathrm{\&rho;}{U}_{i,k}{\stackrel{\&OverBar;}{S}}_{i,k|k-1}\end{array}\right]---\left(57\right)$

$P_{k|k-1}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1})}^T+Q---\left(58\right)$

y_i,k|k-1=h(x_{I, k | k-1})+E_kI=0,1 ..., 2l (59)

${\hat{y}}_{k|k-1}=y_{0,k|k-1}+\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(m\right)}(y_{i,k|k-1}-{\hat{y}}_{k|k-1})---\left(60\right)$

D () measures and updates

$S_{{\hat{y}}_k}=\mathrm{svd}\left\{\right[\sqrt{w_i^{\left(c\right)}}(y_{i,k|k-1}-{\hat{y}}_{k|k-1})\left]\right\}---\left(61\right)$

$P_{{\hat{y}}_k{\hat{y}}_k}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}(y_{i,k|k-1}-{\hat{y}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T+R---\left(62\right)$

$P_{{\hat{X}}_k{\hat{y}}_k}=\underset{i=1}{\overset{2l}{\mathrm{\&Sigma;}}}{w}_i^{\left(c\right)}({\mathrm{\&chi;}}_{i,k|k-1}-{\hat{X}}_{k|k-1}){(y_{i,k|k-1}-{\hat{y}}_{k|k-1})}^T---\left(63\right)$

$K_k=P_{{\hat{X}}_k{\hat{y}}_k}{P}_{{\hat{y}}_k{\hat{y}}_k}^{-1}/\left(S_{{\hat{y}}_k}^T{S}_{{\hat{y}}_k}\right)---\left(64\right)$

${\hat{X}}_k={\hat{X}}_{k|k-1}+K_k(y_k-{\hat{y}}_{k|k-1})---\left(65\right)$

$P_k=P_{k|k-1}-K_k{P}_{{\hat{y}}_k{\hat{y}}_k}{K}_k^T---\left(66\right)$

Q in formula (58) is respectively system noise variance and measuring noise square difference with the R in formula (62).

5. self adaptation square root Unscented particle filter: set system dynamics equation as

X_k=Φ_{K, k-1}X_k-1+W_k (67)

In formula, X_kIt is that k moment n ties up state vector, Φ_{K, k-1}For n × n-state transfer matrix, W_kVectorial for system noise, its Covariance matrix is

Observational equation is

Y_k=H_kX_k+e_k (68)

In formula, Y_kIt is that k moment m ties up observation vector, H_kDesign matrix, e is tieed up for m × n_kFor observation noise vector, its covariance Battle array is ∑_k。W_k, W_j, e_k, e_j(j ≠ k) is orthogonal.

The probability density function of known state is p (X₀|Y₀)=p(X₀), then theoretical according to Bayesian Estimation, non-linear The status predication equation of time-varying system and state renewal equation are respectively

p(X_k|Y_1:K-1)=∫ p (X_k|X_k-1)p(X_k-1|Y_1:k-1)dX_k-1 (69)

$p\left(X_k\right|Y_{1:k})=\frac{p\left(Y_k\right|X_k)p\left(X_k\right|Y_{1:k-1})}{p\left(Y_k\right|Y_{1:k-1})}---\left(70\right)$

In formula, p (X_k|X_k-1) it is state transfering density, p (X_k-1|Y_1:k-1) it is the posterior density in k-1 moment；

p(X_k|Y_1:k-1) it is prior distribution, p (Y_k|X_k) it is likelihood density, p (Y_k|Y_1:k-1) be normaliztion constant, can by under Formula is tried to achieve

p(Y_k|Y_1:k-1)=∫p(Y_k|X_k)p(X_k|Y_1:k-1)dX_k (71)

Formula (69)～(71) constitute recursion Bayesian Estimation.Formula (71) only can obtain analytic solutions to some dynamical system. Integral operation can be converted into the summation operation of finite sample point by Monte Carlo method based on stochastical sampling, after Test the approximate expression form of probability density function.Posterior density p (X in reality_k|Y_1:k) it is probably multivariate, non-standard probability Distribution, needs to sample it by importance sampling algorithm, so that structure importance function.Select appropriate importance letter Number can effectively solve the sample degeneracy problem of particle filter.

This project uses UKF algorithm to produce the importance density function of particle filter, and this algorithm makes full use of up-to-date sight Survey data, the error that correction kinetic model and noise statistics parameter cause in real time.The step of self adaptation square root UPF is as follows.

A () initializes (k=0): randomly draw N number of primary(i=1,2 ..., N).Assume ${\hat{X}}_0^i=E\left[X_0^i\right],S_0^i=\mathrm{chol}\left\{E\right[(X_0^i-{\hat{X}}_0^i){(X_0^i-{\hat{X}}_0^i)}^T\left]\right\},w_0^i=p\left(Y_0\right|X_0^i).$ Wherein,WithRepresent respectively Initial time i-th particle and estimated value thereof,Represent initial time i-th Cholesky factoring,Represent i-th grain The initialization weights of son, chol{ } the Cholesky decomposition operator of representing matrix.

B () uses self adaptation Deep space algorithm to update each particleObtain Represent the covariance of k moment i-th particle.

(b1) Sigma point and weights are calculated

$\left\{\begin{array}{cc}{X}_{0,k-1}^i={\hat{x}}_{k-1}^i& \\ X_{j,k-1}^i={\hat{X}}_{k-1}^i+\sqrt{(n+\mathrm{\&lambda;})}{S}_{k-1}^i& j=1,...,n\\ X_{j,k-1}^i={\hat{X}}_{k-1}^i-\sqrt{(n+\mathrm{\&lambda;})}{S}_{k-1}^i& j=n+1,...,2n\end{array}---\right.\left(72\right)$

$\left\{\begin{array}{cc}{W}_0^m=\mathrm{\&lambda;}/(n+\mathrm{\&lambda;})& \\ W_0^c=\mathrm{\&lambda;}/(n+\mathrm{\&lambda;})+(1-{\mathrm{\&alpha;}}^2+\mathrm{\&beta;})& \\ W_j^m=W_j^c=0.5/(n+\mathrm{\&lambda;})& j=1,...,2n\end{array}\right.---\left(73\right)$

Wherein, λ is scale parameter, and n represents system mode dimension, and α represents that Sigma point is (logical around the diffusion of average Often 1e-4≤α≤1), β is the parameter about system prior information, optimal to Gauss distribution β=2,Represent jth Sigma Point, W_jFor its weights.

(b2) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and adaptive factor.

Prediction residual (or newly breath), containing the state without observation information correction, more can reflect the disturbance of dynamical system.During k Carve i-th prediction residual vectorIt is expressed as

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k---\left(74\right)$

With the differentiation statistic that prediction residual is structure's variable state model error it is

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}}---\left(75\right)$

Wherein,For k moment i-th status prediction information, pass throughObtain,Represent prediction Residual errorCovariance matrix, tr () is Matrix Calculating trace operator.

Adaptive factor function based on prediction residual differentiation statistic is

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.---\left(76\right)$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 < c < 2.5.

(b3) time updates (status predication)

$X_{k\backslash k-1}^i={\mathrm{\&Phi;}}_{k,k-1}{X}_{k-1}^i+W_k---\left(77\right)$

${\hat{X}}_{k/k-1}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^m{X}_{j,k/k-1}^i---\left(78\right)$

$S_{k/k-1}^i=\mathrm{qr}\left\{\right[\sqrt{W_1^c}(X_{1:2n,k/k-1}^i-{\hat{X}}_{k/k-1}^i)\sqrt{{\mathrm{\&Sigma;}}_{W_k}}\left]\right\}---\left(79\right)$

$S_{k/k-1}^i=\mathrm{cholupdate}\{S_{k/k-1}^i,X_{0,k/k-1}^i-{\hat{X}}_{k/k-1}^i,W_0^c\}---\left(80\right)$

${\stackrel{\&OverBar;}{S}}_{k/k-1}^i=S_{k/k-1}^i/\sqrt{{\mathrm{\&alpha;}}_k^i}---\left(81\right)$

$X_{k/k-1}^i=\left[\begin{array}{ccc}{\hat{X}}_{k/k-1}^i& {\hat{X}}_{k/k-1}^i+\sqrt{n+\mathrm{\&lambda;}}{\stackrel{\&OverBar;}{S}}_{k/k-1}^i& {\hat{X}}_{k/k-1}^i-\sqrt{n+\mathrm{\&lambda;}}{\stackrel{\&OverBar;}{S}}_{k/k-1}^i\end{array}\right]---\left(82\right)$

$Y_{k/k-1}^i=H_k{X}_{k/k-1}^i+e_{k-1}---\left(83\right)$

${\hat{Y}}_{k/k-1}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^c{Y}_{j,k/k-1}^i---\left(84\right)$

Wherein,WithRepresent that the k moment shifts to the state of k-1 moment i-th particle and estimates respectively, Cholupdate{ } represent Cholesky factoring update operator, formula (81) utilizes adaptive factorRevise Cholesky factoring

(b4) measurement updaue (state estimation)

$S_{{\hat{y}}_k}^i=\mathrm{qr}\left\{\right[\sqrt{W_1^c}(Y_{1:2n,k/k-1}^i-{\hat{Y}}_{k/k-1}^i)\mathrm{}\sqrt{{\mathrm{\&Sigma;}}_{k-1}^i}\left]\right\}---\left(85\right)$

$S_{{\hat{y}}_k}^i=\mathrm{cholupdate}\{S_{{\hat{y}}_k}^i,Y_{0,k/k-1}^i-{\hat{Y}}_{k/k-1}^i,W_0^c\}---\left(86\right)$

${\mathrm{\&Sigma;}}_{{\hat{x}}_k{\hat{y}}_k}^i=\underset{j=0}{\overset{2n}{\mathrm{\&Sigma;}}}{W}_j^c[X_{j,k/k-1}^i-{\hat{X}}_{k/k-1}^i]{[Y_{j,k/k-1}^i-{\hat{Y}}_{k/k-1}^i]}^T---\left(87\right)$

$K_k^i=({\mathrm{\&Sigma;}}_{{\hat{x}}_k{\hat{y}}_k}^i/{\left(S_{{\hat{y}}_k}^i\right)}^T)/S_{{\hat{y}}_k}^i---\left(88\right)$

${\hat{X}}_k^i={\hat{X}}_{k/k-1}^i+K_k^i(Y_k^i-{\hat{Y}}_{k/k-1}^i)---\left(89\right)$

$U^i=K_k^i{S}_{{\hat{y}}_k}^i---\left(90\right)$

$S_k^i=\mathrm{cholupdate}\{{\stackrel{\&OverBar;}{S}}_{k/k-1}^i,U^i,-1\}---\left(91\right)$

${\mathrm{\&Sigma;}}_k^i=S_k^i{\left(S_k^i\right)}^T---\left(92\right)$

Employ the QR decomposition in linear algebra in this step, Cholesky decomposes, with the shape of Cholesky factoring State covariance matrix is directly propagated and updated to formula, thus enhances the numerical stability in state covariance matrix renewal process Property, it is ensured that the orthotropicity of covariance matrix.Wherein, qr{ } representing matrix QR decompose.

(c) importance sampling weight computing: make significance distribution functionSampling ParticleN () represents normal distribution.Pass through respectively $w_k^i=w_{k-1}^i\frac{p(Y_k/X_k^i)p(X_k^i/X_{k-1}^i)}{q\left({\stackrel{\&OverBar;}{X}}_k^i\right|{\stackrel{\&OverBar;}{X}}_{0:k-1}^i,Y_{1:k})},$ Renewal weights and normalization weights, i=1 ..., N.

(d) resampling:

Arranging thresholding sample points is N_th(being generally chosen as N/2 or N/3), effective sample is counted as N_eff, work as N_eff<N_th Time, to particle collectionResampling, obtains new particle collectionAnd reset weights and be

E () state updates:

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{X}}_k^i{\tilde{w}}_k^i---\left(93\right)$

${\widehat{\mathrm{\&Sigma;}}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{w}}_k^i({\hat{X}}_k-{\tilde{X}}_k^i){({\hat{X}}_k-{\tilde{X}}_k^i)}^T---\left(94\right)$

Claims (6)

1. an independent combined navigation system, it is characterised in that: SINS is led as principle navigation system, SAR, CNS as auxiliary Boat system, constitutes SINS/SAR/CNS integrated navigation system, and it specifically comprises the following steps that

First, design SINS/SAR and SINS/CNS navigation subfilter, obtain the two of integrated navigation system state by calculating Group local optimum estimated valueWith local Optimal error covariance matrixThen, use federated filter technology, will Two groups of local optimum estimated values are sent in senior filter and are carried out information fusion, obtain global optimum's estimated value of system mode With global optimum's error covariance matrixFinally, global optimum's estimated value of the state obtained is utilizedTo inertial navigation system The error of system carries out real time correction；SINS/SAR/CNS integrated navigation system optimal estimation fusion algorithm is

$\left\{\begin{array}{c}{\mathrm{\&Sigma;}}_{\hat{X},g}={({\mathrm{\&Sigma;}}_{\hat{X},1}^{-1}+{\mathrm{\&Sigma;}}_{\hat{X},2}^{-1})}^{-1}\\ {\hat{X}}_g={\mathrm{\&Sigma;}}_{\hat{X},g}({\mathrm{\&Sigma;}}_{\hat{X},1}^{-1}{\hat{X}}_1+{\mathrm{\&Sigma;}}_{\hat{X},2}^{-1}{\hat{X}}_2)\end{array}\right.;$

Described SINS/SAR/CNS integrated navigation system, its mathematical model includes that state equation, measurement equation and independent navigation are high Precision non-linear filtering algorithm:

(1) state equation: the navigation error of SAR and CNS is thought of as white Gaussian noise, and is not classified as the shape of integrated navigation system State amount, is only thought of as the system state amount of SINS/SAR/CNS integrated navigation system by the systematic error of SINS；

Choose sky, northeast E-N-U geographic coordinate system g navigational coordinate system n as integrated navigation system, according to inertial navigation system Error equation, state x (t) of SINS/SAR/CNS integrated navigation system is chosen for:

$x\left(t\right)={\&lsqb;{\left(\mathrm{\&delta;}q\right)}^T,{\left(\mathrm{\&delta;}V\right)}^T,{\left(\mathrm{\&delta;}P\right)}^T,{\mathrm{\&epsiv;}}^T,{\&dtri;}^T\&rsqb;}^T;$

In formula, δ q=[δ q₀、δq₁、δq₂、δq₃]^TAttitude error quaternary number for SINS；δ V=[δ V_E、δV_N、δV_U]^TFor SINS East, north, the velocity error in direction, sky；δ P=[δ L, δ λ, δ h]^TLatitude, longitude and altitude error for SINS；ε=[ε_x、ε_y、 ε_x]^TRepresent the random drift of gyro；It it is the constant value biasing of accelerometer；

Can obtain according to above formula, the state equation of SINS/SAR/CNS integrated navigation system is:

$\stackrel{\&CenterDot;}{x}=f(x,t)+G\left(t\right)w\left(t\right);$

In formula, (x t) is the state array of system to f；W (t)=[w_gx, w_gy, w_gz, w_ax, w_ay, w_az]^TRepresent system noise, [w_gx、 w_gy、w_gz] it is the white noise of gyro, [w_ax、w_ay、w_az] it is the white noise of accelerometer；G (t) is that the noise of system drives matrix, And system mode battle array and noise drive battle array to be respectively as follows:

$f(x,t)=\left[\begin{array}{c}B(I-C_n^c){\mathrm{\&omega;}}_{in}^n-{\mathrm{BC}}_b^c{\mathrm{\&epsiv;}}^b\\ (I-C_c^n)C_b^c{f}^b-(2{\mathrm{\&omega;}}_{ie}^n+{\mathrm{\&omega;}}_{en}^n)\&times;{\mathrm{\&delta;V}}^n-(2{\mathrm{\&delta;\&omega;}}_{ie}^n+{\mathrm{\&delta;\&omega;}}_{en}^n)\&times;V^n+C_b^c{\&dtri;}^b\\ M\mathrm{\&delta;}V+N\mathrm{\&delta;}L\\ 0_{3\&times;1}\\ 0_{3\&times;1}\end{array}\right];$

$G\left(t\right)=\left[\begin{array}{cc}{C}_b^c& 0_{3\&times;3}\\ 0_{3\&times;3}& C_b^c\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& 0_{3\&times;3}\end{array}\right];$

In formula, B represents that the number of attitude quaternion is taken advantage of, I representation unit battle array,Represent between navigational coordinate system n and coordinates computed system c Transition matrix,Represent the transition matrix between carrier coordinate system b and coordinates computed system c, ε^bFor gyro error,Represent Transition matrix between coordinates computed system c and navigational coordinate system n, f^bFor the measurement of accelerometer, i.e. specific force,WithRespectively It is rotational-angular velocity of the earth and position angle speed,δVⁿFor velocity error,For accelerometer error, M and N Being respectively velocity error and position error coefficient battle array, δ V is bearer rate error, and δ L is carrier latitude error；

(2) measurement equation, including following measurement equation:

1. SINS/SAR subsystem measurement equation: the course angle information that inertial navigation can be exported and positional information and synthesis hole The difference of the carrier height information of the course angle information of footpath radar output, horizontal position information and pressure altimeter output is as amount Measuring, the measurement equation of SINS/SAR integrated navigation system is:

z₁(t)=h₁(x, t)+v₁(t)；

In formula, h₁(x t) is the nonlinear function of measurement equation, v (t)=[δ ψ_S, δ L_S, δ λ_S, δ h_e]^TFor measuring white noise, all Value is zero；

2. SINS/CNS subsystem measurement equation: the measurement equation of SINS/CNS algorithm of combined navigation subsystem is:

z₂(t)=h₂(t)x(t)+v₂(t)；

In formula, h₂T () is measurement matrix, v₂(t)=[δ q_C0, δ q_C1, δ q_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TFor measuring white noise, all Value is zero；

(3) independent navigation high precision nonlinear filtering algorithm: devise and a set of be suitable for SINS/CNS/SAR independent combined navigation The high accuracy of system, nonlinear filtering algorithm, this set algorithm includes: 1. robust self adaptation Unscented particle filter algorithm；② Self adaptation Unscented that fades particle filter；3. robust adaptive particle filter is obscured；4. self adaptation SVD-UKF filtering algorithm； 5. self adaptation square root Unscented particle filter.

Independent combined navigation system the most according to claim 1, it is characterised in that: described robust self adaptation Unscented Particle filter algorithm step is as follows:

A () initializes: extract N number of particle according to initial mean value and mean square deviation, in the k=0 moment, Arranging weights is

(b) at k=1,2 ..., n-hour, calculate according to following order:

(b1) equivalence weight is calculatedWith adaptive factor α, selecting IGG scheme structure Modified Equivalent Weight Function, IGG is owned by France in fall weight function, Measuring value i.e. doing robust limit, if taking, it is reciprocal, then be defined as variance inflation factor function:

If equivalent weight matrix is

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_k& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}& k_0<\left|V_k\right|\&le;k_1\\ 0& \left|V_k\right|>k_1\end{array}\right.;$

Can also use another kind of expression formula as required:

${\stackrel{\&OverBar;}{P}}_k=\left(\begin{array}{cc}{p}_i& \left|V_k\right|\&le;k_0\\ p_k\frac{k_0}{\left|V_k\right|}\frac{{(k_1-|V_k\left|\right)}^2}{{(h_1-k_0)}^2}& k_0\&le;\left|V_k\right|<k_1\\ 0& k_1\&le;\left|V_k\right|\end{array}\right.;$

Wherein k₀∈ (1,1.5), k₁∈ (3,8), V_kFor observation l_kResidual vector,A_kFor coefficient matrix,For current time state estimation value；Adaptive factor is chosen as follows:

${\mathrm{\&alpha;}}_k=\left(\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\&le;c_0\\ \frac{c_0}{\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|}\frac{{(c_1-|\mathrm{\&Delta;}{\tilde{x}}_k\left|\right)}^2}{{(c_1-c_0)}^2}& c_0\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|<c_1\\ 0& c_1\&le;\left|\mathrm{\&Delta;}{\tilde{x}}_k\right|\end{array}\right.;$

Wherein c₀∈ (1,1.5), c₁∈ (3,8),The mark of tr () representing matrix,For predicted value, i.e.The former is by choosing the judgement of residual error, and the latter estimates according to state Evaluation and the difference of predicted valueChoose；

(b2) Sigma point is calculated, by UKF algorithm more new particleObtainAndMeetQ () is importance density function, and N () is that normal distyribution function, also referred to as posteriority are close Degree,For the estimated value of system state amount,For the estimated value of k-1 moment i-th system state amount,For system state amount Predictive value, l_kFor observation,For estimating mean squared error matrix, if new samples is2N+1 Sigma point sampling point is:

${\mathrm{\&chi;}}_{k-1}^i=\&lsqb;{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}\&rsqb;---\left(15\right)$

λ=α in formula²(N+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines that sampled point is in advance Survey the degree of scatter of average, W_jRepresent the weights of jth Sigma point, meet ∑ W_j=1, j=0,1 ... 2N；

C () calculates weightsAnd be normalized to

In formula, p () is probability density function, y_kMeasure for system quantities,For sampling particle estimated value,During for k-1 Carve ith sample particle estimated value；

D () calculates estimator

Acquired results is compared with set threshold, it is judged that the order of severity of sample degeneracy,The least, show to degenerate the tightest Weight；In this case, can be to previously obtained posterior densityCarry out resampling, retrieve M new particle, And give each particle identical weights 1/M；

(e) calculating nonlinear state amount estimated value:Repeat previous step (b)；

Above-mentioned steps, when choosing the importance density function, make use of two important regulatory factors, i.e. the equivalence weight factor and from Adaptation factor；After being converted UT by both, gained particle sampler point more reasonably distributes useful information, for importance sampling mistake Journey provides more preferable sample distribution function.

Independent combined navigation system the most according to claim 1, it is characterised in that fade described in: self adaptation Unscented Particle filter algorithm mainly utilizes UT conversion to obtain sampled point, it is achieved the approximation to state vector Posterior distrbutionp；With Monte Carlo Method is different, and Unscented particle filter is not random sampling from given distribution, is to take the Sigma that minority determines Point is as sampled point；Sigma sampled point is

${\mathrm{\&chi;}}_{k-1}^i=\&lsqb;{\stackrel{\&OverBar;}{x}}_{k-1}^i,{\stackrel{\&OverBar;}{x}}_{k-1}^i+\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i},{\stackrel{\&OverBar;}{x}}_{k-1}^i-\sqrt{(N+\mathrm{\&lambda;})P_{k-1}^i}\&rsqb;;$

λ=α in formula²(N+v) representing scale factor, v is second order scale factor, and N is sampling particle number, and α determines that sampled point is in advance Survey the degree of scatter of average,For the predictive value of k-1 moment i-th system state amount,Estimate all for k-1 moment i-th Side's error battle array；For different system modeies and noise it is assumed that UT mapping algorithm has different forms, determine UT mapping algorithm Expression formula it is crucial that determine Sigma point sampling strategy, i.e. Sigma point number, position and weights；Its algorithm mainly comprises the following steps:

A () initializes, during k=0,Wherein k represents the moment；Unification arranges weights and isWherein k table Show that moment, N represent particle number；

B () calculates Sigma point, if new samples isCalculate 2N+1 Sigma sampled point, utilize UKF algorithm to carry out pre-to particle Survey and update；Utilization fade adaptive extended kalman filtering thought calculate fading factor, utilize formula α_k, and calculate

${\stackrel{\&OverBar;}{x}}_k^i={\stackrel{\&OverBar;}{x}}_{k|k-1}^i+K_k(y_k-{\stackrel{\&OverBar;}{y}}_{k|k-1}^i);$

$P_k^i=P_{k|k-1}^i-{\mathrm{\&alpha;}}_k{K}_k{P}_{l_k{l}_k}{K}_k^T;$

ObtainAs the importance density function of particle sampler, carry out importance sampling,

In formula, K_kFor gain matrix, y_kMeasure for system quantities,Predictive value is measured for k-1 moment i-th system quantities；

C () is from importance density letterIn sample after, and to each particle calculate weights

$w_k^i=w_{k-1}^i\frac{p\left(y_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{k-1}^i,y_k)};$

And calculate normalization weights；

${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\&Sigma;}}{w}_k^i;$

D () utilizes estimatorJudge that sample degeneracy degree is the most serious, posteriority the most obtained from the above Density carries out resampling and retrieves M new particle, again gives each particle identical weights

(e) calculating nonlinear state amount estimated value:

${\hat{x}}_k=\underset{i=0}{\overset{N}{\&Sigma;}}{\tilde{w}}_k^i{x}_k^{i**},$

In formula,Resampling estimated value for system state amount；

Return step (b), by the state estimation of new observed quantity recursive calculation subsequent time.

Independent combined navigation system the most according to claim 1, it is characterised in that: described fuzzy robust self adaptation particle is filtered Ripple includes based on fuzzy theory structure equivalence weight and specific algorithm step, wherein:

(I) equivalence weight is constructed based on fuzzy theory: power be divided into according to data residual error: Bao Quan district, keep former observation not Become, Jiang Quan district, observation is made robust limit and region of rejection, Quan Weiling；Design one-dimensional fuzzy controller, construct weight factor The step of γ is as follows:

(a) obfuscation: by input quantityExact value, obfuscation becomes fuzzy variable, whereinFor standardized residual, will determine Input be converted into the fuzzy set described by degree of membership；Its detailed process is: by input variableFuzzy subset be divided into { too Greatly, more greatly, normal }, it is abbreviated as { B_e, B_c, B_n, after input quantifies,Domain be X={0,1,2,3,4}；The mould of output γ Sticking with paste subset be { minimum, less, normally }, is abbreviated as { S_e, S_c, S_n, its domain is that Y size is divided into 5 grades, to represent different Value, i.e. Y={0,1,2,3,4}；Respectively to inputFuzzy quantization is carried out with output γ；

(b) according to the intuitive thinking reasoning of people and practical experience, by input quantityWith the relation of output weight factor γ, design mould Stick with paste and control rule；IfToo big, then γ is minimum；IfRelatively big, then γ is less；IfNormally, then γ is normal；According to mould Stick with paste rule and may determine that fuzzy relation is

R=(B_e×S_e)+(B_c×S_c)+(B_n×S_n)；

Wherein, "×" represents the cartesian product of fuzzy vector；It is computed

$R=\left[\begin{array}{ccccc}0& 0& 0& 0.5& 1\\ 0& 0.5& 0.5& 0.5& 0.5\\ 0& 0.5& 1& 0.5& 0\\ 0.5& 0.5& 0.5& 0.5& 0\\ 1& 0.5& 0& 0& 0\end{array}\right];$

(c) according to fuzzy control principle, by input variableFuzzy subset and fuzzy relationship matrix r obtained by fuzzy reasoning Weight factor is a fuzzy set of γ, draws final fuzzy control quantity；

D this fuzzy control quantity is carried out ambiguity solution and is exported controlled quentity controlled variable, i.e. weight factor γ accurately by ()；Fuzzy reasoning result turns The process turning to exact value is referred to as anti fuzzy method；When anti fuzzy method processing procedure, use maximum membership grade principle；

(II) step of fuzzy robust adaptive particle filter algorithm is as follows:

A () initializes: in the k=0 moment, samples out N number of particle according to emphasis density, it is assumed that each particle sampled out is usedRepresent, make k=1；

(b) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and adaptive factor: be with prediction residual The differentiation statistic of structure's variable state model error is:

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{tr\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}};$

Wherein, pass throughObtain, f_kG () is systematic state transfer matrix；Represent prediction residualAssociation Variance matrix；Tr () is Matrix Calculating trace operator；Adaptive factor function based on prediction residual differentiation statistic is:

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.;$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 ＜ c ＜ 2.5；

C () updates:

$x_k^i=f(x_{k-1}^i,v_{k-1});$

ByLimit the particle updating the k moment according to formula (42)

$p\left(x_{0:k}\right|y_{1:k})\&cong;\underset{i=1}{\overset{N}{\&Sigma;}}{w}_k^{\left(i\right)}\mathrm{\&delta;}(x_{0:k}-x_{0:k}^{\left(i\right)});$

$w_k^i=w_{k-1}^i\frac{p(y_k/x_k^i)p(x_k^i/x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1}^i,y_{1:k})};$

Renewal weights and normalization weights, i=1,2 ..., N；

D () resampling: be ranked up the weights of all particles according to descending, arranging thresholding sample points is N_th, the most optional For N/2 or N/3, effective sample is counted as N_eff, work as N_eff＜ N_thTime, to particle collectionResampling, obtains new grain SubsetAnd reset weights and be

E () filters:

$p\left(x_k^1\right|y_{1:k})=\underset{i=1}{\overset{N}{\&Sigma;}}{w}_k^i\mathrm{\&delta;}(x_k^1-x_k^i);$

Calculate the filtering density in k moment and carry out re-sampling, making k=k+1, returning (b).

Independent combined navigation system the most according to claim 1, it is characterised in that: the filtering of described self adaptation SVD-UKF is calculated Method includes:

(I) singular value decomposition: singular value decomposition is defined as follows:

Assuming that A ∈ R^m×n(m >=n), then the singular value decomposition of matrix A is represented by

$A={\mathrm{U\&Lambda;V}}^T=U\left[\begin{array}{cc}S& 0\\ 0& 0\end{array}\right]V^T;$

In formula, U ∈ R^m×m, Λ ∈ R^m×n, V ∈ R^n×n, S=diag (s₁, s₂..., s_r)；s₁≥s₂≥...≥s_r>=0 is referred to as square The singular value of battle array A, the column vector of U and V is called the left and right singular vector of matrix A；

(II) statistic and the determination of adaptive factor: with prediction residual as variable, the Error subtraction scheme statistics of structural regime model Amount and adaptive factor, it was predicted that residual errorIt is expressed as:

${\stackrel{\&OverBar;}{V}}_k=g\left({\stackrel{\&OverBar;}{X}}_k\right)-y_k;$

In formula,For k moment status prediction information；Use prediction residualStructural regime model error differentiates that statistic is as follows:

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k={({\left({\stackrel{\&OverBar;}{V}}_k\right)}^T{\stackrel{\&OverBar;}{V}}_k/tr({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\left)\right)}^{1/2};$

In formula,Represent prediction residualCovariance matrix, tr () is Matrix Calculating trace operator；

Adaptive factor function is elected as:

${\mathrm{\&alpha;}}_k=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k\right|>c\end{array}\right.;$

In formula, α_kRepresenting adaptive factor, c is empirical, under normal circumstances 1 ＜ c ＜ 2.5；

(III) self adaptation SVD-UKF algorithm steps is as follows:

A () initializes: to the parameter initialization in state equation, and calculate the weight coefficient w of Sigma point average and covariance^(m)、w^(c)；(b) singular value decomposition, calculating Sigma point vector；C () time updates；D () measures and updates.

Independent combined navigation system the most according to claim 1, it is characterised in that: described self adaptation square root Unscented particle filter step is as follows:

A () initializes, k=0: randomly draw N number of primaryAssumeWherein,WithRepresent respectively Initial time i-th particle and estimated value thereof,Represent initial time i-th Cholesky factoring,Represent i-th The initialization weights of particle, chol{ } the Cholesky decomposition operator of representing matrix；

B () uses self adaptation Deep space algorithm to update each particleObtain Table Showing the covariance of k moment i-th particle, step is as follows:

(b1) Sigma point and weights are calculated:

(b2) with prediction residual as variable, the Error subtraction scheme statistic of structural regime model and adaptive factor；

Prediction residual or new breath, containing the state without observation information correction, more can reflect the disturbance of dynamical system, k moment i-th Individual prediction residual vectorIt is expressed as:

${\stackrel{\&OverBar;}{V}}_k^i=H_k{\stackrel{\&OverBar;}{X}}_k^i-Y_k;$

With the differentiation statistic that prediction residual is structure's variable state model error it is:

$\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i={\left(\frac{{\left({\stackrel{\&OverBar;}{V}}_k^i\right)}^T{\stackrel{\&OverBar;}{V}}_k^i}{tr\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k^i}\right)}\right)}^{\frac{1}{2}};$

Wherein, H_kRepresent measurement matrix, Y_kRepresent that system quantities is measured,For k moment i-th status prediction information, pass throughObtain,Represent prediction residualCovariance matrix, tr () is Matrix Calculating trace operator；

Adaptive factor function based on prediction residual differentiation statistic is:

${\mathrm{\&alpha;}}_k^i=\left\{\begin{array}{cc}1& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|\&le;c\\ \frac{c}{\left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|}& \left|\mathrm{\&Delta;}{\stackrel{\&OverBar;}{V}}_k^i\right|>c\end{array}\right.;$

In formula,Representing k moment i-th adaptive factor, c is empirical, typically takes 1.0 ＜ c ＜ 2.5；

(b3) time updates, status predication；

(b4) measurement updaue, state estimation: employ the QR decomposition in linear algebra in this step, Cholesky decomposes, with State covariance matrix is directly propagated and updated to the form of Cholesky factoring, thus enhances state covariance matrix more Numerical stability during Xin, it is ensured that the orthotropicity of covariance matrix；

(c) importance sampling weight computing: make significance distribution functionSampling particleN () represents normal distribution；Pass through respectively Renewal weights and normalization weights, i=1 ..., N,

In formula,For the estimated value of system state amount,For k moment i-th status prediction information；

(d) resampling: arranging thresholding sample points is N_th, generally it being chosen as N/2 or N/3, effective sample is counted as N_eff, work as N_eff ＜ N_thTime, to particle collectionResampling, obtains new particle collectionAnd reset weights For

E () state updates:

In formula,Estimated value for current time system state amount.