CN101975585B - A method for initial alignment of strapdown inertial navigation system with large azimuth misalignment angle based on MRUPF - Google Patents

Abstract

The invention relates to a strap-down inertial navigation system large azimuth misalignment angle initial alignment method based on MRUPF (Multi-resolution Unscented Particle Filter), comprising the following steps of: firstly, establishing a state space model for the initial alignment of a strap-down inertial navigation system static substrate under the large azimuth misalignment angle condition to carry out filtering initialization; then carrying out the state estimation of the initial alignment by utilizing a UPF filtering algorithm; selecting particles and particle weights by utilizing a multi-resolution method at a moment to reduce the number of the particles; and continuing to carry out the state estimation of the initial alignment by utilizing the UPF filtering method by taking the selected particle set and the particle weights as initial particle sets and the weights to obtain a misalignment angle estimated value. In the invention, the UPF particle number is reduced through the multi-resolution method, thereby reducing the calculated amount, and the real-time performance of the initial alignment of the strap-down inertial navigation system under a large azimuth misalignment angle is improved while the initial alignment accuracy is ensured. The invention is suitable for the initial alignment of the strap-down inertial navigation system.

Description

A kind of SINS Initial Alignment of Large Azimuth Misalignment On method based on MRUPF

Technical field

The present invention relates to a kind of based on MRUPF (Multiresolution Unscented Particle Filter; The Unscented particle filters of differentiating) SINS Initial Alignment of Large Azimuth Misalignment On method more; Be applicable to that error model is non-linear under the big orientation misalignment situation, quiet pedestal inertial navigation initial alignment under the non-Gaussian noise condition.

Background technology

Strapdown inertial navigation system (Strap-down Inertial Navigation System; SINS) utilize inertia device and initial parameter to confirm the total movement parameters such as position, speed and attitude of carrier; Do not rely on any external information during its work; Also not outside emittance is a kind of autonomous navigational system fully.For SINS, because attitude matrix plays the effect of " digital platform ", so navigation work need obtain the initial value of strapdown attitude matrix at the very start, so that accomplish the task of navigation.Obviously how to obtain the initial value of strapdown matrix, promptly the initial alignment problem has just become the problem that at first must solve.Alignment precision is two the important technology indexs of inertial navigation system when carrying out initial alignment with aiming at real-time.The initial alignment precision influences the performance of SINS, and the initial alignment real-time indicates the quick-reaction capability (QRC) of system, so initial alignment is counted as a navigation gordian technique always, thereby becomes one of inertial navigation hot research fields in recent years.

The initial alignment process of SINS adopts state estimator to estimate the error of coarse alignment usually, and then accurately confirms the initial attitude matrix of SINS.State estimator commonly used is exactly that (Kalman Filter, KF), but the KF filtering method can only be handled linear system to Kalman filter.When the initial heading of SINS misalignment is big, can cause the non-linear of system.EKF (Extended Kalman Filter, though nonlinear problem that EKF) can treatment S INS initial alignment, non-linear when strong when system, for example aerial big misalignment is aimed at, the linearization procedure of EKF will bring very mistake.Unscented Kalman filtering (UKF) is when handling any NLS; Through calculating expectation of NLS posteriority and variance; Can reach the precision of NLS second order Taylor series, but UKF does not just reach desired effects for the state estimation that has than strong nonlinearity, non-Gauss system.

It is white Gaussian noise that filtering methods such as KF, EKF and UKF all require the noise of system; Because the system noise of actual SINS is a non-Gaussian noise; Therefore can cause the decline of SINS initial alignment precision; (Particle filter PF) is applied to the SINS initial alignment, can solve the precise decreasing problem that SINS system non-Gaussian noise causes with the particle filter that is applicable to non-Gauss system.But there is the particle degenerate problem in the PF filtering method, and to this problem, existing scholar applies to UPF filtering method (Unscented Particle Filter, Unscented particle filter) in the non-linear initial alignment of SINS.But the shortcoming of UPF is that calculated amount is very big, and along with the increase of the number of particles of choosing, calculated amount can sharply increase, and the real-time of initial alignment seriously reduces, and the performance of inertial navigation system will be had a strong impact on.

Summary of the invention

Technology of the present invention is dealt with problems and is: to big orientation misalignment; The existing filtering method calculated amount of the quiet pedestal initial alignment of inertial navigation utilization causes the initial alignment time long greatly under the non-Gaussian noise condition; The deficiency of real-time difference; Adopt the MRUPF non-linear filtering method to carry out inertial navigation initial alignment under the big orientation misalignment, when guaranteeing the initial alignment precision, improve the initial alignment real-time.

Technical solution of the present invention is: the present invention proposes a kind of non-linear filtering method based on MRUPF and solve inertial navigation Initial Alignment of Large Azimuth Misalignment On problem.The MRUPF filtering method combines the filtering of many resolution wavelet with UPF; When utilizing UPF to carry out the state estimation of initial alignment; At a time utilize many resolutions method that particle is selected down, reduce number of particles, with the particle collection of selecting and particle weights as primary collection and weights; Continue to utilize the UPF filtering method to carry out the state estimation of initial alignment then, obtain the misalignment estimated value.Concrete steps are following:

(1) sets up the state-space model of the quiet pedestal initial alignment of the big orientation of inertial navigation misalignment

The state variable of system does

With east orientation and north orientation velocity error is observed quantity, i.e. Z=[Δ V _EΔ V _N] ^TGeographic coordinate system is the geographical coordinate system in sky, northeast; Δ V _eWith Δ V _NBe respectively east orientation and north orientation velocity error; ψ _E, ψ _N, ψ _NBe respectively east orientation, north orientation and sky to misalignment;

With

Be respectively east orientation and the north orientation accelerometer often is worth biasing; ε _E, ε _NAnd ε _UBe respectively the gyrostatic constant value drift on three directions.

According to the speed under the quiet pedestal of SINS, attitude error equation, set up error equation and observation equation under the big orientation of the SINS misalignment:

$\left.\begin{array}{c}\stackrel{\·}{X}=f\left(X\right)+\mathrm{\υ}\\ Z=\mathrm{HX}+\mathrm{\η}\end{array}\right\}$

Wherein, f (X) is the nonlinearity erron equation under the big orientation of the SINS misalignment;

Be systematic observation matrix, I _{2 * 2}Be 2 * 2 unit matrix, O _{2 * 8}It is 2 * 8 null matrix; υ and η are respectively system noise and observation noise, and non-Gaussian distribution: υ～p below obeying _υ, η～P _η

Described state-space model discretize is got:

$\left.\begin{array}{c}{X}_k=F\left(X_{k-1}\right)+{\mathrm{\υ}}_{k-1}\\ Z_k={\mathrm{HX}}_k+{\mathrm{\η}}_k\end{array}\right\}$

In the formula, X _K-1And X _kBe respectively the k-1 moment and k state variable constantly; F (X _K-1) be the k-1 system equation nonlinear terms of discretize constantly; υ _K-1It is k-1 system noise constantly; Z _kIt is k observed quantity constantly; η _kIt is k observation noise constantly.

(2) filtering initialization

At initial time is the k=0 moment, the state variable in the step (1) is carried out initialization, promptly to original state X ₀Distribution p (X ₀) sample, generate N and obey p (X ₀) particle of probability distribution

Its average And variance

Satisfy respectively:

$\left.\begin{array}{c}{\stackrel{\‾}{X}}_0^i=E\left[X_0^i\right]\\ P_0^i=E\left[(X_0^i-{\stackrel{\‾}{x}}_0^i){(X_0^i-{\stackrel{\‾}{x}}_0^i)}^T\right]\end{array}\right\}$

In the formula, E [] representes mathematical expectation.

The weights that make each particle are

(3) establishing M is the moment of utilizing many resolutions method that particle and particle weights are selected; K＜M is constantly and when having new observed quantity; According to the state-space model in the step (1); Utilize the UPF filtering method to carry out the state estimation of initial alignment, obtain k constantly initial alignment state estimation value

particle collection

and particle weights

thus obtain the misalignment estimated value.

(4) k=M and when new observed quantity is arranged constantly according to the state-space model in the step (1), selects the resulting k-1 of step (3) particle and particle weights constantly based on many resolutions method, and then utilizes UPF to carry out the state estimation of initial alignment.The particle weights that this step utilizes the filtering of many resolution wavelet that k-1 is obtained are constantly earlier handled; Select particle collection

and particle weights

after

individual particle

and particle weights

will be selected at last as k primary and weights constantly to the judgement of weights size behind the wavelet transformation again, utilize UPF filtering obtain k Shi Ke initial alignment state estimation value

particle collection

He particle weights

thus obtain the misalignment estimated value.In this course, number of particles is reduced to

by N

(5) k＞M and when new observed quantity is arranged constantly; According to the state-space model in the step (1); Particle collection

weights

that obtain with previous moment are as current time initial particle collection and weights; Utilize the UPF filtering method to carry out the state estimation of initial alignment, obtain the misalignment estimated value.In this course, number of particles is

Principle of the present invention is: the systematic error equation of SINS quiet pedestal initial alignment under big orientation misalignment is non-linear, and the system noise of actual SINS is a non-Gaussian noise.To these problems, the present invention proposes a kind of MRUPF non-linear filtering method and carry out the SINS Initial Alignment of Large Azimuth Misalignment On.The MRUPF filtering method combines the filtering of many resolution wavelet with UPF; When utilizing UPF to carry out the state estimation of initial alignment; At a time utilize many resolutions method that particle is selected down; Reduce number of particles, as primary collection and weights, continue to utilize the UPF filtering method to carry out the state estimation of initial alignment then the particle collection of selecting and particle weights.

The present invention's advantage compared with prior art is: the present invention solves inertial navigation Initial Alignment of Large Azimuth Misalignment On problem with a kind of non-linear filtering method based on MRUPF.The MRUPF filtering method combines the filtering of many resolution wavelet with UPF, utilize many resolutions method that particle is selected, and reduces number of particles.Overcome under the non-Gaussian noise condition; The existing filtering method calculated amount of the quiet pedestal initial alignment of the big orientation of inertial navigation misalignment utilization is big; The deficiency of initial alignment real-time difference improves the real-time of initial alignment when guaranteeing the initial alignment precision, thereby improves the performance of SINS.

Description of drawings

Fig. 1 is the process flow diagram of a kind of SINS Initial Alignment of Large Azimuth Misalignment On method based on MRUPF of the present invention.

Embodiment

As shown in Figure 1, the present invention utilizes the MRUPF nonlinear filtering to solve the quiet pedestal initial alignment of the SINS problem under the big orientation misalignment, and its concrete steps are following:

1, sets up the state-space model of the quiet pedestal initial alignment of the big orientation of SINS misalignment

In SINS, the speed of initial alignment, attitude error equation are respectively:

$\mathrm{\Δ}\stackrel{\·}{V}=(I-C_t^p){\stackrel{\‾}{f}}_p-\mathrm{\δ}{f}_t-{\▿}_t---\left(1\right)$

$\stackrel{\·}{\mathrm{\ψ}}=(I-C_t^p){\mathrm{\ω}}_{\mathrm{it}}^t+{\mathrm{\δ\ω}}_{\mathrm{it}}^t+{\mathrm{\ϵ}}^p---\left(2\right)$

In the formula, i, t, p represent inertia, geography, platform coordinate system respectively, and geographic coordinate system is the geographical coordinate system in sky, northeast;

For than force information; δ f _tBe specific force information error item;

Normal value biasing for the accelerometer of three directions of SINS;

Be the angular velocity of Department of Geography with respect to inertial system;

Be angular velocity error term; ε ^p=[ε _Eε _Nε _U] ^TGyrostatic constant value drift for three directions of SINS;

Expression is tied to the attitude matrix of platform coordinate system from geographic coordinate, under greater than 2 ° big orientation misalignment situation, Can be reduced to:

$C_t^p=\left[\begin{array}{ccc}\cos {\mathrm{\ψ}}_z& -\sin {\mathrm{\ψ}}_z& {\mathrm{\ψ}}_y\cos {\mathrm{\ψ}}_z+{\mathrm{\ψ}}_x\sin {\mathrm{\ψ}}_z\\ \sin {\mathrm{\ψ}}_z& -\cos {\mathrm{\ψ}}_z& {\mathrm{\ψ}}_y\sin {\mathrm{\ψ}}_z-{\mathrm{\ψ}}_x\cos {\mathrm{\ψ}}_z\\ -{\mathrm{\ψ}}_y& {\mathrm{\ψ}}_x& 1\end{array}\right]---\left(3\right)$

Carry out quiet pedestal initial alignment at the earth's surface, it is acceleration of gravity that

g is arranged;

Lat is a local latitude, and R is an earth radius.The nonlinearity erron equation that gets inertial navigation initial alignment under the big orientation misalignment by the velocity error equation and the attitude error of inertial navigation system is:

$\left.\begin{array}{c}\mathrm{\Δ}{\stackrel{\·}{V}}_E=-g({\mathrm{\ψ}}_N\cos {\mathrm{\ψ}}_U+{\mathrm{\ψ}}_E\sin {\mathrm{\ψ}}_U)+2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Lat\Δ}{V}_N-C_{11}{\▿}_E-C_{12}{\▿}_N\\ \mathrm{\Δ}{\stackrel{\·}{V}}_N=g({\mathrm{\ψ}}_E\cos {\mathrm{\ψ}}_U-{\mathrm{\ψ}}_N\sin {\mathrm{\ψ}}_U)-2{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Lat\Δ}{V}_E-C_{21}{\▿}_E-C_{22}{\▿}_N\\ {\stackrel{\·}{\mathrm{\ψ}}}_E=-\sin {\mathrm{\ψ}}_U{\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Lat}+{\mathrm{\ψ}}_N{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Lat}-\mathrm{\Δ}{V}_N/R+C_{11}{\mathrm{\ϵ}}_E+C_{12}{\mathrm{\ϵ}}_N+C_{13}{\mathrm{\ϵ}}_U\\ {\stackrel{\·}{\mathrm{\ψ}}}_N=(1-\cos {\mathrm{\ψ}}_U){\mathrm{\ω}}_{\mathrm{ie}}\cos \mathrm{Lat}-{\mathrm{\ψ}}_E{\mathrm{\ω}}_{\mathrm{ie}}\sin \mathrm{Lat}+\mathrm{\Δ}{V}_E/R+C_{21}{\mathrm{\ϵ}}_E+C_{22}{\mathrm{\ϵ}}_N+C_{23}{\mathrm{\ϵ}}_U\\ {\stackrel{\·}{\mathrm{\ψ}}}_U=({\mathrm{\ψ}}_E\cos {\mathrm{\ψ}}_U-{\mathrm{\ψ}}_N\sin {\mathrm{\ψ}}_U)\cos \mathrm{Lat}+\mathrm{\Δ}{V}_E\tan \mathrm{Lat}/R+C_{31}{\mathrm{\ϵ}}_E+C_{32}{\mathrm{\ϵ}}_N+C_{33}{\mathrm{\ϵ}}_U\end{array}\right\}---\left(4\right)$

In the formula, C _IjBe transition matrix

I capable, the j column element, i=1 ..., 3; J=1 ..., 3; ω _IeBe rotational-angular velocity of the earth.

System state variables does

With east orientation and north orientation velocity error is observed quantity, i.e. Z=[Δ V _EΔ V _N] ^T, then the error equation of system and observation equation are respectively:

$\left.\begin{array}{c}\stackrel{\·}{X}=f\left(X\right)+\mathrm{\υ}\\ Z=\mathrm{HX}+\mathrm{\η}\end{array}\right\}---\left(5\right)$

In the formula, f (X) is the nonlinearity erron equation of SINS under big orientation misalignment;

Be systematic observation matrix, I _{2 * 2}Be 2 * 2 unit matrix, O _{2 * 8}It is 2 * 8 null matrix; υ and η are respectively system noise and observation noise, and non-Gaussian distribution: υ～p below obeying respectively _υ, η～p _η

Above-mentioned state-space model discretize is got:

$\left.\begin{array}{c}{X}_k=F\left(X_{k-1}\right)+{\mathrm{\υ}}_{k-1}\\ Z_k={\mathrm{HX}}_k+{\mathrm{\η}}_k\end{array}\right\}---\left(6\right)$

In the formula, X _K-1And X _kBe respectively the k-1 moment and k state variable constantly; F (X _K-1) be the k-1 system equation nonlinear terms of discretize constantly; υ _K-1It is k-1 system noise constantly; Z _kIt is k observed quantity constantly; η _kIt is k observation noise constantly.

Prior probability p (the X of state variable _k| X _K-1) and likelihood probability p (z _k| X _k) satisfy respectively:

p(X _k|X _k-1)＝p _υ(X _k-F(X _k-1)) (7)

p(z _k|X _k)＝p _η(z _k-HX _k) (8)

2, filtering initialization

At initial time is the k=0 moment, the state variable that step (1) is chosen is carried out initialization, promptly to original state X ₀Distribution p (X ₀) sample, generate N and obey p (X ₀) particle of probability distribution

Its average

And variance

Satisfy respectively:

$\left.\begin{array}{c}{\stackrel{\‾}{X}}_0^i=E\left[X_0^i\right]\\ P_0^i=E\left[(X_0^i-{\stackrel{\‾}{X}}_0^i){(X_0^i-{\stackrel{\‾}{X}}_0^i)}^T\right]\end{array}\right\}---\left(9\right)$

In the formula, E [] representes mathematical expectation.

Make the weights of each particle do ${\mathrm{\ω}}_0^i=1/N,i=1,...,N---\left(10\right)$

3, establishing M is the moment of utilizing many resolutions method that particle and particle weights are selected, and as k＜M and when new observed quantity is arranged, the state-space model according in the step (1) utilizes the UPF filtering method, obtains initial alignment misalignment estimated value.Its basic step is following:

A. sampling

At first utilize UKF to obtain the importance sampling function of particle filter,

and

that promptly utilizes the UKF filtering algorithm to be obtained constantly by k-1 asks the average

and the variance of k each particle of the moment

to obtain the importance sampling function:

$q\left(X_k^i\right|X_{0:k-1},Z_{1:k})=N({\stackrel{\‾}{X}}_k^i,P_k^i)---\left(11\right)$

In the formula, Expression with

Be average, with

Normal distribution for variance; X _0:k-1The state variable in expression moment from initial time to k-1; Z _1:kExpression was carved into k observed reading constantly from the 1st o'clock.

Then from the importance density function

extract particles is

$X_k^i~q\left(X_k^i\right|X_{0:k-1},Z_{1:k})=N({\stackrel{\‾}{X}}_k^i,P_k^i),i=1,...,N---\left(12\right)$

B. calculate the particle weights

${\mathrm{\ω}}_k^i={\widetilde{\mathrm{\ω}}}_{k-1}^i\frac{p\left(Z_k\right|X_k^i)p\left(X_k^i\right|X_{k-1}^i)}{q\left(X_k^i\right|X_{0:k-1},Z_{1:k})},i=1,...,N---\left(13\right)$

In the formula,

is the weights after the k-1 normalization constantly.

C. weights normalization

${\widetilde{\mathrm{\ω}}}_k^i={\mathrm{\ω}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\mathrm{\ω}}_k^i,i=1,...,N---\left(14\right)$

D. calculate the size N of effective particle _Eff

$N_{\mathrm{eff}}=1/\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\left({\widetilde{\mathrm{\ω}}}_k^i\right)}^2---\left(15\right)$

If N _EffLess than threshold value N _Th, explain that the particle diversity reduces, need change step (e) over to and resample, carry out the initial alignment state estimation otherwise change step (f) over to.

E. resample

The purpose that resamples is to eliminate the less particles of weights, increases the bigger particle of weights, makes the distribution of the sample set after the resampling meet posterior density p (X _k| Z _k).The weights

of each particle are re-set as 1/N after resampling.

F. initial alignment state estimation

K is initial alignment state estimation value constantly With estimation error variance battle array P _kBe respectively:

${\hat{X}}_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\ω}}}_k^i{X}_k^i---\left(16\right)$

$P_k=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\ω}}}_k^i{P}_k^i=\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{\widetilde{\mathrm{\ω}}}_k^i(X_k^i-{\hat{X}}_k){(X_k^i-{\hat{X}}_k)}^T---\left(17\right)$

After obtaining

, its 3rd～5 dimension is exactly an initial alignment misalignment estimated value.

4, k=M and when new observed reading is arranged constantly; According to the state-space model in the step (1); Based on many resolutions method the resulting k-1 of step (3) particle and particle weights are constantly selected, utilized UPF to carry out state estimation again, obtain k initial alignment misalignment constantly.This step is applied to many resolutions method among the UPF; The particle weights that utilize many resolutions method that k-1 is obtained are constantly earlier handled; Select corresponding particle again; Particle collection after will selecting at last

and particle weights

carry out UPF filtering as k moment primary and weights, thereby obtain the misalignment estimated value.Its basic step is following:

A. many resolution wavelet bank of filters is decomposed weights

If L is the scale parameter of wavelet decomposition, with k-1 moment particle collection

And particle weights

Be divided into N/2 ^LIndividual time block carries out many resolution decomposition to the weights of each time block, and it is decomposed into low pass part and high pass part:

$W_{m,\mathrm{lh},k-1}=\left[\begin{array}{c}{\widetilde{\mathrm{\ω}}}_{m,l_1,k-1}^1\\ {\widetilde{\mathrm{\ω}}}_{m,h_1,k-1}^1\\ .\\ .\\ .\\ {\widetilde{\mathrm{\ω}}}_{m,h_L,k-1}^1\\ .\\ .\\ .\\ {\widetilde{\mathrm{\ω}}}_{m,h_L,k-1}^{2^{L-1}}\end{array}\right]=T\left[\begin{array}{c}{\widetilde{\mathrm{\ω}}}_{m,k-1}^1\\ .\\ .\\ .\\ {\widetilde{\mathrm{\ω}}}_{m,k-1}^{2^L}\end{array}\right],m=1,...,N/2^L---\left(18\right)$

In the formula,

The particle weights of representing m time block;

The low pass part of scale 1 when being k-1;

High pass part when being k-1 on the scale j, and the weights distribution number on each time block mesoscale j is 2 ^J-1, j=1 ..., L; T is the wavelet decomposition transformation matrix of quadrature.

B. the high pass of each yardstick is partly carried out threshold process

${\underset{\‾}{\mathrm{\ω}}}_{m,h_j,k-1}^i=\left\{\begin{array}{cc}{\widetilde{\mathrm{\ω}}}_{m,h_j,k-1}^i,& \left|{\widetilde{\mathrm{\ω}}}_{m,h_j,k-1}^i\right|>T_s\\ 0,& \left|{\widetilde{\mathrm{\ω}}}_{m,h_j,k-1}^i\right|\≤T_s\end{array}\right.---\left(19\right)$

In the formula, j=1 ..., L; I=1 ..., 2 ^J-1T _sBe the weights threshold value of setting, T _s∈ (10 ^-4, 10 ^-2).

After then passing through threshold process, the particle weights of high pass and low pass part are write as vector form and are:

${\underset{\‾}{W}}_{m,\mathrm{lh},k-1}={[{\widetilde{\mathrm{\ω}}}_{m,l_1,k-1}^1,{\underset{\‾}{\mathrm{\ω}}}_{m,h_1,k-1}^1,...,{\underset{\‾}{\mathrm{\ω}}}_{m,h_L,k-1}^1,...,{\underset{\‾}{\mathrm{\ω}}}_{m,h_L,k-1}^{2^{L-1}}]}^T---\left(20\right)$

C. particle is chosen

To the particle weights after the threshold process W _{M, lh, k-1}Carrying out the T inverse transformation gets:

$\left[\begin{array}{c}{\underset{\‾}{\mathrm{\ω}}}_{m,k-1}^1\\ .\\ .\\ .\\ {\underset{\‾}{\mathrm{\ω}}}_{m,k-1}^{2^L}\end{array}\right]=T^{-1}{\underset{\‾}{W}}_{m,\mathrm{lh},k-1}---\left(21\right)$

In the formula, the particle weights of m time block after

expression conversion.Through threshold process, W _{M, lh, k-1}In some weights be zero because wavelet decomposition transformation matrix T is an orthogonal matrix, so

In some weights can equate.For the weights that equate, only choose one of them representative weights and get final product, choose its corresponding particle simultaneously.Kee was selected after a set of particles

weight is

After selection of the number of particles.

D) with particle collection

and weights

as k constantly initial particle collection and weights; Step a) in the repeating step (3)～f) obtain in k initial alignment state estimation value

the particle collection

and this process of particle weights constantly, number of particles is

5, k＞M and when new observed quantity is arranged constantly; According to the state-space model in the step (1); Particle collection

weights

that obtain with previous moment are as current time initial particle collection and weights; Utilize the UPF filtering method; Step a) in the repeating step (3)～f) carry out the initial alignment state estimation obtains the misalignment estimated value.In this process, number of particles is

The present invention utilizes MRUPF filtering when carrying out initial alignment, and the filtering of many resolution wavelet is combined with UPF.At first utilize UPF to carry out the state estimation of initial alignment; At a time utilize many resolutions method that particle is selected down; Reduce number of particles; As primary collection and weights, continue to utilize the UPF filtering method to carry out the state estimation of initial alignment then the particle collection of selecting and particle weights, obtain the misalignment estimated value.Reduce the number of particles of UPF filtering through many resolutions method, thereby reduced calculated amount, when guaranteeing the initial alignment precision, shortened the initial alignment time, improved the real-time of SINS initial alignment under the big orientation misalignment.

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

Claims (1)

1. a kind of strapdown inertial navigation system large azimuth misalignment angle initial alignment method based on MRUPF, it is characterized in that comprising the following steps: (1) Establish the state space model of the initial alignment of the static base of the strapdown inertial navigation system with a large azimuth misalignment angle The state variables of the SINS are $x={\left[\begin{array}{cccccccccc}{\mathrm{\ΔV}}_{\mathrm{E.}}& {\mathrm{\ΔV}}_N& {\mathrm{\ψ}}_{\mathrm{E.}}& {\mathrm{\ψ}}_N& {\mathrm{\ψ}}_u& {\▿}_{\mathrm{E.}}& {\▿}_N& {\mathrm{\ϵ}}_{\mathrm{E.}}& {\mathrm{\ϵ}}_N& {\mathrm{\ϵ}}_u\end{array}\right]}^T;$ Take the eastward and northward velocity errors as observations, that is, Z=[ΔV _E ΔV _N ] ^T ; the geographic coordinate system is the northeast sky geographic coordinate system; ΔV _E and ΔV _N are the eastward and northward velocity errors respectively; ψ _E , ψ _N , ψ _U are eastward, northward and celestial misalignment angles respectively; and

are the constant biases of the east and north accelerometers; ε _E , ε _N and ε _U are the constant drifts of the gyroscopes in the three directions; According to the speed and attitude angle error equations of the strapdown inertial navigation system under the static base, the state space model of the strapdown inertial navigation system under a large azimuth misalignment angle is established, and the error equation and observation equation are respectively: $\left.\begin{array}{c}\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&upsi;</span>}\\ \mathrm{<span\; class="notranslate">\; Z</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; HX</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&eta;</span>}\end{array}\right\}$ Among them, f(X) is the nonlinear error equation of SINS under large azimuth misalignment angle;

is the system observation matrix, I _2×2 is a 2×2 unit matrix, O _2×8 is a 2×8 zero matrix; υ and η are system noise and observation noise, respectively, and obey the following non-Gaussian probability distribution: υ～ p _υ ，η～p _η ; Discretize the above state-space model to get: $\left.\begin{array}{c}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&upsi;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; HX</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&eta;</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}\end{array}\right\}$ In the formula, X _k-1 and X _k are the state variables at the k-1th moment and the k-th moment respectively; F(X _k-1 ) is the nonlinear term of the discretized system equation at the k-1th moment; υ _{k- 1} is the system noise at the k-1th moment; Z _k is the observation at the kth moment; η _k is the observation noise at the kth moment; (2) Filter initialization Initialize the state variables in step (1) at the initial moment, that is, the kth time = 0, and generate N particles that obey the p(X ₀ ) distribution

i=1,...,N, the particle weight is

i=1,...,N; where p(X ₀ ) is the probability distribution that the initial state X ₀ obeys; (3) Let M be the moment when the particles and their weights are selected by the multi-resolution method. When time k<M and there are new observations, according to the state-space model in step (1), use the UPF filter method to perform initial Aligned state estimation, to obtain the initial alignment state estimation value at the kth moment

particle set and particle weights

The estimated value of the misalignment angle is thus obtained, and the specific steps are as follows: a. Sampling Firstly, UKF is used to obtain the importance sampling function of the particle filter, that is, the UKF filter algorithm is obtained from the k-1th moment and

Find each particle at the kth time

mean of

and variance Get the importance sampling function: $\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; |</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; :</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; Z</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; :</span>\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$ In the formula,

expressed by

as the mean value, with

is the normal distribution of variance; X _0:k-1 represents the state variable from the initial moment to the k-1th moment; Z _1:k represents the observed value from the 1st moment to the kth moment; Then from the importance density function

Particles extracted from

i=1,...,N, namely $x_k^i~q\left(x_k^i\right|x_{0:k-1},Z_{1:k})=N({\stackrel{\&OverBar;}{x}}_k^i,P_k^i),$ i=1,...,N (12) b. Calculate particle weights ${\mathrm{\&omega;}}_k^i={\widetilde{\mathrm{\&omega;}}}_{k-1}^i\frac{p\left(Z_k\right|x_k^i)p\left(x_k^i\right|x_{k-1}^i)}{q\left(x_k^i\right|x_{0:k-1},Z_{1:k})},$ i=1,...,N (13) In the formula, is the normalized weight at the k-1th moment; c. Weight normalization ${\widetilde{\mathrm{\&omega;}}}_k^i={\mathrm{\&omega;}}_k^i/\underset{i=1}{\overset{N}{\mathrm{\&Sigma;}}}{\mathrm{\&omega;}}_k^i,$ i=1,...,N (14) d. Calculate the effective particle size N _eff ${\mathrm{<span\; class="notranslate">\; N</span>}}_{\mathrm{<span\; class="notranslate">\; eff</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; /</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{<span\; class="notranslate">\; (</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$ If N _eff is less than the threshold value N _th , it means that the particle diversity is reduced, and it is necessary to turn to step (e) for resampling, otherwise turn to step (f) for initial alignment state estimation; e. Resampling The purpose of resampling is to eliminate particles with smaller weights and increase particles with larger weights, so that the distribution of the sample set after resampling conforms to the posterior density p(X _k |Z _k ), and the distribution of each particle after resampling Weight

is reset to 1/N; f. Initial Alignment State Estimation Estimated value of the initial alignment state at the kth moment

and the estimated error variance matrix P _k are: ${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$ ${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\underset{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\overset{\mathrm{<span\; class="notranslate">\; N</span>}}{\mathrm{<span\; class="notranslate">\; \&Sigma;</span>}}}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; x</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span>$ get

After that, the 3rd to 5th dimension is the estimated value of the initial misalignment angle; (4) When time k=M and there are new observations, according to the state-space model in step (1), based on the multi-resolution method, the particles and particle weights obtained in step (3) at the k-1th moment Select, and then use the UPF to estimate the state of the initial alignment to obtain the estimated value of the initial alignment state at the kth moment

particle set

and particle weights

The estimated value of the misalignment angle is thus obtained, and the specific steps are as follows: a. Multi-resolution wavelet filter bank decomposes weights Let L be the scale number of wavelet decomposition, and the particle set at the k-1th moment

and particle weights Divided into N/2 ^L time blocks, perform multi-resolution decomposition on the weight of each time block, and decompose it into low-pass part and high-pass part: ${\mathrm{<span\; class="notranslate">\; W</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; lh</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ {\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ {\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ {\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}\end{array}\right]<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; T</span>}\left[\begin{array}{c}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ {\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; L</span>}}}\end{array}\right]<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; N</span>}<span\; class="notranslate">\; /</span>{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$ In the formula,

Indicates the particle weight of the mth time block;

is the low-pass part of scale 1 at time k-1;

is the high-pass part on scale j at the k-1th moment, and the number of weight distributions on scale j in each time block is 2 ^j-1 , j=1,...,L; T is the orthogonal wavelet decomposition transformation matrix; b. Threshold the high-pass part of each scale ${\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\left\{\begin{array}{cc}{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}& <span\; class="notranslate">\; |</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; ></span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}<span\; class="notranslate">\; ,</span>& <span\; class="notranslate">\; |</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; j</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; |</span><span\; class="notranslate">\; \&le;</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 19</span>}<span\; class="notranslate">\; )</span>$ In the formula, j=1,...,L; i=1,...,2 ^j-1 ; T _s is the set weight threshold, T _s ∈(10 ^-4 , 10 ^-2 ); After thresholding, the particle weights of the high-pass and low-pass parts are written in vector form as: ${\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; W</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; lh</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>{<span\; class="notranslate">\; [</span>{\stackrel{<span\; class="notranslate">\; ~</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; l</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>{\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; ,</span>{\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; ,</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; .</span><span\; class="notranslate">\; ,</span>{\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; L</span>}}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}<span\; class="notranslate">\; ]</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 20</span>}<span\; class="notranslate">\; )</span>$ c. Particle selection Particle weights after thresholding

Perform T inverse transformation to get: $\left[\begin{array}{c}{\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 1</span>}}\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ <span\; class="notranslate">\; .</span>\\ {\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; L</span>}}}\end{array}\right]<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\underset{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; W</span>}}}_{\mathrm{<span\; class="notranslate">\; m</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; lh</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; twenty\; one</span>}<span\; class="notranslate">\; )</span>$ In the formula,

Indicates the particle weight of the transformed mth time block, d) Set the particle set

and weight

As the initial particle set and weight at the kth moment, repeat steps a to f in step (3) to obtain the initial alignment state estimate at the kth moment

particle set

and particle weights

During this process, the number of particles is reduced from N to

(5) When time k>M and there are new observations, according to the state space model in step (1), the particle set obtained at the previous moment

Weight

As the initial particle set and weight at the current moment, use the UPF filter method to repeat steps a to f in step (3) to estimate the initial alignment state and obtain the estimated value of the misalignment angle. In this process, the number of particles is

Images