CN103528587A - Autonomous integrated navigation system - Google Patents

Abstract

The invention relates to an autonomous integrated navigation system which belongs to the technical field of navigation systems. The SINS (Strapdown Inertial Navigation System)/SAR (Synthetic Aperture Radar)/CNS (Celestial Navigation System) integrated navigation system takes SINS as a main navigation system and SAR and CNS as aided navigation systems and is established by the following steps: firstly, designing SINS/SAR and SINS/CNS navigation sub-filters, calculating to obtain two groups of local optimal estimation values and local optimal error covariance matrixes of the integrated navigation system state, then transmitting the two groups of local optimal estimation values into a main filter by a federal filter technology for fusion to obtain an overall optimal estimation value and an overall optimal error covariance matrix, and finally, performing real-time correction on the error according to the overall optimal estimation value so as to obtain an optimal estimation fusion algorithm of the SINS/SAR/CNS integrated navigation system. The autonomous integrated navigation system, disclosed by the invention, is less in calculation amount and high in reliability, is applicable to aircrafts in near space, aircrafts flying back and forth in the aerospace, aircrafts for carrying ballistic missiles, orbit spacecrafts and the like, and has wide application prospect.

Description

Independent combined navigation system

Technical field

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

Background technique

Independent navigation refers under conditions of not depending on ground staff's observing and controlling, the position and speed of accurate determining aircraft that can be autonomous.That is when aircraft is in a unknown, complicated, dynamic non-structure environment, under conditions of nobody intervenes, by environment sensing, desired destination can be reached, while the consumption etc. of time or energy can be reduced to the greatest extent.Independent navigation sensor has 4 features: autonomous, real-time, passive and not against terrestrial information.

Independent navigation is the research topic new, with strong challenge actually proposed towards China's military spacecraft (military satellite, military aircraft and Cruise Missile etc.) development.It is an inexorable trend that the direction of military spacecraft airmanship towards independent combined navigation, which is developed,.

Autonomous navigation technology includes inertial navigation, celestial navigation, radionavigation, satellite navigation, earth-magnetism navigation, landform/image and the navigation of various visual informations.Inertial navigation system is, then by integral calculation, to obtain the navigational parameters such as the Position And Velocity of carrier using the acceleration of inertance elements sensitivity vehicle during the motion such as accelerometers.The advantages of inertial navigation system, is entirely autonomous formula, and strong security, the interference vulnerable to external condition and human factor, not round-the-clock, is not limited by weather, but disadvantage is that error can accumulate over time.Celestial navigation system, which using optics or radio telescope receives celestial body and emits the electromagnetic wave come, removes tracking celestial body, and attitude measurement accuracy height, high-altitude and spacecraft for rarefaction of air are ideal navigation modes.Radio navigation system utilizes radio technology measure and navigation parameter, it tests the speed including Doppler effect, positioned etc. with radar range finding and interception, with guidance station, its output information is mainly carrier positions, for precision navigation system, its positioning accuracy is not high, and working range is limited by earth station overlay area.Satellite navigation system is celestial navigation and radionavigational conjugate, classical satellite navigation system includes GPS system, " Galileo " system in Europe and the Beidou system in China in the U.S., has many advantages, such as navigation accuracy height, using simple, but the quantity and geometry of the visible star of positioning accuracy heavy dependence are distributed, region poor for observing environment, because visible star number amount causes precision to be greatly reduced less, and signal is easily disturbed or covers.Earth-magnetism navigation technology has many advantages, such as strong antijamming capability, without accumulated error as a kind of passive autonomous navigation method, the disadvantage is that precision is poor, is more suitable for cruise missile, surface vessel, the auxiliary navigation methods such as device of diving in water.

Obviously, existing any single navigation system is all difficult to fully achieve high-precision independent navigation.High-precision independent navigation needs are completed by the integrated navigation system that multiple airborne sensors are constituted, and the key technology realized is multi-sensor information fusion.The multi-sensor information fusion research field very wide as the frontier nature, prospect to emerge recently, is widely used in the integrated disposal processing of multi-source information.It fully utilizes the different characteristics of multiple types sensor, it can be with multi-faceted Overall Acquisition target different attribute information, the coverage area of autonomous navigation system temporally and spatially is improved, the service efficiency of navigation sensor information is improved and increases the confidence level of information.The appearance of especially various filtering algorithms, provides theoretical basis and mathematical tool for integrated navigation system.

Currently, combining the filtering method that guiding systems use in engineering is mainly Kalman filtering (Kalman Filter, KF) and Extended Kalman filter (Extended Kalman Filter, EKF).KF requires system mathematic model to be necessary for linearly, when integrated navigation system model has nonlinear characteristic, still being described integrated navigation system using linear model and being filtered using KF, it will cause linear model approximate error.

Although EKF is widely applied in integrated navigation system nonlinear filtering, but it still has theoretical limitation, it is in particular in: (1) when mission nonlinear degree is more serious, the higher order term for ignoring Taylor expansion will cause linearized stability to increase, and cause the filtering error of EKF to increase and even dissipate；(2) Jacobian matrix is sought complicated, computationally intensive, is difficult to carry out in practical applications, even hardly results in the Jacobian matrix of nonlinear function sometimes；(3) EKF handles the model error in state equation as process noise, and is assumed to be white Gaussian noise, this is not consistent with the actual noise situation of integrated navigation system；Meanwhile EKF is derived by based on KF, is required the statistical property of system initial state stringent.Therefore EKF is very poor about the probabilistic robustness of system model.

Non-linear filtering methods and the interacting multiple model algorithms such as the model prediction filtering (MPF), particle filter (PF) and the Unscented kalman filtering (UKF) that occur in the recent period, the Nonlinear Filtering Problem and in terms of overcome model in handling integrated navigation system, there is respective original advantage.Although they achieve certain theoretical result in integrated navigation system application aspect, but there are the following problems merits attention: (1) selection of importance function directly affects the performance height of PF, and the Common Criteria for carrying out the selection of PF importance function studies great meaning；It is directed to many practical problems at present, the innovatory algorithm of many selection importance functions occurs, but the importance function selection method being applied in integrated navigation system is less, needs further theory analysis；(2) though classical method for resampling can effectively overcome particle degeneracy, calculation amount increases into series with population and is incremented by, and system real time is deteriorated, and how to solve realizability of the particle filter in integrated navigation system as main problem；(3) there is presently no any mathematical theories makes answer to PF the problem of which kind of condition can restrain, therefore the performance of evaluation and analysis PF in integrated navigation system application also just becomes very difficult.

For UT transformation and UKF, although the precision of available UT transformation proves that UKF algorithm cannot still provide stability analysis as EKF at present, its application in integrated navigation system is affected.The sampling policy of UKF there are many kinds of, or precision is low, can not be accurately obtained nonlinear system higher order term information, bad to the filter effect of non-Gaussian filtering.Precision is high, but calculates excessively complicated and real-time is poor, and UKF algorithm is caused to realize difficulty in integrated navigation system.

It can be seen that, existing filtering technique cannot fully meet the requirement of high-precision independent navigation, it needs to further investigate independent navigation high precision nonlinear algorithm and generating date technology, seek new way to solve basic theories and the basic technology problem of military spacecraft independent navigation, to further increase the fight capability of military spacecraft.

Summary of the invention

It is an object of the invention to a kind of SINS/SAR/CNS independent combined navigation systems based on comprehensive optimal correction, posture, location information progress information processing and the fusion that inertial navigation system, synthetic aperture radar and celestial navigation system are exported using highly-precise filtering technology and multisource information fusion technology, and then navigation error optimally estimate comprehensively, is corrected, to improve the precision of integrated navigation system.

To achieve 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, since SINS can round-the-clock, round-the-clock offer 3 d pose, speed and location information, and good concealment, strong antijamming capability, therefore, using SINS as principle navigation system, SAR, CNS constitute SINS/SAR/CNS integrated navigation system as secondary navigation system.It the steps include:

The subfilter firstly, design SINS/SAR and SINS/CNS navigates, two groups of local optimum estimated values of integrated navigation system state are obtained by calculating

With local Optimal error covariance matrix

Then, using federated filter technology, two groups of local optimum estimated values is sent into senior filter and carry out information fusion, obtain the global best estimates value of system mode

With global optimum's error covariance matrix

；Finally, utilizing the global best estimates value of the state obtainedReal time correction is carried out to the error of Strapdown Inertial Navigation System.Since CNS and SAR cannot export the elevation information of carrier, this system using pressure altimeter output carrier elevation information SINS altitude channel is corrected, with inhibit SINS altitude channel dissipate the problem of.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, setting SINS/SAR/CNS integrated navigation system mathematical model are as follows:

(1) state equation: in SINS/SAR/CNS integrated navigation system, since the positioning accuracy of SAR and CNS is high, error is much smaller than the error of SINS, and does not accumulate at any time.Therefore, in order to reduce system dimension, the navigation error of SAR and CNS is thought of as white Gaussian noise, and be not classified as the quantity of state of integrated navigation system, the systematic error of SINS is only thought of as to the system state amount of SINS/SAR/CNS integrated navigation system.

Navigational coordinate system n of northeast day (E-N-U) the geographic coordinate system g as integrated navigation system is chosen, according to the error equation of inertial navigation system, the 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]^TFor the attitude error quaternary number of SINS；δV=[δV_E、δV_N、δV_U]^TFor the velocity error in the east SINS, north, day direction；δP=[δL,δλ,δh]^TLatitude, longitude and altitude error for SINS；ε=[ε_x、ε_y、ε_x]^TIndicate the random drift of gyro；

It is the constant value biasing of accelerometer.

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

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

In formula, f (x, t) is the state array of system；w(t)=[w_gx, w_gy, w_gz, w_ax, w_ay, w_az]^TIndicate system noise, [w_gx、w_gy、w_gz] be gyro white noise, [w_ax、w_ay、w_az] be accelerometer white noise；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, the horizontal position information and course angle information of carrier can be obtained, strapdown inertial navigation system is resolved by the angular movement information and line motion information exported to gyroscope and acceleration, can obtain the posture, speed and location information of carrier.Since SAR cannot obtain the elevation information of carrier, in order to inhibit SINS altitude channel to dissipate, measured using elevation information of the pressure altimeter to carrier.Therefore, as measurement, the measurement equation of SINS/SAR integrated navigation system is the difference for the carrier height information that course angle information, horizontal position information and the pressure altimeter of the course angle information and location information and synthetic aperture radar output that inertial navigation can be exported export

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]^TTo measure white noise, mean value zero.

2. SINS/CNS subsystem measurement equation: in SINS/CNS algorithm of combined navigation subsystem, inertial navigation system can obtain the attitude quaternion and location information of carrier.Celestial navigation system is observed the attitude quaternion of available carrier and the horizontal position information (longitude and latitude) of carrier by star sensor to celestial body, but the elevation information of carrier cannot be obtained, therefore it needs to introduce height air gauge to be observed the elevation information of carrier, inhibits the diverging of SINS altitude channel.Choose the attitude of carrier quaternary number of inertial navigation system output and the attitude of carrier quaternary number and horizontal position information of location information and celestial navigation system output, and the difference of the carrier height information of height air gauge output, as measurement, the measurement equation of SINS/CNS algorithm of combined navigation subsystem is

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

In formula, h₂It (t) is measurement matrix, v₂(t)=[δq_C0, δ q_C1,δq_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TTo measure white noise, mean value zero.

(3) independent navigation high precision nonlinear filtering algorithm: devising a set of high-precision for being suitable for SINS/CNS/SAR independent combined navigation system, nonlinear filtering algorithm, which includes:

1. the adaptive Unscented particle filter algorithm of robust；2. fade adaptive Unscented particle filter；3. fuzzy robust adaptive particle filter；4. adaptive SVD-UKF filtering algorithm；5. adaptive square root Unscented particle filter.It is specific as follows:

1. the adaptive Unscented particle filter algorithm of robust:

The adaptive Unscented particle filter of robust, the advantages of having fully absorbed Robust filter, robust adaptive-filtering and particle filter, Robust filter principle is combined with UPF by weight factor of equal value and adaptive factor, weight function appropriate and adaptive factor state of a control model information and measurement model information are selected, the influence interfered extremely is inhibited.

The step of robust adaptive Unscented particle filter algorithm, is as follows:

(a) it initializes, N number of particle is extracted according to initial mean value and mean square deviation, at the moment of k=0,

I=1,2 ..., N, setting weight are

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

(b1) equivalence weight is calculated

With adaptive factor α.IGG scheme is selected to construct Modified Equivalent Weight Function, IGG is owned by France to do robust limitation to measuring value in drop weight function, if taking its inverse, be 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)$

It as needed can also be using another expression formula

${\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 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.---\left(14\right)$

Wherein c₀∈ (1,1.5), c₁∈ (3,8),

The mark of tr () representing matrix,For predicted value, i.e.,

It can be seen that weight function and adaptive factor structural form are essentially identical, the two is all important regulatory factor.The former is chosen by the judgement to residual error, and the latter is according to the difference of state estimation and predicted value

To choose.

(b2) Sigma point is calculated, by UKF algorithm more new particle

It obtains

And

Meet

If new samples are

2N+1 Sigma point sampling point be

${\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) scale factor is indicated, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value, and generally (value best for Gaussian Profile is (b2), W to β according to priori knowledge come value_jThe weight for indicating j-th of Sigma point, meets ∑ W_j=1, j=0,1 ... 2N.

(c) weight is calculated $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 it is normalized to ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

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

Acquired results are compared with set threshold, judge the severity of sample degeneracy,

It is smaller, show to degenerate more serious.In this case, resampling can be carried out to previously obtained posterior density, retrieves M new particles, and assign each particle identical weight 1/M.

(e) nonlinear state amount estimated value is calculated.It repeats previous step (b).

Two important regulatory factors are utilized when choosing the importance density function in above-mentioned steps, i.e., weight factor and adaptive factor of equal value.Particle sampler point obtained by after being converted as the two to UT more reasonably distributes useful information, provides better sample distribution function for importance sampling process.

2. fade adaptive Unscented particle filter:

The improved adaptive Unscented particle filter that fades is using particle filter as basic framework, merge fade adaptive filtration theory and UT conversion process, absorb each single algorithm advantage, establish parameter can automatic adjusument importance density fonction, sufficiently efficiently utilize newest measurement information, make it closer to true distribution function, to make filtering algorithm that there is better adaptivity and robustness.

Unscented particle filter algorithm mainly converts to obtain sampled point using UT, realizes the approximation to state vector Posterior distrbutionp.It is different from Monte Carlo method, Unscented particle filter be not it is random sampled from given distribution, be to take a small number of Sigma points determined 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) scale factor is represented, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value.For different system model and noise it is assumed that UT transformation algorithm has different forms, determine that the key of UT transformation algorithm expression formula is determining Sigma point sampling strategy, i.e. Sigma point number, position and weight.

The adaptive Unscented particle filter algorithm that fades uses the memory span of fading factor restriction filter, makes full use of Current observation value constantly to correct predicted value, and unknown or inapt system model parameter, noise statistics parameter etc. are estimated and corrected.Algorithm key step are as follows:

(a) it initializes, when k=0,

Wherein k indicates the moment.Weight is arranged in unification

Wherein k indicates the moment, and N indicates particle number.

(b) Sigma point is calculated, if new samples are

2N+1 Sigma sampled point is calculated, particle is predicted and updated using UKF algorithm,

Wherein each symbolic significance is same as above, and β is generally according to priori knowledge come value (value best for Gaussian Profile is 2), W in following formula_jThe weight for indicating j-th of Sigma point, meets ∑ W_j=1, j=0,1 ... 2N, and carry out time update and measurement updaue.Using fading, adaptive extended kalman filtering thought calculates fading factor, utilizes 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)$

It obtains

As the importance density function of particle sampler, importance sampling is carried out.

(c) from importance density letterIn sampled after, and to each particle calculate weight

$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 weight.

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

(d) estimator is utilized

Judge whether sample degeneracy degree is serious, then posterior density obtained from the above carries out resampling and retrieves M new particles, assigns each particle identical weight again

(e) nonlinear state amount estimated value is calculated.

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

(2) step is returned to, by the state estimation of new observed quantity recursive calculation subsequent time.

3. fuzzy robust adaptive particle filter:

(I) construct equivalence weight based on fuzzy theory: existing stronger Robustness least squares have the valuation of higher adaptivity again in order to obtain, can be divided power according to data residual error are as follows: Bao Quanqu (keeping former observation constant), the area Jiang Quan (making robust limitation to observation) and reject region (Quan Weiling).The step of designing one-dimensional fuzzy controller, constructing weight factor γ is as follows:

(a) it is blurred.The effect of blurring is that the precise volume of input is converted into blurring amount, i.e., by input quantity

(exact value) blurring becomes fuzzy variable (whereinFor standardized residual), convert determining input to the fuzzy set described by degree of membership.

Detailed process: by input variableFuzzy subset be divided into { too big, larger, normal }, be abbreviated as { B_e, B_c, B_n, after input quantization,

Domain be X={ 0,1,2,3,4 }；The fuzzy subset for exporting γ is { minimum, smaller, normal }, is abbreviated as { S_e, S_c, S_n, domain is that Y size is divided into 5 grades, to indicate different values, i.e. Y={ 0,1,2,3,4 }.Respectively to input

Fuzzy quantization is carried out with output γ.

(b) according to the intuitive thought reasoning of people and practical experience, by input quantityWith the relationship of output quantity weight factor γ, fuzzy control rule is designed.If

Too big, then γ is minimum；If

Larger, then γ is smaller；If

Normally, then γ is normal.

It can determine that fuzzy relation is according to fuzzy rule

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

Wherein, "×" indicates 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 variable

Fuzzy subset and fuzzy relationship matrix r by fuzzy reasoning obtain weight factor be γ a fuzzy set, obtain final fuzzy control quantity.

(d) it obtains fuzzy control quantity progress ambiguity solution accurately to export control amount, i.e. weight factor γ.The process that fuzzy reasoning result is converted into exact value is known as anti fuzzy method.In anti fuzzy method treatment process, using maximum membership grade principle.

(II) robust adaptive particle filter algorithm is obscured

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

(a) it initializes.At the moment of k=0, sampled out N number of particle according to emphasis density, it is assumed that each particle sampled out is used

It indicates, enables k=1；

(b) using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model.It is by the differentiation statistic of structure's variable state model error of prediction residual

$\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 i-th of status prediction information of k moment, pass through

It finds out；

Indicate prediction residual

Covariance matrix；Tr () is Matrix Calculating trace operator.Based on prediction residual differentiate statistic adaptive factor function be

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally takes 1.0 < c < 2.5.

(c) it updates:

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

By

The particle at 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)$

Update weight and normalization weight, i=1,2 ..., N.

(d) resampling: being ranked up the weight of all particles according to descending, and setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collection

Resampling obtains new particle collection

And it resets weight and is

(e) it 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)$

It calculates the filtering density at k moment and carries out re-sampling, enable k=k+1, return (b).

The adaptive factor α of robust adaptive-filtering_kNeither individually handle model error covariance matrix

The covariance matrix of previous epoch state parameter vector estimation is not handled individually yet

But act on the equivalent covariance matrix of whole state parameter forecast vector

Since robust adaptive-filtering uses Robust filter principle to observation information, when observation is deposited when abnormal, as a whole by dynamic model information, using unified adaptive factor motivation of adjustment model information to the overall contribution of state parameter, to obtain reliable filter result.

4. adaptive SVD-UKF filtering algorithm:

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

It is assumed 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 is known as the singular value of matrix A, and the column vector of U and V are referred to as 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, A=USU^T, left singular vector is equal with right singular vector at this time, so as to reduce calculation amount.

(II) determination of statistic and adaptive factor: using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model.Prediction residual (new breath) can more reflect the disturbance that dynamical system is subject to containing without the modified state of observation information.Prediction residual

It 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 residual

Structural 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/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

In formula (49),

Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator.

Adaptive factor function is selected 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), α_kIndicate adaptive factor, c is empirical, under normal conditions 1 < c < 2.5.

(III) adaptive SVD-UKF algorithm steps:

Adaptive SVD-UKF algorithm steps are as follows:

(a) it initializes: to the parameter initialization in state equation, and calculating the weight coefficient w of Sigma point mean value and covariance^(m)、w^(c)。

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

(c) time updates:

(d) it measures and updates；

5. adaptive 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 k moment n dimension state vector, Φ_{K, k-1}For n × n-state transfer matrix, W_kFor system noise vector, covariance matrix is

Observational equation is

Y_k=H_kX_k+e_k    (68)

In formula, Y_kIt is k moment m dimension observation vector, H_kDesign matrix, e are tieed up for m × n_kFor observation noise vector, covariance matrix is ∑_k。W_k, W_j, e_k, e_j(j ≠ k) is irrelevant.

The probability density function of known state is p (X₀|Y₀)=p(X₀), then according to Bayesian Estimation theory, the status predication equation and state renewal equation of nonlinear and time-varying system 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) be the k-1 moment posterior density；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, it can be acquired 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 certain dynamical systems.Monte Carlo method based on stochastical sampling can convert integral operation to the summation operation of finite sample point, can obtain the approximate expression form of posterior probability density function.Posterior density p (X in practice_k|Y_1:k) it may be multivariable, non-standard probability distribution, it needs to sample it by importance sampling algorithm, thus need to construct importance function.Appropriate importance function is selected to can effectively solve the sample degeneracy problem of particle filter.

This project generates the importance density function of particle filter using UKF algorithm, which makes full use of newest observation data, corrects error caused by kinetic model and noise statistics parameter in real time.The step of adaptive square root UPF, is as follows.

(a) it initializes (k=0): randomly selecting N number of primary

(i=1,2 ..., N).Assuming that ${\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,

With

I-th of particle of initial time and its estimated value are respectively indicated,

Indicate i-th of Cholesky factoring of initial time,

Indicate the initialization weight of i-th of particle, the Cholesky decomposition operator of chol { } representing matrix.

(b) each particle is updated using adaptive Deep space algorithmIt obtains

Indicate the covariance of i-th of particle of k moment.

(b1) Sigma point and weight are calculated；

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

Prediction residual (or new breath) can more reflect the disturbance of dynamical system containing without the modified state of observation information.I-th of prediction residual vector of k moment

It is expressed as

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

It is by the differentiation statistic of structure's variable state model error of prediction residual

$\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 i-th of status prediction information of k moment, pass throughIt finds out,Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator.

Based on prediction residual differentiate statistic adaptive factor function be

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally takes 1.0 < c < 2.5.

(b3) time updates (status predication)；

(b4) measurement updaue (state estimation)；

The QR in linear algebra has been used to decompose in the step, Cholesky is decomposed, state covariance matrix is directly propagated and updated in the form of Cholesky factoring, to enhance the numerical stability in state covariance matrix renewal process, the orthotropicity of covariance matrix ensure that.Wherein, the QR of qr { } representing matrix is decomposed.

(c) importance sampling weight computing: significance distribution function is enabled

Sample particle

N () indicates 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})},$

Update weight and normalization weight, i=1 ..., N.

(d) resampling: setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collection

Resampling obtains new particle collection

And it resets weight and is

(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 measures the line movement and angular movement of carrier using gyro and accelerometer, obtain the angle increment and specific force increment of carrier, then navigation calculating is carried out using 3 d pose more new algorithm, speed more new algorithm and location updating algorithm, the real-time 3 d pose information of carrier, velocity information and location information can be obtained.Synthetic aperture radar passes through Range-Doppler Imaging principle, the high-resolution SAR image in target area can be obtained in real time, then images match, the course angle and horizontal position information of available carrier are carried out using the reference map in the image matching algorithm based on feature and airborne digital map library.Since synthetic aperture radar can not obtain the elevation information of carrier, and the altitude channel of strapdown inertial navigation system is diverging, therefore measures the real-time height of carrier using pressure altimeter, to solve the problems, such as that inertial navigation altitude channel is not considerable.Using the method filtered indirectly, using the course angle and horizontal position information of the course angle of Strapdown Inertial Navigation System output and location information and synthetic aperture radar output and the difference of the elevation information of pressure altimeter output as measurement, it is then fed into filter and carries out nonlinear filtering, the optimal estimation value for obtaining integrated navigation system state, recycles the estimated value to be modified the error of strapdown inertial navigation system.Motion compensation is carried out to synthetic aperture radar using revised SINS navigation information simultaneously, 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 information, these information are resolved using the navigation computation of inertial navigation, the real-time 3 d pose information of carrier, velocity information and location information can be obtained.Star sensor in celestial navigation system can export the right ascension for being observed star, declination and swing angle, these information resolve with the horizontal position information and posture information of available carrier.Since celestial navigation system cannot obtain the elevation information of carrier, and the altitude channel of strapdown inertial navigation system is diverging, therefore, using the height of pressure altimeter measurement carrier, is corrected to the altitude channel of strapdown inertial navigation system.

Since the star sensor of celestial navigation system is connected on carrier, ignore installation error, then star sensor coordinate system is overlapped with carrier coordinate.The altitude of the heavenly body angle and azimuth that star sensor observes can obtain the starlight unit direction vector of celestial body by calculating, can calculate attitude matrix of the carrier system relative to inertial system using attitude algorithm algorithm

Further according to

Can acquire carrier system to navigation system pose transformation matrix, thus obtain carrier system b that celestial navigation system is calculated to navigate be n attitude quaternion.The mathematical platform benchmark that the unit direction vector that star sensor obtains is provided by SINS

It is coordinately transformed, then utilizes 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_sinsWith the q of celestial navigation system output_cns, L_cns, λ_cnsThe carrier height information h that information and pressure altimeter measure_eIt is poor to make, and is sent in SINS/CNS filter and is filtered calculating, can obtain the optimal estimation value of state.Finally, being corrected with navigational parameter error of the optimal estimation value of state to Strapdown Inertial Navigation System, Strapdown Inertial Navigation System celestial navigation system is made to provide more accurate mathematical platform benchmark.

The beneficial effects of the present invention are: the present invention proposes a kind of SINS/SAR/CNS independent combined navigation system based on comprehensive optimal correction, it can apply to the Altitude Long Endurance Unmanned Air Vehicle for being unsatisfactory for dynamics of orbits characteristic, realize the parsing astrofix based on starlight refraction；Posture, location information progress information processing and the fusion that inertial navigation system, synthetic aperture radar and celestial navigation system are exported using highly-precise filtering algorithm and information fusion technology, and then navigation error optimally estimate comprehensively, is corrected, improve the precision of integrated navigation system；, high reliability small with calculation amount, and near space vehicle can also be applied to, empty day shuttle vehicle, ballistic missile, become the aircraft such as rail spacecraft, it has broad application prospects.

Detailed description of the invention

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

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

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

Specific embodiment

The specific embodiment of the invention is further described with reference to the accompanying drawing.

Embodiment

SINS/SAR integrated navigation schematic diagram is as shown in Figure 1.Fig. 2 is SINS/CNS algorithm of combined navigation subsystem principle in the invention.Fig. 3 is SINS/SAR/CNS integrated navigation schematic diagram in the invention.

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

(1) state equation: in SINS/SAR/CNS integrated navigation system, since the positioning accuracy of SAR and CNS is high, error is much smaller than the error of SINS, and does not accumulate at any time.Therefore, in order to reduce system dimension, the navigation error of SAR and CNS is thought of as white Gaussian noise, and be not classified as the quantity of state of integrated navigation system, the systematic error of SINS is only thought of as to the system state amount of SINS/SAR/CNS integrated navigation system.

Navigational coordinate system n of northeast day (E-N-U) the geographic coordinate system g as integrated navigation system is chosen, according to the error equation of inertial navigation system, the 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₃]^TFor the attitude error quaternary number of SINS；δV=[δV_E、δV_N、δV_U]^TFor the velocity error in the east SINS, north, day direction；δP=[δL,δλ,δh]^TLatitude, longitude and altitude error for SINS；ε=[ε_x、ε_y、ε_x]^TIndicate the random drift of gyro；

It is the constant value biasing of accelerometer.

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

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

In formula, f (x, t) is the state array of system；W (t)=[w_gx, w_gy, w_gz, w_ax, w_ay, w_az]^TIndicate system noise, [w_gx、w_gy、w_gz] be gyro white noise, [w_ax、w_ay、w_az] be accelerometer white noise；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, the horizontal position information and course angle information of carrier can be obtained, strapdown inertial navigation system is resolved by the angular movement information and line motion information exported to gyroscope and acceleration, can obtain the posture, speed and location information of carrier.Since SAR cannot obtain the elevation information of carrier, in order to inhibit SINS altitude channel to dissipate, measured using elevation information of the pressure altimeter to carrier.Therefore, the difference for the carrier height information that course angle information, horizontal position information and the pressure altimeter of the course angle information and location information and synthetic aperture radar output that inertial navigation can be exported export 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_IThe course angle of respectively SINS output, latitude, longitude and altitude information, ψ_S、L_SAnd λ_SCourse angle, latitude and the longitude information of respectively SAR output, h_eFor the elevation information of pressure altimeter output, δ indicates every corresponding error.

Attitude error angle (the course error angle δ ψ of Eulerian angles form_I, pitch error angle δ θ_IWith roll error angle δ γ_I) to describe preferably to navigate be n (i.e. ideal platform system T) to the error that the navigation that strapdown inertial navigation system navigational computer calculates is between c (i.e. actual platform system P).It is that c passes through rotation, the transition matrix of available c system to n system by the calculated navigation of navigational computerFor

$\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 using attitude quaternion error angle to indicate that ideal navigation is that n and calculating are navigated when being the error angle between c, if attitude error quaternary number is δ q=[δ q₀, δ q₁, δ q₂, δ q₃]^T, then the transition matrix of the c system that is indicated with attitude error quaternary number to n system

For

$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 matrix

With matrix

All describe that the navigation that navigational computer is calculated is c and ideal navigation is the transition matrix between n, therefore formula (6) is equal with formula (7), and the transformational relation that peer-to-peer can be calculated between the attitude error angle of Eulerian angles form and 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]^TTo measure white noise, mean value zero.

2. SINS/CNS subsystem measurement equation: in SINS/CNS algorithm of combined navigation subsystem, inertial navigation system can obtain the attitude quaternion and location information of carrier.Celestial navigation system is observed the attitude quaternion of available carrier and the horizontal position information (longitude and latitude) of carrier by star sensor to celestial body, but the elevation information of carrier cannot be obtained, therefore it needs to introduce height air gauge to be observed the elevation information of carrier, inhibits the diverging of SINS altitude channel.Choose the attitude of carrier quaternary number of inertial navigation system output and the attitude of carrier quaternary number and horizontal position information of location information and celestial navigation system output, and the difference of the carrier height information of height air gauge output as measurement, then measurement can be expressed as

$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_IThe respectively latitude of inertial navigation system output, longitude and altitude information；q_C0、q_C1、q_C2、q_C3For the attitude quaternion of celestial navigation system output, L_C、λ_CThe respectively latitude and longitude information of celestial navigation output；h_eFor the elevation information of pressure altimeter output；δ indicates every corresponding error.

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

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

In formula, h₂It (t) is measurement matrix, v₂(t)=[δq_C0, δ q_C1,δq_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TTo measure white noise, mean value zero.

(3) independent navigation high precision nonlinear filtering algorithm: devising a set of high-precision for being suitable for SINS/CNS/SAR independent combined navigation system, nonlinear filtering algorithm, which includes:

1. the adaptive Unscented particle filter algorithm of robust；2. fade adaptive Unscented particle filter；3. fuzzy robust adaptive particle filter；4. adaptive SVD-UKF filtering algorithm；5. adaptive square root Unscented particle filter.It is specific as follows:

1. the adaptive Unscented particle filter algorithm of robust:

The adaptive Unscented particle filter of robust, the advantages of having fully absorbed Robust filter, robust adaptive-filtering and particle filter, Robust filter principle is combined with UPF by weight factor of equal value and adaptive factor, weight function appropriate and adaptive factor state of a control model information and measurement model information are selected, the influence interfered extremely is inhibited.

The step of robust adaptive Unscented particle filter algorithm, is as follows:

(a) it initializes, N number of particle is extracted according to initial mean value and mean square deviation, at the moment of k=0,

I=1,2 ..., N, setting weight are

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

1) equivalence weight is calculatedWith adaptive factor α.IGG scheme is selected to construct Modified Equivalent Weight Function, IGG is owned by France to do robust limitation to measuring value in drop weight function, if taking its inverse, be 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)$

It as needed can also be using another expression formula

$\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 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.---\left(14\right)$

Wherein c₀∈ (1,1.5), c₁∈ (3,8),

The mark of tr () representing matrix,

For predicted value, i.e.,

It can be seen that weight function and adaptive factor structural form are essentially identical, the two is all important regulatory factor.The former is chosen by the judgement to residual error, and the latter is according to the difference of state estimation and predicted value

To choose.

2) Sigma point is calculated, by UKF algorithm more new particle

It obtains

And

Meet

If new samples are

2N+1 Sigma point sampling point be

${\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) scale factor is indicated, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value, and β is generally according to priori knowledge come value (value best for Gaussian Profile is 2), W_jThe weight for indicating j-th of Sigma point, meets ∑ W_j=1, j=0,1 ... 2N.

Particle is predicted and is updated using UKF algorithm:

$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)$

It is available by formula (26)~(27)

Sampling particle valuation and variance are calculated using equivalence weight and adaptive factor

${\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 through adaptive factor α_kWith

It can influence and adjust

Make the importance density function closer to actual distribution.It is obtained according to formula (16)~(19)

As the importance density function of particle sampler, then carry out importance sampling.From formula (18) as can be seen that when measurement model is deposited when abnormal, equivalent weight matrix

Element reduces, and is reduced when parameter estimation using measurement information rate, reduces influence of the exception information to valuation.Conversely, parameter estimation increases the utilization rate of useful measurement information；Similarly, when state model is deposited when abnormal, adaptive factor α_kReduce, reduced when parameter estimation using status prediction information rate, weakens the interference of model exception, it is on the contrary also to set up.If equivalence weight

And when α=0,

With

As UKF algorithm obtained sample average and variance.

(c) weight is calculated $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 it is normalized to ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i.$

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

Acquired results are compared with set threshold, judge the severity of sample degeneracy,

It is smaller, show to degenerate more serious.In this case, resampling can be carried out to previously obtained posterior density, retrieves M new particles, and assign each particle identical weight 1/M.

(e) nonlinear state amount estimated value is calculated.

It repeats previous step (b).

Two important regulatory factors are utilized when choosing the importance density function in above-mentioned steps, i.e., weight factor and adaptive factor of equal value.Particle sampler point obtained by after being converted as the two to UT more reasonably distributes useful information, provides better sample distribution function for importance sampling process.

2. fade adaptive Unscented particle filter: the improved adaptive Unscented particle filter that fades is using particle filter as basic framework, merge fade adaptive filtration theory and UT conversion process, absorb each single algorithm advantage, establish parameter can automatic adjusument importance density fonction, sufficiently efficiently utilize newest measurement information, make it closer to true distribution function, to make filtering algorithm that there is better adaptivity and robustness.

Unscented particle filter algorithm mainly converts to obtain sampled point using UT, realizes the approximation to state vector Posterior distrbutionp.It is different from Monte Carlo method, Unscented particle filter be not it is random sampled from given distribution, be to take a small number of Sigma points determined 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) scale factor is represented, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value.For different system model and noise it is assumed that UT transformation algorithm has different forms, determine that the key of UT transformation algorithm expression formula is determining Sigma point sampling strategy, i.e. Sigma point number, position and weight.

The adaptive Unscented particle filter algorithm that fades uses the memory span of fading factor restriction filter, makes full use of Current observation value constantly to correct predicted value, and unknown or inapt system model parameter, noise statistics parameter etc. are estimated and corrected.Algorithm key step are as follows:

(a) it initializes, when k=0,

Wherein k indicates the moment.Weight is arranged in unification

Wherein k indicates the moment, and N indicates particle number.

(b) Sigma point is calculated, if new samples are

2N+1 Sigma sampled point is calculated, particle is predicted and updated using UKF algorithm,

Wherein each symbolic significance is same as above, and β is generally according to priori knowledge come value (value best for Gaussian Profile is 2), W in following formula_jThe weight for indicating j-th of Sigma point, meets ∑ 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)$

It measures and updates

$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)$

Fading factor is calculated using the adaptive extended kalman filtering thought that fades at this time, utilizes 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)$

It obtains

As the importance density function of particle sampler, importance sampling is carried out.

(c) from importance density letter

In sampled after, and to each particle calculate weight

$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 weight.

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

(d) estimator is utilized

Judge whether sample degeneracy degree is serious, then posterior density obtained from the above carries out resampling and retrieves M new particles, assigns each particle identical weight again

(e) nonlinear state amount estimated value is calculated.

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

(2) step is returned to, by the state estimation of new observed quantity recursive calculation subsequent time.

3. fuzzy robust adaptive particle filter:

(I) equivalence weight is constructed based on fuzzy theory:

Existing stronger Robustness least squares have the valuation of higher adaptivity again in order to obtain, can be divided power according to data residual error are as follows: Bao Quanqu (keeping former observation constant), the area Jiang Quan (making robust limitation to observation) and reject region (Quan Weiling).The step of designing one-dimensional fuzzy controller, constructing weight factor γ is as follows:

(a) it is blurred.The effect of blurring is that the precise volume of input is converted into blurring amount, i.e., by input quantity

(exact value) blurring becomes fuzzy variable (wherein

For standardized residual), convert determining input to the fuzzy set described by degree of membership.

Detailed process: by input variableFuzzy subset be divided into { too big, larger, normal }, be abbreviated as { B_e, B_c, B_n, after input quantization,

Domain be X={ 0,1,2,3,4 }；The fuzzy subset for exporting γ is { minimum, smaller, normal }, is abbreviated as { S_e, S_c, S_n, domain is that Y size is divided into 5 grades, to indicate different values, i.e. Y={ 0,1,2,3,4 }.Respectively to inputFuzzy quantization is carried out with output γ.

(b) according to the intuitive thought reasoning of people and practical experience, by input quantity

With the relationship of output quantity weight factor γ, fuzzy control rule is designed.If

Too big, then γ is minimum；If

Larger, then γ is smaller；If

Normally, then γ is normal.

It can determine that fuzzy relation is according to fuzzy rule

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

Wherein, "×" indicates 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 variable

Fuzzy subset and fuzzy relationship matrix r by fuzzy reasoning obtain weight factor be γ a fuzzy set, obtain final fuzzy control quantity.

(d) it obtains fuzzy control quantity progress ambiguity solution accurately to export control amount, i.e. weight factor γ.The process that fuzzy reasoning result is converted into exact value is known as anti fuzzy method.In anti fuzzy method treatment process, using maximum membership grade principle.

(II) robust adaptive particle filter algorithm is obscured

The step of fuzzy robust adaptive particle filter (Fuzzy Robust Adaptively Particle Filter, FRAPF) algorithm, is as follows: (a) initializing.At the k=0 moment, sampled out N number of particle according to emphasis density, it is assumed that each particle sampled out is used

It indicates, enables k=1；(b) using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model.

Error equation are as follows:

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

In formula, A_kFor coefficient matrix；Z_kFor observation vector, weight matrix P_k；V_kFor residual vector；

For state parameter vector.The risk function is taken to be

${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 without the modified state of observation information, can more reflect the disturbance of dynamical system.I-th of prediction residual vector of k moment

It is expressed as

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

It is by the differentiation statistic of structure's variable state model error of prediction residual

$\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 i-th of status prediction information of k moment, pass through

It finds out；Indicate prediction residualCovariance matrix；Tr () is Matrix Calculating trace operator.Based on prediction residual differentiate statistic adaptive factor function be

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally takes 1.0 < c < 2.5.

(c) it updates:

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

By

The particle at 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)$

Update weight and normalization weight, i=1,2 ..., N.

(d) resampling: being ranked up the weight of all particles according to descending, and setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collection

Resampling obtains new particle collection

And it resets weight and is

(e) it 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)$

It calculates the filtering density at k moment and carries out re-sampling, enable k=k+1, return (b).

The adaptive factor α of robust adaptive-filtering_kNeither individually handle model error covariance matrix

The covariance matrix of previous epoch state parameter vector estimation is not handled individually yet

But act on the equivalent covariance matrix of whole state parameter forecast vector

Since robust adaptive-filtering uses Robust filter principle to observation information, when observation is deposited when abnormal, as a whole by dynamic model information, using unified adaptive factor motivation of adjustment model information to the overall contribution of state parameter, to obtain reliable filter result.

4. adaptive SVD-UKF filtering algorithm:

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

It is assumed 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 is known as the singular value of matrix A, and the column vector of U and V are referred to as 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, A=USU^T, left singular vector is equal with right singular vector at this time, so as to reduce calculation amount.

(II) determination of statistic and adaptive factor: using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model.Prediction residual (new breath) can more reflect the disturbance that dynamical system is subject to containing without the modified state of observation information.Prediction residual

It 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 residual

Structural 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/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2}---\left(49\right)$

In formula (49),

Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator.

Adaptive factor function is selected 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), α_kIndicate adaptive factor, c is empirical, under normal conditions 1 < c < 2.5.

(III) adaptive SVD-UKF algorithm steps: adaptive SVD-UKF algorithm steps are as follows:

(a) it initializes: to the parameter initialization in state equation, and calculating the weight coefficient w of Sigma point mean value and covariance^(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, α indicates Sigma point with respect to the diffusion of mean value (usual 1e-4≤α≤1), and β is the parameter about system prior information.To Gaussian Profile, β=2 are best, P₀It is 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 more appropriate value is

S_{I, k}For the singular value decomposition factor.

(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 { } is SVD decomposition operator, the adaptive factor α in formula (56)_kIt is determined by formula (50), uses α_kCorrect 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) it 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)$

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

5. adaptive 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 k moment n dimension state vector, Φ_{K, k-1}For n × n-state transfer matrix, W_kFor system noise vector, covariance matrix is

Observational equation is

Y_k=H_kX_k+e_k    (68)

In formula, Y_kIt is k moment m dimension observation vector, H_kDesign matrix, e are tieed up for m × n_kFor observation noise vector, covariance matrix is ∑_k。W_k, W_j, e_k, e_j(j ≠ k) is irrelevant.

The probability density function of known state is p (X₀|Y₀)=p(X₀), then according to Bayesian Estimation theory, the status predication equation and state renewal equation of nonlinear and time-varying system 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:kIt -1) is the posterior density at k-1 moment；

p(X_k|Y_1:kIt -1) is prior distribution, p (Y_k|X_k) it is likelihood density, p (Y_k|Y_1:k-1) it is normaliztion constant, it can be acquired 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 certain dynamical systems.Monte Carlo method based on stochastical sampling can convert integral operation to the summation operation of finite sample point, can obtain the approximate expression form of posterior probability density function.Posterior density p (X in practice_k|Y_1:k) it may be multivariable, non-standard probability distribution, it needs to sample it by importance sampling algorithm, thus need to construct importance function.Appropriate importance function is selected to can effectively solve the sample degeneracy problem of particle filter.

This project generates the importance density function of particle filter using UKF algorithm, which makes full use of newest observation data, corrects error caused by kinetic model and noise statistics parameter in real time.The step of adaptive square root UPF, is as follows.

(a) it initializes (k=0): randomly selecting N number of primary

(i=1,2 ..., N).Assuming that ${\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,

With

I-th of particle of initial time and its estimated value are respectively indicated,

Indicate i-th of Cholesky factoring of initial time,

Indicate the initialization weight of i-th of particle, the Cholesky decomposition operator of chol { } representing matrix.

(b) each particle is updated using adaptive Deep space algorithmIt obtains

Indicate the covariance of i-th of particle of k moment.

(b1) Sigma point and weight 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 indicates that system mode dimension, α indicate that Sigma point surrounds the diffusion (usual 1e-4≤α≤1) of mean value, and β is the parameter about system prior information, best to Gaussian Profile β=2,

Indicate j-th of Sigma point, W_jFor its weight.

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

Prediction residual (or new breath) can more reflect the disturbance of dynamical system containing without the modified state of observation information.I-th of prediction residual vector of k momentIt is expressed as

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

It is by the differentiation statistic of structure's variable state model error of prediction residual

$\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 i-th of status prediction information of k moment, pass throughIt finds out,

Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator.

Based on prediction residual differentiate statistic adaptive factor function be

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally 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,

With

The state transfer and its estimation of i-th of particle of k moment to k-1 moment are respectively indicated, cholupdate { } indicates Cholesky factoring update operator, and formula (81) utilizes adaptive factor

Correct 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)$

The QR in linear algebra has been used to decompose in the step, Cholesky is decomposed, state covariance matrix is directly propagated and updated in the form of Cholesky factoring, to enhance the numerical stability in state covariance matrix renewal process, the orthotropicity of covariance matrix ensure that.Wherein, the QR of qr { } representing matrix is decomposed.

(c) importance sampling weight computing: significance distribution function is enabledSample particle

N () indicates 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})},$

Update weight and normalization weight, i=1 ..., N.

(d) resampling:

Setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collection

Resampling obtains new particle collection

And it resets weight and is

(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 (7)

1. a kind of independent combined navigation system, it is characterised in that: using SINS as principle navigation system, SAR, CNS constitute SINS/SAR/CNS integrated navigation system as secondary navigation system, the specific steps of which are as follows:

The subfilter firstly, design SINS/SAR and SINS/CNS navigates, two groups of local optimum estimated values of integrated navigation system state are obtained by calculating

With local Optimal error covariance matrix

Then, using federated filter technology, two groups of local optimum estimated values is sent into senior filter and carry out information fusion, obtain the global best estimates value of system mode

With global optimum's error covariance matrix

Finally, utilizing the global best estimates value of the state obtained

Real time correction is carried out to the error of Strapdown Inertial Navigation System；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..$

2. independent combined navigation system according to claim 1, it is characterised in that: the SINS/SAR/CNS integrated navigation system, mathematical model include state equation, measurement equation and independent navigation high precision nonlinear filtering algorithm:

(1) state equation: being thought of as white Gaussian noise for the navigation error of SAR and CNS, and be not classified as the quantity of state of integrated navigation system, and the systematic error of SINS is only thought of as to the system state amount of SINS/SAR/CNS integrated navigation system；The state equation of SINS/SAR/CNS integrated navigation system are as follows:

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

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

$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];$

$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];$

(2) measurement equation, including following measurement equation:

1. SINS/SAR subsystem measurement equation: the difference for the carrier height information that course angle information, horizontal position information and the pressure altimeter of the course angle information and location information and synthetic aperture radar output that inertial navigation can be exported export is as measurement, the measurement equation of SINS/SAR integrated navigation system are as follows:

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]^TTo measure white noise, mean value zero；

2. SINS/CNS subsystem measurement equation: the measurement equation of SINS/CNS algorithm of combined navigation subsystem are as follows:

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

In formula, h₂It (t) is measurement matrix, v₂(t)=[δq_C0, δ q_C1,δq_C2, δ q_C3, δ L_C, δ λ_C, δ h_e]^TTo measure white noise, mean value zero；

(3) independent navigation high precision nonlinear filtering algorithm: devising a set of high-precision for being suitable for SINS/CNS/SAR independent combined navigation system, nonlinear filtering algorithm, which includes: the 1. adaptive Unscented particle filter algorithm of robust；2. fade adaptive Unscented particle filter；3. fuzzy robust adaptive particle filter；4. adaptive SVD-UKF filtering algorithm；5. adaptive square root Unscented particle filter.

3. independent combined navigation system according to claim 2, it is characterised in that: steps are as follows for the adaptive Unscented particle filter algorithm of robust:

(a) it initializes: N number of particle is extracted according to initial mean value and mean square deviation, at the moment of k=0,I=1,2 ..., N, setting weight are

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

(b1) equivalence weight is calculated

With adaptive factor α.IGG scheme is selected to construct Modified Equivalent Weight Function, IGG is owned by France to do robust limitation to measuring value in drop weight function, if taking its inverse, it is 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.;$

It as needed can also be using another expression formula:

${\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.;$

Wherein k₀∈ (1,1.5), k₁∈ (3,8), V_kFor observation l_kResidual vector,

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 chosen by the judgement to residual error, and the latter is according to state

(b2) Sigma point is calculated, by UKF algorithm more new particle

It obtains

AndMeetIf new samples are

2N+1 Sigma point sampling point are as follows:

${\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) scale factor is indicated, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value, and generally (value best for Gaussian Profile is (b2), W to β according to priori knowledge come value_jThe weight for indicating j-th of Sigma point, meets ∑ W_j=1, j=0,1 ... 2N；

(c) weight is calculated $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 it is normalized to ${\tilde{w}}_k^i=w_k^i/\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{w}_k^i;$

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

Acquired results are compared with set threshold, judge the severity of sample degeneracy,

It is smaller, show to degenerate more serious；In this case, resampling can be carried out to previously obtained posterior density, retrieves M new particles, and assign each particle identical weight 1/M；

(e) nonlinear state amount estimated value is calculated:

It repeats previous step (b)；

Two important regulatory factors are utilized when choosing the importance density function in above-mentioned steps, i.e., weight factor and adaptive factor of equal value；Particle sampler point obtained by after being converted as the two to UT more reasonably distributes useful information, provides better sample distribution function for importance sampling process.

4. independent combined navigation system according to claim 2, it is characterised in that: the adaptive Unscented particle filter algorithm that fades mainly converts to obtain sampled point using UT, realizes the approximation to state vector Posterior distrbutionp；It is different from Monte Carlo method, Unscented particle filter be not it is random sampled from given distribution, be to take a small number of Sigma points determined 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}];$

λ=α in formula²(N+v) scale factor is represented, v is second order scale factor, and N is sampling particle number, and α determines sampled point to the degree of scatter of prediction mean value；For different system model and noise it is assumed that UT transformation algorithm has different forms, determine that the key of UT transformation algorithm expression formula is determining Sigma point sampling strategy, i.e. Sigma point number, position and weight；Its algorithm key step are as follows:

(a) it initializes, when k=0,Wherein k indicates the moment；Weight is arranged in unification

(b) Sigma point is calculated, if new samples are

2N+1 Sigma sampled point is calculated, particle is predicted and updated using UKF algorithm；Using fading, adaptive extended kalman filtering thought calculates fading factor, utilizes 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;$

It obtains

As the importance density function of particle sampler, importance sampling is carried out；

(c) from importance density letter

In sampled after, and to each particle calculate weight

$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 weight.

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

(d) estimator is utilized

Judge whether sample degeneracy degree is serious, then posterior density obtained from the above carries out resampling and retrieves M new particles, assigns each particle identical weight again

(e) nonlinear state amount estimated value is calculated:

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

(2) step is returned to, by the state estimation of new observed quantity recursive calculation subsequent time.

5. independent combined navigation system according to claim 2, it is characterised in that: the fuzzy robust adaptive particle filter includes based on fuzzy theory construction equivalence weight and specific algorithm step, in which:

(I) equivalence weight is constructed based on fuzzy theory: is divided power according to data residual error are as follows: Bao Quanqu (keeping former observation constant), the area Jiang Quan (robust limitation is made to observation) and reject region (Quan Weiling)；The step of designing one-dimensional fuzzy controller, constructing weight factor γ is as follows:

(a) it is blurred: by input quantity

(exact value) blurring becomes fuzzy variable (wherein

For standardized residual), convert determining input to the fuzzy set described by degree of membership；Its detailed process are as follows: by input variable

Fuzzy subset be divided into { too big, larger, normal }, be abbreviated as { B_e, B_c, B_n, after input quantization,

Domain be X={ 0,1,2,3,4 }；The fuzzy subset for exporting γ is { minimum, smaller, normal }, is abbreviated as { S_e, S_c, S_n, domain is that Y size is divided into 5 grades, to indicate different values, i.e. Y={ 0,1,2,3,4 }；Respectively to input

Fuzzy quantization is carried out with output γ；

(b) according to the intuitive thought reasoning of people and practical experience, by input quantity

With the relationship of output quantity weight factor γ, fuzzy control rule is designed；If

Too big, then γ is minimum；If

Larger, then γ is smaller；If

Normally, then γ is normal；It can determine that fuzzy relation is according to fuzzy rule

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

Wherein, "×" indicates 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 variable

Fuzzy subset and fuzzy relationship matrix r by fuzzy reasoning obtain weight factor be γ a fuzzy set, obtain final fuzzy control quantity；

(d) it obtains fuzzy control quantity progress ambiguity solution accurately to export control amount, i.e. weight factor γ；The process that fuzzy reasoning result is converted into exact value is known as anti fuzzy method；In anti fuzzy method treatment process, using maximum membership grade principle；

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

(a) it initializes: at the moment of k=0, being sampled out N number of particle according to emphasis density, it is assumed that each particle sampled out is used

It indicates, enables k=1；

(b) using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model: using prediction residual as the differentiation statistic of structure's variable state model error are as follows:

$\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}};$

Wherein,

For i-th of status prediction information of k moment, pass through

It finds out；

Indicate prediction residual

Covariance matrix；Tr () is Matrix Calculating trace operator；The adaptive factor function of statistic is differentiated based on prediction residual are as follows:

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally takes 1.0 < c < 2.5；

(c) it updates:

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

By

The particle at 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)});$

$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})};$

Update weight and normalization weight, i=1,2 .., N；

(d) resampling: being ranked up the weight of all particles according to descending, and setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collectionResampling obtains new particle collection

And it resets weight and is

(e) it 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);$

It calculates the filtering density at k moment and carries out re-sampling, enable k=k+1, return (b).

6. independent combined navigation system according to claim 2, it is characterised in that: the adaptive SVD-UKF filtering algorithm includes:

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

It is assumed 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;$

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

(II) determination of statistic and adaptive factor: using prediction residual as variable, the Error subtraction scheme statistic and adaptive factor of structural regime model.Prediction residualIt indicates are as follows:

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

In formula,

For k moment status prediction information；Use prediction residual

Structural 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/\mathrm{tr}\left({\mathrm{\&Sigma;}}_{{\stackrel{\&OverBar;}{V}}_k}\right))}^{1/2};$

In formula,

Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator；

Adaptive factor function is selected 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, α_kIndicate adaptive factor, c is empirical, under normal conditions 1 < c < 2.5；

(III) adaptive SVD-UKF algorithm steps are as follows:

(a) it initializes: to the parameter initialization in state equation, and calculating the weight coefficient w of Sigma point mean value and covariance^(m)、w^(c)；(b) singular value decomposition, calculating Sigma point vector；(c) time updates；(d) it measures and updates.

7. independent combined navigation system according to claim 2, it is characterised in that: steps are as follows for the adaptive square root Unscented particle filter:

(a) it initializes (k=0): randomly selecting N number of primary(i=1,2 ..., N).Assuming that ${\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,

With

I-th of particle of initial time and its estimated value are respectively indicated,

Indicate i-th of Cholesky factoring of initial time,

Indicate the initialization weight of i-th of particle, the Cholesky decomposition operator of chol { } representing matrix；

(b) each particle is updated using adaptive Deep space algorithm

It obtains

Indicate the covariance of i-th of particle of k moment, steps are as follows:

(b1) Sigma point and weight are calculated:

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

Prediction residual (or new breath) can more reflect the disturbance of dynamical system containing without the modified state of observation information.I-th of prediction residual vector of k moment

It indicates are as follows:

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

Using prediction residual as the differentiation statistic of structure's variable state model error are as follows:

$\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}};$

Wherein,

For i-th of status prediction information of k moment, pass throughIt finds out,

Indicate prediction residual

Covariance matrix, tr () be Matrix Calculating trace operator；

The adaptive factor function of statistic is differentiated based on prediction residual are as follows:

${\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,

Indicate i-th of adaptive factor of k moment, c is empirical, generally takes 1.0 < c < 2.5；

(b3) time updates (status predication)；

(b4) measurement updaue (state estimation): the QR in linear algebra has been used to decompose in the step, Cholesky is decomposed, state covariance matrix is directly propagated and updated in the form of Cholesky factoring, to enhance the numerical stability in state covariance matrix renewal process, the orthotropicity of covariance matrix ensure that；

(c) importance sampling weight computing: significance distribution function is enabled

Sample particle

N () indicates 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})},$

Update weight and normalization weight, i=1 ..., N；

(d) resampling: setting thresholding sample points are N_th(being usually chosen as N/2 or N/3), effective sample points are N_eff, work as N_eff<N_thWhen, to particle collection

Resampling obtains new particle collection

And it resets weight and is

(e) state updates: ${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\tilde{X}}_k^i{\tilde{w}}_k^i;{\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.$

Images