CN101907460A - Particle filtering method for north-seeking of fiber optic gyroscope - Google Patents

Abstract

The invention discloses a particle filtering method for the north-seeking of a fiber optic gyroscope, which comprises the following steps of: establishing the output characteristics of a horizontal component of uniaxial fiber optic gyroscope sensitive earth rotational angular velocity; establishing a state space model and an observation model of a nonlinear fiber optic gyroscope north-seeking system according to a selected fiber optic gyroscope north-seeking scheme; performing mixed Gaussian approximation on a posterior probability density function; performing grid approximation on continuous filtering density; constructing a grid-based particle filter; and finally acquiring the output information of the fiber optic gyroscope according to a state equation and an observation equation of the fiber optic gyroscope north-seeking system, and finishing the filtration resolution of the north-seeking by using a navigation computer. The particle filtering method has the advantage of high north-seeking accuracy.

Description

A kind of particle filter method that is used for north-seeking of fiber optic gyroscope

Technical field

Invention belongs to multi-sensor information fusion technology, be a kind of data fusion method that is used for north-seeking of fiber optic gyroscope, also be applicable to the field of other multi-sensor information fusion such as automatic target recognition and tracking, automatic aircraft navigation and control, robot, industrial process control, Flame Image Process, pattern-recognition and application.

Background technology

Optical fibre gyro is based on the angular rate sensor of new generation of Sagnac effect, seeking northern orientation is one of important application of optical fibre gyro, in military domain such as Aeronautics and Astronautics and navigations, and civil areas such as geophysical exploration, geodetic surveying, subsurface investigation and tunnelling are all being brought into play important effect.The gyroscope north searching technology is the important component part in inertial technology field, also be guarantee in the present information war that armament systems are quick, motor-driven, one of the important leverage technology of precision strike.Along with science and technology development, gyroscope north searching instrument towards the precision height, speed is fast, volume is little, direction such as in light weight develops, the plurality of advantages of optical fibre gyro has just in time been catered to the development trend of gyroscope north searching instrument, becomes the desirable inertance element of gyroscope north searching instrument.

The north finding precision of optical fibre gyro depend primarily on the performance of optical fibre gyro self and adopted seek northern case.Under the situation of optical fibre gyro, seek northern case north finding precision is played decisive influence with the performance grade.The FOG of the most popular moving pedestal of the level that is based on rotates scheme in the method that fiber gyro north seeker is adopted at present, its basic thought is to utilize optical fibre gyro measurement output to earth rotation angular speed horizontal component on two or more regularities of distribution position to carry out computing, eliminates the constant value drift of optical fibre gyro.

Particle filter has become the focus of research as the effective method that solves the optimal estimation of non-linear, non-Gauss's dynamic system.The north-seeking of fiber optic gyroscope scheme that this paper introduces based on particle filter, on the basis that takes into full account the optical fibre gyro output characteristics, set up the state-space model and the observation model of north-seeking system, utilize particle filter in the advantage that solves on the nonlinear system estimation problem, calculate the angle of optical fibre gyro sensitive axes and real north.

Contradiction between particle degeneration and particle poorness, calculated amount and the precision is that particle filter is applied to the key problem that north-seeking of fiber optic gyroscope need solve.The resampling that has its source in of particle poorness is the sampling on Discrete Distribution; Contradiction between calculated amount and the precision is derived from the approximate low likelihood region to probability density function in Monte Carlo and lacks description, must obtain by the method that increases particle hanging down comprehensive description of likelihood region.The method that solves these two contradictions can reduce: seek particle more excellent, that have more efficient, thereby obtain more excellent filtering performance.

The grid approximation method is similar with the Monte Carlo approximation method, and it comes probability density function is similar to by the grid of a series of cum rights values, can describe the distribution situation of probability density function in high likelihood region and low likelihood region all sidedly.The Monte Carlo is approximate to be by probability density function is sampled, frequency with sampling comes probability density function is described, so the approximate relative Monte Carlo of grid is approximate has a higher approximation quality. and the particle filter that incorporates the approximate thought of grid will help to improve the precision of north-seeking of fiber optic gyroscope.

Summary of the invention

Technical matters: the purpose of this invention is to provide a kind of particle filter method that is used for north-seeking of fiber optic gyroscope, this method can utilize particle filter in the advantage that solves on the nonlinear system estimation problem, calculates the angle of optical fibre gyro sensitive axes and real north.

Technical scheme: the present invention adopts following technical scheme for achieving the above object:

The present invention is a kind of particle filter method that is used for north-seeking of fiber optic gyroscope, it is characterized in that comprising the following steps:

1) sets up the single axis fiber gyro output characteristics of revolutions angular speed horizontal component sensitively;

2) set up the state-space model and the observation model of nonlinear optical fiber gyroscope north searching system;

3) the posterior probability density function being carried out mixed Gaussian is similar to;

4) grid of continuous filtering density is approximate;

5) structure is based on the particle filter of grid;

6) last by state equation and the observation equation of navigational computer according to the north-seeking of fiber optic gyroscope system, gather optical fibre gyro output information, and finish the filtering of seeking north and resolve.

Beneficial effect:

Method of the present invention has following advantage: utilize grid to be similar to filtering density is similar to, thereby acquisition has more the particle of efficient, improved the precision of particle filter, solved the problem of particle degeneration and particle poorness, and can reach north finding precision preferably.The above beneficial effect of the invention is described as follows:

Put aside factors such as the lateral error of alignment error, turntable of gyro and temperature variation.If the optical fibre gyro constant value drift be 0.2 (°)/h, the optical fibre gyro white noise be 0.2 (°)/h, with geographical real north angle be 60 °, geographic latitude is 30 ° of north latitude, it is 61 ° that original state is obeyed average, standard deviation is 5 ° normal distribution.

Ordinary particle filtering and all increase with number of particles based on the particle filter precision of grid improves, for ease of comparing the performance of two kinds of algorithms, emulation is guaranteeing to have chosen less number of particles under the filtering convergent situation, and two kinds of filtering algorithms all extract 50 of number of particles.Particle filter algorithm based on grid has provided filtering density with discrete form, therefore must the filtering density that this is discrete be converted into continuous filtering density, utilize the Gaussian approximation method that the filtering density of discrete form is converted to continuous density function in the emulation.Continuous filtering density is carried out grid when approximate, take into account the influence to filtering accuracy of truncation error and mesh-density, in-1.725 δ～1.725 δ intervals continuous filtering density is evenly divided grid, wherein δ is the mean square deviation of filtering density.In surface level, differ when seeking north on two positions of 180 ° and gather gyro data, get its difference and obtain observation data.

Figure 3 shows that the comparison of seeking northern result based on the particle filter algorithm and the ordinary particle filtering algorithm of grid, Fig. 4 is the filtering angular errors of two kinds of filtering algorithms when being used to seek north.For the difference of two kinds of filtering algorithms of contrast, under the situation that guarantees two kinds of filtering algorithm operate as normal, chosen less particle during emulation.Seek Beijiao degree error mean about 0.07 ° based on the particle filter algorithm of grid as can be seen from Figure, and the ordinary particle algorithm is sought Beijiao degree error mean about 0.15 °, have higher north finding precision based on the particle filter algorithm of grid.Increase number of particles, two kinds of methods all will be further enhanced on precision.

Description of drawings

Fig. 1 is a grid particle filter FB(flow block).

Fig. 2 is the approximate of probability density function.

Fig. 3 seeks northern result relatively.

Fig. 4 seeks northern error ratio (absolute value).

Embodiment

Be elaborated below in conjunction with the technical scheme of accompanying drawing to invention:

At first set up the single axis fiber gyro output characteristics of revolutions angular speed horizontal component sensitively; Set up the state-space model and the observation model of nonlinear optical fiber gyroscope north searching system then according to selected north-seeking of fiber optic gyroscope scheme; The posterior probability density function is carried out mixed Gaussian to be similar to; Continuous filtering density is carried out grid to be similar to; Structure is based on the particle filter of grid; Last by state equation and the observation equation of navigational computer according to the north-seeking of fiber optic gyroscope system, gather optical fibre gyro output information, and finish the filtering of seeking north and resolve.

1) sets up the single axis fiber gyro output characteristics of revolutions angular speed horizontal component sensitively

According to sampling with resolve the different of mode, the seeking northern case and can be divided into static state and seek northern case and dynamically seek northern case of optical fibre gyro, static state is sought northern case and is comprised that northern case is sought in two positions, northern case and multi-location north seeking case etc. are sought in three positions.To have a turned position few for the northern case of seeking of two positions, eliminates gyroscope constant value drift easily, and therefore precision height, fireballing characteristics adopt two positions to seek northern case herein.Optical fibre gyro is fixed on the turntable, and turntable level surface relative to the earth is parallel, input shaft of optical fibre gyro and turntable plane parallel.During measurement of bearing, the relative geostationary of optical fibre gyro, then the component of earth rotation angular speed on optical fibre gyro sensitive axes direction is:

In the formula, ω (t) is the component of earth rotation angular speed on optical fibre gyro sensitive axes direction, ω _IeBe the earth rotation angular speed, Be local latitude, θ is the angle of optical fibre gyro sensitive axes and north orientation, and ε (t) is an optic fiber gyroscope random drift.ε (t) mainly is made of normal value and white Gaussian noise, and expression formula is:

ε(t)＝ε ₀+G(t) (2)

In the formula, ε ₀Be the optical fibre gyro constant value drift, G (t) is a white Gaussian noise.

2) set up the state-space model and the observation model of nonlinear optical fiber gyroscope north searching system

When seeking north optical fibre gyro is fixed on the horizontal revolving stage, be position 1 this moment, turntable horizontally rotates 180 °, be position 2 this moment, two locational optical fibre gyro output quantities that differ 180 ° in constantly with surface level at k are subtracted each other, constitute two positions and measure method, can effectively eliminate the constant value drift of optical fibre gyro.Its two position measures:

In the formula, ω _k ¹Be the k i.e. output of the k time filtering, 1 place optical fibre gyro constantly, ω in the position _k ²The output of 2 place optical fibre gyros in the position constantly for k, θ _kFor the angle of optical fibre gyro sensitive axes and north orientation at k value constantly, ε _k ¹Be the random drift of the k moment 1 place optical fibre gyro, ε in the position _k ²Random drift for the k moment 2 place optical fibre gyros in the position.

The angle theta of optical fibre gyro sensitive axes and north orientation is a normal value when seeking north, considers the influence of platform vibration etc., sets up the discrete state equations of system:

θ _k＝θ _k-1+w _k-1 (4)

In the formula, θ _K-1Be the angle of k-1 moment optical fibre gyro sensitive axes and north orientation, w _K-1System state equation stochastic error for reasons such as k-1 platform vibration constantly cause is thought of as white Gaussian noise.

The difference of getting the measurement of two position quantity is observed quantity, sets up the measurement equation of system:

In the formula, z _kBe k two position detection amounts constantly, v _kObservation equation stochastic error for k gyroscopic drift constantly causes is thought of as white Gaussian noise.

3) the posterior probability density function being carried out mixed Gaussian is similar to

To carry out grid dividing to probability density function, then must obtain the probability density function expression formula being convenient to calculate, and the particle point of particle filter by the cum rights value provided filtering density with discrete form in the actual conditions, therefore must the filtering density that this is discrete be converted into continuous filtering density.A simple disposal route is a Gaussian approximation, and its average and variance can be obtained by average and the covariance of calculating particle point.

${\mathrm{\μ}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i---\left(6\right)$

${\mathrm{\Σ}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i({\mathrm{\μ}}_k-x_k^i){({\mathrm{\μ}}_k-x_k^i)}^T---\left(7\right)$

In the formula, μ _kBe the average of k moment filtering density, w _k ⁱBe the weights of k moment particle i, x _k ⁱBe the value of k moment particle i, ∑ _kBe the covariance of k moment filtering density, N is the natural number greater than 1.

When Gaussian approximation causes than mistake, can consider that mixed Gauss model is approximate.

$p\left(x\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\π}}_{j}N(x;{\mathrm{\μ}}_j,{\mathrm{\Σ}}_j)---\left(8\right)$

In the formula, p () is a probability density function, and N () is a Gaussian distribution, and x is a state variable, $\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\π}}_j=1,$ π _j＞0, j=1,2,3 ..., N, π _jWeight for j Gaussian distribution in the mixed Gauss model is called blending ratio, μ _j, ∑ _j, j=1,2,3 ..., N is the average and the covariance of j Gaussian distribution in the mixed Gauss model.

The standard method that mixture model is fitted by observation data is the EM algorithm, is a kind of iterative optimization techniques at the probability model design.In mixed Gauss model, the EM algorithm is as follows to the study of parameter:

A given group observations x={x ₁, x ₂..., x _M, the mixed Gauss model parameter is made up of 3 parts:

θ＝[μ，∑，π]

μ＝[μ ₁，μ ₂，…，μ _N] (9)

∑＝[∑ ₁，∑ ₂，…，∑ _N]

π＝[π ₁，π ₂，…，π _N]

Allow a _Ij, i=1,2,3 ..., M, j=1,2,3 ... N is implicit variable, expression x _iThe probability that is produced by j multidimensional normal distribution is a _Ij=p (C _i=j), C is a stochastic variable, is used for the designation data composition, and E-step is then arranged:

$a_{\mathrm{ij}}^t=p(C_i=j|x_i,{\mathrm{\θ}}^t)=\frac{p(C_i=j,x_i|{\mathrm{\θ}}^t)}{p\left(x_i\right|{\mathrm{\θ}}^t)}$

$=\frac{p\left(x_i\right|C_i=j,{\mathrm{\θ}}^t)p(C_i=j|{\mathrm{\θ}}^t)}{\underset{j=1}{\overset{N}{\mathrm{\Σ}}}p\left(x_i\right|C_i=j,{\mathrm{\θ}}^t)p(C_i=j|{\mathrm{\θ}}^t)}---\left(10\right)$

Carry out M-step then:

${\mathrm{\μ}}_j^{t+1}=\frac{\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}^t{x}_i}{\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}^t}---\left(11\right)$

${\mathrm{\Σ}}_j^{t+1}=\frac{\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}^t(x_i-{\mathrm{\μ}}_j^{t+1}){(x_i-{\mathrm{\μ}}_j^{t+1})}^T}{\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}^t}---\left(12\right)$

${\mathrm{\π}}_j^{t+1}=\frac{1}{M}\underset{i=1}{\overset{M}{\mathrm{\Σ}}}{a}_{\mathrm{ij}}^t---\left(13\right)$

In the formula, subscript t and t+1 represent the t time iteration and the t+1 time iteration, and t is the natural number greater than 1.

4) grid of continuous filtering density is approximate

To any one probability density function, several approximation methods commonly used have: the approximate and Monte Carlo of Gaussian approximation, grid is approximate etc.What Fig. 2 (a) described is the approximate thought in Monte Carlo, and the frequency of circle representative sample, monte carlo method adopt the mode of statistical sampling that probability density function is simulated, and this interval probability density function of intensive more expression of sampling is big more.Fig. 2 (b) has described the approximate thought of grid, and the approximate hypothesis of grid state space is discrete and bounded, and promptly state space is by limited discrete state (x _K-1 ^j, j=1,2 ... N) form, this discrete state generally is taken as the central point of grid, k-1 constantly state can get in the above-mentioned N state any one, for each state x _K-1 ^j, use w _K-1 ^jThe weights of representing each state, when grid evenly distributes, w _K-1 ^jGet the likelihood value of grid element center point, then k-1 probability density function constantly can be expressed as:

$p\left(x_{k-1}\right|z_{1:k-1})=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^j\mathrm{\δ}(x_{k-1}-x_{k-1}^j)---\left(14\right)$

In the formula, δ () is an impulse function, x _K-1Be k-1 moment state variable, z _1:k-1Be to be carved into k-1 observed quantity constantly at 1 o'clock.

The approximate method of grid obviously has higher precision, also can obtain precision preferably under less grid situation.In grid was approximate, better approximate in order to obtain the continuous state space, grid must be sufficiently intensive, along with the increase of state space dimension, also must cause the sharp increase of calculated amount.Simultaneously, if state space is not limited, just must block state space, since during grid is approximate with likelihood value being similar to of grid element center point as the grid weights, there is certain error, therefore under the situation that number of grid is fixed, increases the zone of blocking, must make grid dividing more sparse, thereby increase the approximate error of grid weights.The principle that should follow when state space is blocked is, when probability density function profiles is relatively more sharp-pointed, dwindles the zone of blocking, and when probability density function profiles is smoother, increases the zone of blocking.

5) structure is based on the particle filter of grid

Under equivalent amount grid (particle) situation, approximate will being similar to than the Monte Carlo of grid have higher precision, and therefore that grid is approximate thought incorporates in the particle filter, can obtain better filter effect.Its basic thought of particle filter algorithm based on grid is: after obtaining filtering density, it is approximate that it is carried out Gauss or mixed Gaussian, discrete filtering density is converted to continuous filtering density, and it is approximate rather than the Monte Carlo is approximate that this filtering density is carried out grid, with the weight w of obtaining that has _K-1Grid enter filtering next time circulation as new particle.Have higher efficient because the approximate relative Monte Carlo of grid is approximate, and can describe the low likelihood region of probability density function better, therefore introduce grid approximate after, will improve the precision of particle filter.Simultaneously, because new particle obtains by the continuous density function, there is not the problem of particle degeneration and particle poorness.It is as follows that grid particle filter weights calculate recursive process:

Can obtain priori probability density according to the Chapman-Kolmogrov equation:

In the formula, x _kBe k moment state variable.

Because system can represent then have by (14) formula in k-1 posterior probability density constantly:

Posterior probability density can obtain by the Bayes rule:

$p\left(x_k\right|z_{1:k})\∝p\left(z_k\right|x_k)p\left(x_k\right|z_{1:k-1})\∝p\left(z_k\right|x_k)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k\right|x_{k-1}^j)---\left(17\right)$

In the formula, z _1:kBe to be carved into k observed quantity constantly at 1 o'clock.

Generally can not sample from posterior probability density, utilize k-1 moment particle to sample from the suggestion probability density, because k-1 moment posterior probability density utilizes grid to be similar to, the particle weights are w _K-1, therefore have:

$q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^i\mathrm{\δ}(x_k-x_k^i)---\left(18\right)$

In the formula, q () is suggestion density function, w _K-1 ⁱWeights for k-1 moment i particle.

Then have:

$p\left(x_k\right|z_{1:k})\∝\frac{p\left(z_k\right|x_k)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k\right|x_{k-1}^j)}{q\left(x_k\right|x_{k-1},z_k)}q\left(x_k\right|x_{k-1},z_k)$

$=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^i\frac{p\left(z_k\right|x_k^i)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k^i\right|x_{k-1}^j)}{q\left(x_k^i\right|x_{k-1},z_k)}\mathrm{\δ}(x_k-x_k^i)---\left(19\right)$

$=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i\mathrm{\δ}(x_k-x_k^i)$

In the formula,

During for normalization not, i particle is at k weights constantly.

Then the weights computing formula is:

${\hat{w}}_k^i=w_{k-1}^i\frac{p\left(z_k\right|x_k^i)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k^i\right|x_{k-1}^j)}{q\left(x_k^i\right|x_{k-1},z_k)}---\left(20\right)$

$=w_{k-1}^{i}p\left(z_k\right|x_k^i)$

The normalization weights:

$w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i---\left(21\right)$

In the formula, w _k ⁱWeights for k moment i particle.

Provide particle filter algorithm basic step (as shown in Figure 1) below based on grid:

Step1: initialization, at p (x ₀) N sample point x of extraction ₀ ⁱ, corresponding weight value 1/N;

Step2: sampling $x_k^i\·q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1});$

Step3: calculate weights and normalization, ${\hat{w}}_k^i\∝w_{k-1}^{i}p\left(z_k\right|x_k^i),$ $w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i;$

Step4: calculate estimated value, ${\stackrel{\‾}{x}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i;$

Step5: to discrete filtering density carry out Gaussian approximation or resample after to utilize the EM algorithm to carry out mixed Gaussian approximate;

Step6: continuous filtering density is carried out grid dividing in certain truncated region;

Step7: calculate the weights of each grid, the value of getting grid element center point likelihood function is new weight w _k ^j

Step8: with the net point of cum rights value as new particle x _k ^jCarry out the filtering of following one-period;

Wherein, x ₀Be filtering initial value, x ₀ ⁱBe the filtering primary,

Be k filtering valuation constantly, w _k ⁱ, w _k ^jBe the k weights of particle and grid weights after approximate constantly, x _k ⁱ, x _k ^jBe the value of k moment particle and the value of grid element center point, i, j=1,2,3 ... N.

Under given accuracy, only need very small particles based on the particle filter algorithm of grid, just can reach the ordinary particle precision of filtering of use than multiparticle, have stronger real-time performance.At last, discrete filtering density is converted into continuous filtering density, utilizes grid to generate new particle, avoided particle in the ordinary particle filtering to degenerate and the problem of particle poorness based on the particle filter algorithm of grid.

6) finishing north-seeking of fiber optic gyroscope filtering resolves

By state equation and the observation equation of navigational computer, gather optical fibre gyro output information, and finish the filtering of seeking north and resolve according to the north-seeking of fiber optic gyroscope system.

In sum, this method is carried out effect analysis.Based on the particle filter algorithm of grid discrete filtering density is converted into continuous filtering density, then it is carried out grid dividing, generates new particle, avoided the problem of particle degeneration and particle poorness in the ordinary particle filtering.Under equal conditions will seek the north contrast based on the particle filter algorithm and the particle filter algorithm of grid.

Figure 2 shows that based on the particle filter algorithm of grid and the northern result that seeks of ordinary particle filtering algorithm and compare, wherein 1. be used to seek the filtering result in north for the ordinary particle wave filter; 2. be the northern result that seeks of Grid-PF particle filter; 3. be the true value of optical fibre gyro sensitive axes and north orientation angle, owing to reasons such as platform vibration center on 60 ° of slight fluctuations.As seen from Figure 2, ordinary particle filtering algorithm and can be applied in the northern case of seeking of optical fibre gyro based on the particle filter algorithm of grid, and can obtain comparatively desirable north finding precision.Two kinds of filtering algorithms are converged to rapidly near 60 ° of the true value by 61 ° of original states, and seek northern result based on the particle filter algorithm of grid and more approach 60 ° of time of days.

Fig. 3 is two kinds of absolute errors that filtering algorithm is sought the Beijiao degree, as can be seen from Figure 3, seek Beijiao degree error mean about 0.07 ° based on the particle filter algorithm of grid, and the ordinary particle algorithm is sought Beijiao degree error mean about 0.15 °, as seen based on the particle filter algorithm of grid, by introducing the approximate thought of grid, under equal particle situation, have the precision higher, will be applied to seeking north and reaching higher precision of optical fibre gyro based on the particle filter algorithm of grid than ordinary particle filtering algorithm.

The content that is not described in detail in the instructions of the present invention belongs to this area professional and technical personnel's known prior art.

Claims (8)

1. a particle filter method that is used for north-seeking of fiber optic gyroscope is characterized in that comprising the following steps:

1) sets up the single axis fiber gyro output characteristics of revolutions angular speed horizontal component sensitively;

2) set up the state-space model and the observation model of nonlinear optical fiber gyroscope north searching system;

3) the posterior probability density function being carried out mixed Gaussian is similar to;

4) grid of continuous filtering density is approximate;

5) structure is based on the particle filter of grid;

6) last by state equation and the observation equation of navigational computer according to the north-seeking of fiber optic gyroscope system, gather optical fibre gyro

Output information, and finish the filtering of seeking north and resolve.

2. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 1, the method for the output characteristics of revolutions angular speed horizontal component is as follows sensitively to it is characterized in that setting up described in the step (1) single axis fiber gyro:

Optical fibre gyro is fixed on the turntable, and turntable level surface relative to the earth is parallel, input shaft of optical fibre gyro and turntable plane parallel; During measurement of bearing, the relative geostationary of optical fibre gyro, then the component of earth rotation angular speed on optical fibre gyro sensitive axes direction is:

In the formula, ω (t) is the component of earth rotation angular speed on optical fibre gyro sensitive axes direction, ω _IeBe the earth rotation angular speed,

Be local latitude, θ is the angle of optical fibre gyro sensitive axes and north orientation, and ε (t) is an optic fiber gyroscope random drift, and its expression formula is:

ε(t)＝ε ₀+G(t) (2)

In the formula, ε ₀Be the optical fibre gyro constant value drift, G (t) is a white Gaussian noise.

3. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 1, the method for setting up the state-space model of nonlinear optical fiber gyroscope north searching system and observation model described in the step that it is characterized in that (2) is as follows:

When seeking north optical fibre gyro is fixed on the horizontal revolving stage, be position 1 this moment, and turntable horizontally rotates 180 °, and be position 2 this moment, and two locational optical fibre gyro output quantities that differ 180 ° in the surface level are subtracted each other, and constitutes two positions and measure:

(3)

In the formula, ε _k ¹Be the k i.e. output of the k time filtering, 1 place optical fibre gyro constantly, ε in the position _k ²The output of 2 place optical fibre gyros in the position constantly for k, θ _kFor the angle of optical fibre gyro sensitive axes and north orientation at k value constantly, ε _k ¹Be the random drift of the k moment 1 place optical fibre gyro, ε in the position _k ²Random drift for the k moment 2 place optical fibre gyros in the position;

Set up the state equation and the observation equation of system:

θ _k＝θ _k-1+w _k-1 (4)

In the formula, θ _K-1Be the angle of k-1 moment optical fibre gyro sensitive axes and north orientation, w _K-1Be the system state equation stochastic error that reasons such as k-1 platform vibration constantly cause, z _kBe k two position detection amounts constantly, v _kThe observation equation stochastic error that causes for k gyroscopic drift constantly.

4. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 1, it is as follows to it is characterized in that described in the step (3) that the posterior probability density function is carried out the approximate method of mixed Gaussian:

Probability density function is carried out grid dividing, discrete filtering density is converted into continuous filtering density.A simple disposal route is a Gaussian approximation, and its average and variance can be obtained by average and the variance of calculating particle point.

${\mathrm{\μ}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i---\left(6\right)$

${\mathrm{\Σ}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i({\mathrm{\μ}}_k-x_k^i){({\mathrm{\μ}}_k-x_k^i)}^T---\left(7\right)$

In the formula, μ _kBe the average of k moment filtering density, w _k ⁱBe the weights of k moment particle i, x _k ⁱBe the value of k moment particle i, ∑ _kBe the covariance of k moment filtering density, N is the natural number greater than 1.

5. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 4 is characterized in that adopting in the step (3) mixed Gauss model approximate:

$p\left(x\right)=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\π}}_{j}N(x;{\mathrm{\μ}}_j,{\mathrm{\Σ}}_j)---\left(8\right)$

In the formula, p () is a probability density function, and N () is a Gaussian distribution, and x is a state variable, $\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\π}}_j=1,{\mathrm{\π}}_j>0,j=\mathrm{1,2,3},...,N,$ π _jWeight for j Gaussian distribution in the mixed Gauss model is called blending ratio, μ _j, ∑ _j, j=1,2,3 ..., N is the average and the covariance of j Gaussian distribution in the mixed Gauss model.

6. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 1, the method that the grid of continuous filtering density described in the step that it is characterized in that (4) is similar to is as follows:

At state space is that promptly state space is by limited discrete state: x under the situation of discrete and bounded _K-1 ^j, j=1,2...N form, this discrete state is taken as the central point of grid, k-1 constantly state can get in the above-mentioned N state any one, for each state x _K-1 ^j, use w _K-1 ^jThe weights of representing each state, when grid evenly distributes, w _K-1 ^jGet the likelihood value of grid element center point, then k-1 probability density function constantly can be expressed as:

$p\left(x_{k-1}\right|z_{1:k-1})=\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^j\mathrm{\δ}(x_{k-1}-x_{k-1}^j)---\left(14\right)$

In the formula, δ () is an impulse function, x _K-1Be k-1 moment state variable, z _1:k-1Be to be carved into k-1 observed quantity constantly at 1 o'clock.

7. a kind of particle filter method that is used for north-seeking of fiber optic gyroscope according to claim 1, structure is as follows based on the method for the particle filter of grid described in the step that it is characterized in that (5):

After obtaining filtering density, it is approximate that it is carried out Gauss or mixed Gaussian, and discrete filtering density is converted to continuous filtering density, and it is approximate that this filtering density is carried out grid, with the weight w of obtaining that has _K-1Grid enter filtering next time circulation as new particle; It is as follows that grid particle filter weights calculate recursive process:

Can obtain priori probability density according to the Chapman-Kolmogrov equation:

In the formula, x _kBe k moment state variable.

Because system can represent then have by (14) formula in k-1 posterior probability density constantly:

Posterior probability density can obtain by the Bayes rule:

$p\left(x_k\right|z_{1:k})\∝p\left(z_k\right|x_k)p\left(x_k\right|z_{1:k-1})\∝p\left(z_k\right|x_k)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k\right|x_{k-1}^j)---\left(17\right)$

In the formula, z _1:kBe to be carved into k observed quantity constantly at 1 o'clock.

Utilize k-1 moment particle to sample from the suggestion probability density, because k-1 moment posterior probability density utilizes grid to be similar to, the particle weights are w _K-1, therefore have:

$q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1})=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^i\mathrm{\δ}\left(x_k{-x}_k^i\right)---\left(18\right)$

In the formula, q () is suggestion density function, w _K-1 ⁱWeights for k-1 moment i particle.Then have:

$p\left(x_k\right|z_{1:k})\∝\frac{p\left(z_k\right|x_k)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k\right|x_{k-1}^j)}{q\left(x_k\right|x_{k-1},z_k)}q\left(x_k\right|x_{k-1},z_k)$

$=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^i\frac{p\left(z_k\right|x_k^i)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k^i\right|x_{k-1}^j)}{q\left(x_k^i\right|x_{k-1},z_k)}\mathrm{\δ}(x_k-x_k^i)---\left(19\right)$

$=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i\mathrm{\δ}(x_k-x_k^i)$

In the formula, During for normalization not, i particle is at k weights constantly.Then the weights computing formula is:

${\hat{w}}_k^i=w_{k-1}^i\frac{p\left(z_k\right|x_k^i)\underset{j=1}{\overset{N}{\mathrm{\Σ}}}{w}_{k-1}^{j}p\left(x_k^i\right|x_{k-1}^j)}{q\left(x_k^i\right|x_{k-1},z_k)}---\left(20\right)$

$=w_{k-1}^{i}p\left(z_k\right|x_k^i)$

The normalization weights:

$w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i---\left(21\right)$

In the formula, w _k ⁱWeights for k moment i particle.

8. according to claim 1 or 6 described a kind of particle filter methods that are used for north-seeking of fiber optic gyroscope, it is characterized in that adopting the step based on the particle filter method of grid of described particle filter as follows:

Step1: initialization, at p (x ₀) N sample point x of extraction ₀ ⁱ, corresponding weight value 1/N;

Step2: sampling $x_k^i\·q\left(x_k\right|x_{k-1},z_k)=p\left(x_k\right|z_{1:k-1});$

Step3: calculate weights and normalization, ${\hat{w}}_k^i{\∝w}_{k-1}^{i}p\left(z_k\right|x_k^i),$ $w_k^i={\hat{w}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\hat{w}}_k^i;$

Step4: calculate estimated value, ${\stackrel{\‾}{x}}_k\≈\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{w}_k^i{x}_k^i;$

Step5: to discrete filtering density carry out Gaussian approximation or resample after to utilize the EM algorithm to carry out mixed Gaussian approximate;

Step6: continuous filtering density is carried out grid dividing in certain truncated region;

Step7: calculate the weights of each grid, the value of getting grid element center point likelihood function is new weight w _k ^j

Step8: with the net point of cum rights value as new particle x _k ^jCarry out the filtering of following one-period;

Wherein, x ₀Be filtering initial value, x ₀ ⁱBe the filtering primary,

Be k filtering valuation constantly, w _k ⁱ, w _k ^jBe the k weights of particle and grid weights after approximate constantly, x _k ⁱ, x _k ^jBe the value of k moment particle and the value of grid element center point, i, j=1,2,3......N.

Images