CN103727941B - Based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match - Google Patents

Abstract

The invention provides the volume Kalman nonlinear combination air navigation aid based on carrier system speeds match.Key step comprises: initialization integrated navigation system; Setting up is directly quantity of state and with carrier system speed be measurement amount nonlinear filtering wave pattern and discretize thereof with navigational parameter; Structure state equation and measurement equation are nonlinear volume Kalman filtering algorithm, realize the information fusion of integrated navigation and accurate navigator fix.Advantage is the nonlinear filtering algorithm of the nonlinear model that the present invention sets up and use, direct output be navigational parameter, do not need to carry out error correction, and the renewal of strapdown inertial navitation system (SINS) parameter upgrades synchronous realization with the time of wave filter, algorithm is simple, for the Information Fusion for Integrated Navigation Application based on strapdown inertial navigation system provides a kind of new departure.

Description

Based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match

Technical field

The present invention relates to a kind of strapdown inertial navigation system and information fusion technology when providing the servicing unit of carrier system speed (as the navigator such as Doppler anemometer, odometer) to form integrated navigation system, belong to Navigation, Guidance and Control technical field.

Background technology

In strapdown inertial navigation system, Inertial Measurement Unit mainly comprises three gyroscopes and three accelerometers, the linear acceleration information recorded by the angular velocity information that records gyroscope and accelerometer is carried out integration and is obtained the navigation informations such as carrier positions, speed and attitude, have that information is complete, independence is strong, do not rely on any external information and can work alone, precision advantages of higher in short-term, under many circumstances, the first-selected navigate mode of carrier is become.But the error of strapdown inertial navigation system, along with time integral, needs other sensor to assist during long-time navigation, therefore normal and other navigator form integrated navigation system.

The Filtering Model of the integrated navigation system at present based on strap-down inertial is based on error model mostly, when error is considered as in a small amount, by error model linearization, then Kalman filtering is adopted to estimate error, and the error model essence of strapdown inertial navigation system is nonlinear, extra error can be introduced when doing linearization approximate; When adopting speeds match mode, measurement amount is generally the velocity error in navigation system, and under actual conditions, measurement amount is all the speed of carrier system usually, and this needs carrier system rate conversion in navigation system; Adopt during error model and be generally two cover algorithms, a set of is strapdown inertial navigation system parameter update algorithm, and another set of is filtering algorithm, estimates navigational parameter error, and need carry out error correction to navigational parameter.In the present invention, quantity of state directly adopts navigational parameter, instead of error, and measurement amount adopts carrier system speed, instead of conventional navigation is velocity error, and model does not do any approximate and constraint, more accurately; Adopt volume Kalman nonlinear filtering algorithm, do not need linear hypothesis, more meet the non-linear nature of inertial navigation system, be applicable to state equation and measurement equation is nonlinear situation; The navigational parameter of strapdown inertial navitation system (SINS) upgrades and upgrades synchronous realization with the time of filtering, and filtering exports as navigational parameter, and do not need to carry out error correction, algorithm is simple.

Summary of the invention

The object of the invention is to quantity of state directly adopt navigational parameter, measurement amount to adopt carrier system speed, utilize volume Kalman non-linear filtering method to calculate, for the Information Fusion for Integrated Navigation Application based on strapdown inertial navigation system provides a kind of new departure to realize data fusion.

The present invention mainly comprises following steps:

Step 1, initialization integrated navigation system, the navigational parameter for system upgrades and provides initial value: use location sensor obtains the initial position parameters of carrier; The linear acceleration information that the angular velocity information recorded according to gyroscope and accelerometer record, and the carrier system speed that speed pickup records, initial attitude Eulerian angle information and the initial navigation system velocity information of carrier is obtained by strapdown inertial navigation system Alignment Algorithm;

Step 2, the navigational parameter differential equation according to strapdown inertial navigation system, inertial sensor model, and with the measurement equation that carrier system speed is measurement amount, selected 10 dimension system state amount and 3 dimension measurement amounts, set up nonlinear continuous model;

Step 3, the nonlinear continuous model discretize will set up in step 2, form nonlinear discrete systems model;

Step 4, be nonlinear feature according to state equation and the measurement equation of the nonlinear discrete systems model set up in step 3, build volume Kalman nonlinear filtering algorithm, in filtering, update cycle time is different from measuring the update cycle;

Step 5, the carrier system speed output of inertial sensor and aiding sensors provided substitute in the filtering algorithm of step 4, carry out nonlinear filtering, directly export the navigational parameter after combination.

The present invention can also comprise some features like this:

1. the step 2 described in is according to velocity differentials equation, the Eulerian angle differential equation, the inertial sensor model of strapdown inertial navigation system, and with the measurement equation that carrier system speed is measurement amount, selected 10 dimension quantity of states and 3 dimension measurement amounts, set up nonlinear continuous model, be specially:

Using sky, northeast (east-north-up) geographic coordinate system as navigational coordinate system (n system), the coordinate system that on right front using carrier, (bx-by-bz) direction vector right-hand rule is formed is as carrier coordinate system (b system).

The velocity differentials equation of strapdown inertial navigation system is

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

In formula, v _east, v _northand v _upbe respectively navigate east orientation, north orientation and sky in being to speed; [.] ^trepresenting matrix transposition; ω _iefor earth rotation angular speed, L is local geographic latitude; r _mfor radius of curvature of meridian, r _nfor radius of curvature in prime vertical; r _m=r _q(1-2 ρ+3 ρ sin ²l), r _n=r _q(ρ sin ²l+1), r _qfor reference ellipsoid equatorial plane radius, ρ is compression of the earth; and $f_{ib}^b={\[{\tilde{f}}_{ibx}^b-{\mathrm{\δf}}_{ibx}^b,{\tilde{f}}_{iby}^b-{\mathrm{\δf}}_{iby}^b,{\tilde{f}}_{ibz}^b-{\mathrm{\δf}}_{ibz}^b\]}^T,$ with be respectively the measured value of carrier system change in coordinate axis direction three accelerometers, with be respectively three corresponding measuring error of accelerometer, measuring error is divided into two parts, and a part is random constant value ▽ _ax, ▽ _ayand ▽ _az, another part is random noise w _axand w _ayand w _az, random noise is all assumed to be zero mean Gaussian white noise,

${\mathrm{\δf}}_{ibx}^b={\▿}_{ax}+w_{ax},{\mathrm{\δf}}_{iby}^b={\▿}_{ay}+w_{ay},{\mathrm{\δf}}_{ibz}^b={\▿}_{az}+w_{az};C_b^n={\left(C_n^b\right)}^T,$ $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right],$ θ is pitch angle, and γ is roll angle, and ψ is course angle; g ⁿ=[0,0 ,-g] ^t, g is acceleration of gravity;

The Eulerian angle differential equation of strapdown inertial navigation system is

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

In formula, $\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n),$

${\mathrm{\ω}}_{ib}^b={\[{\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\δ\ω}}_{ibx}^b,{\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\δ\ω}}_{iby}^b,{\widetilde{\mathrm{\ω}}}_{ibz}^b-{\mathrm{\δ\ω}}_{ibz}^b\]}^T,$ with for carrier system change in coordinate axis direction three gyroscope survey values, with be three corresponding measuring error of gyroscope, measuring error is divided into two parts, and a part is random constant value ε _gx, ε _gyand ε _gz, another part is random noise w _gx, w _gyand w _gz, random noise is all assumed to be zero mean Gaussian white noise, ${\mathrm{\δ\ω}}_{ibx}^b={\mathrm{\ϵ}}_{gx}+w_{gx},{\mathrm{\δ\ω}}_{iby}^b={\mathrm{\ϵ}}_{gy}+w_{gy},{\mathrm{\δ\ω}}_{ibz}^b={\mathrm{\ϵ}}_{gz}+w_{gz};$

The random constant value department pattern of inertial sensor (gyroscope and the accelerometer) measuring error of strapdown inertial navigation system is

${\stackrel{\·}{\▿}}_{ax}=0,{\stackrel{\·}{\▿}}_{ay}=0,{\stackrel{\·}{\▿}}_{az}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gx}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gy}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gz}=0---\left(3\right)$

Selected 10 maintain system state vector is

x＝[v _east,v _north,θ,γ,ψ,▽ _ax,▽ _ay,ε _gx,ε _gy,ε _gz] ^T

In formula, x is system state vector;

System noise vector is

w＝[w _ax,w _ay,w _gx,w _gy,w _gz,0,0,0,0,0] ^T

In formula, w is system noise vector;

3 dimension amounts are measured as

z＝[v _bx,v _by,v _bz] ^T

In formula, z is for measuring vector; v _bx, v _byand v _bzbe respectively the speed component of carrier system three change in coordinate axis direction;

Measurement noise vector is

η＝[η _vbx,η _vby,η _vbz] ^T

In formula, η is measurement noise vector, η _vbx, η _vby, η _vbzbe respectively speed v _bx, v _byand v _bzmeasurement noise, be all assumed to be zero mean Gaussian white noise;

Measurement equation is

$z=C_n^b{\[v_{east},v_{north},v_{up}\]}^T+\mathrm{\η}---\left(4\right)$

Nonlinear filtering continuous system model is set up as follows according to the velocity differentials equation of strapdown inertial navigation system, the attitude Eulerian angle differential equation, inertial sensor model and measurement equation:

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+\mathrm{\η}\end{array}\right.$

In formula, F (x) is nonlinear state transfer function; H (x) is non-linear measurement function; G (x) is system noise input function; F (x) and G (x) writes out with reference to formula (1)-(3), and H (x) writes out with reference to formula (4).

2. the nonlinear continuous model discretize will set up in step 2 in the step 3 described in, obtains nonlinear discrete systems model.Adopt fourth-order Runge-Kutta method by state equation discretize time discrete, nonlinear discrete systems model is

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

In formula, x _kand x _k-1be respectively t _kand t _k-1the system state vector in moment; z _kfor t _kthe measurement vector in moment; F (x _k-1) be t _k-1the nonlinear state transfer function in moment; H (x _k) be t _kthe non-linear measurement function in moment; w _kand w _k-1be respectively t _kand t _k-1the system noise vector in moment; η _kfor t _kthe measurement noise vector in moment, and w _kand η _kmeet

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

In formula, δ _kjit is δ function.

Discrete system noise variance intensity battle array is

Q _k＝G(x _k)QG(x _k) ^TT _s

Discrete measuring noise square difference intensity battle array is

R _k＝R/T _f

In formula, Q _kfor discrete system noise w _kvariance intensity battle array; G (x _k) be t _kthe system noise input function in moment; Q is the variance intensity battle array of continuous system noise w; T _sfor update cycle time; R _kfor discrete system noise η _kvariance intensity battle array; R is the variance intensity battle array of continuous system noise η; T _ffor the filtering cycle.

3. in the step 4 described in, volume Kalman nonlinear filter algorithm block diagram as shown in Figure 2, nonlinear feature is according to the state equation of nonlinear discrete systems model set up in step 3 and measurement equation, build volume Kalman nonlinear filter, its filter step is

1) initialization system state vector and variance matrix thereof

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

In formula, and P ₀be respectively t ₀the system state vector estimated value and variance matrix thereof in moment;

2) time upgrades

Decompose the variance matrix of back:

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

In formula, P _k-1|k-1and S _k-1|k-1be respectively t _k-1the system state vector variance matrix in moment and square root battle array thereof;

Computing mode volume point:

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

In formula, χ _{i, k-1|k-1}for t _k-1i-th volume point of the system state vector in moment; for t _k-1the system state vector estimated value in moment; ξ _ifor volume Kalman i-th volume point;

Calculate the volume point that nonlinear state function is propagated:

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

In formula, for χ _{i, k-1|k-1}by nonlinear state function F (χ _{i, k-1|k-1}) the volume point propagated;

The one-step prediction value of computing mode:

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

In formula, for the t of one-step prediction _kmoment system state vector; a _ifor volume point ξ _icorresponding weight; M is the total number of volume point; I is the sequence number of volume point;

Calculate one-step prediction variance matrix:

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

In formula, P _k|k-1for the t of one-step prediction _kmoment system state vector variance matrix; Q _k-1for t _k-1the system noise variance intensity battle array in moment;

More new state and state variance battle array:

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P _k|k-1→P _k|k

In formula, for t _kmoment system state vector estimated value; P _k|kfor t _kmoment system state vector variance matrix;

In formula, A → B represents the value assignment of A to B;

3) arrive if measure time update cycle, then carry out measurement and upgrade

Decompose one-step prediction variance matrix:

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

In formula, S _k|k-1for the t of one-step prediction _kthe square root battle array of moment system state vector variance matrix;

Calculate one-step prediction volume point:

${\mathrm{\χ}}_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

In formula, χ _{i, k|k-1}for the t of one-step prediction _ki-th volume point of moment system state vector;

Calculate the measurement volume point propagated by non-linear measurement function:

Z _i,k|k-1＝H(χ _i,k|k-1)

In formula, Z _{i, k|k-1}for χ _{i, k|k-1}by non-linear measurement function H (χ _{i, k|k-1}) the volume point propagated, i.e. the t of one-step prediction _ki-th volume point of moment system measurements vector;

Calculate one-step prediction measuring value:

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{Z}_{i,k|k-1}{a}_i$

In formula, for the t of one-step prediction _kmoment measures vector;

Calculate one-step prediction and measure variance matrix:

$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

In formula, P _{zz, k|k-1}for the t of one-step prediction _kmoment measures variance matrix;

Calculate one-step prediction covariance matrix:

$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

In formula, P _{xz, k|k-1}for the t of one-step prediction _kmoment covariance matrix;

The gain of computer card Kalman Filtering:

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

In formula, K _kfor t _kmoment Kalman filtering gain battle array;

More new state:

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

Upgrade state variance battle array:

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

Volume Kalman volume point and respective weights are

$\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\[1\]}_i\\ a_i=\frac{1}{20}\end{array},i=1,...,20$

In formula,

${\[1\]}_i\∈\{\left(\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right)\},$

And each column vector has 10 elements, have 20 column vectors;

Volume point total number is

m＝20；

In volume Kalman nonlinear filtering, update cycle time is different from the measurement update cycle, and update cycle time is consistent with the Data Update cycle of gyroscope and accelerometer, and the cycle is T _s; Realize strap-down navigation parameter in time renewal process to upgrade simultaneously; Measure the update cycle consistent with the measurement amount update cycle, be the filtering cycle, the cycle is T _f.

4., because system state vector is navigational parameter in the step 5 described in, therefore filtering exports does not need to carry out error correction, is directly navigational parameter, i.e. v _east, v _north, θ, γ and ψ.

The content be not described in detail in instructions of the present invention belongs to the known prior art of professional and technical personnel in the field.

The invention has the advantages that:

1) in the present invention, quantity of state directly adopts navigational parameter, and measurement amount adopts carrier system speed, and state equation and range equation are non-linear, and model does not do any approximate and constraint, more accurately;

2) in the present invention, filtering exports is directly navigational parameter, and do not need to carry out error correction, strapdown inertial navitation system (SINS) navigational parameter upgrades and upgrades synchronous realization with the time of wave filter, and algorithm is simple;

3) the present invention adopts volume Kalman nonlinear filtering algorithm, more meets the non-linear nature of inertial navigation system;

Therefore the present invention is that Information Fusion for Integrated Navigation Application based on strapdown inertial navigation system provides a kind of new solution, can realize accurate navigator fix.

The scheme that the present invention proposes is verified by following semi physical:

1) this test figure derives from the actual sport car data of certain type inertial navigation IMU, odometer assisting navigation, carrier system forward speed measurement amount is provided, carrier is right, upper both direction speed is 0, form inertial navigation/odometer integrated navigation system, compare to obtain positioning error by the positional information exported with external differential GPS;

2) inertial sensor data update cycle T _sfor 5ms, filtering cycle T _ffor 0.1s, simulation time 75 minutes, before 5 minutes for integrated navigation system initialization, total kilometres are 33.5km, are finally parked in default terminal;

3) three random constant value of accelerometer are biased and are 0.2mg, and random noise all establishes 0.2mg, and three gyroscope Random Constant Drift are 0.03 °/h, and random noise is 0.03 °/h;

4) initial position: latitude is 29.654 °, initial longitude is 115.98 °;

5) starting condition of wave filter is set to

X=[-v _bysin (ψ) cos (θ), v _bycos (ψ) cos (θ), θ, γ, ψ, 0,0,0,0,0] ^t, θ in formula, γ, ψ, v _bybe respectively the forward speed that attitude angle at the end of integrated navigation system initialization in 5 minutes and odometer provide;

Z=[0, v _by, 0] ^t, v _byfor the forward speed that odometer provides in real time;

P ₀＝diag{(0.1m/s) ²,(0.1m/s) ²,(0.1°) ²,(0.1°) ²,(0.3°) ²,(0.2mg) ²,(0.2mg) ²,(0.03°/h) ²,(0.03°/h) ²,(0.03°/h) ²}；

Q＝diag{(0.2mg) ²,(0.2mg) ²,(0.03°/h) ²,(0.03°/h) ²,(0.03°/h) ²,0,0,0,0,0}；

R＝diag{(0.1m/s) ²,(0.1m/s) ²,(0.1m/s) ²}。

In formula, diag{.} represents diagonal matrix.

By Computer Simulation, the east orientation speed v of integrated navigation system _east, north orientation speed v _north, pitch angle θ, roll angle γ, course angle ψ and auditory localization cues be as shown in accompanying drawing 3-Fig. 8.Terminal positioning precision is 0.47%D (D is mileage number), can realize accurate location, meet integrated navigation system requirement completely.

Accompanying drawing explanation

Fig. 1 is the embodiment of Combinated navigation method of the present invention;

Fig. 2 is the algorithm block diagram of volume Kalman nonlinear filtering of the present invention;

Fig. 3 is the east orientation speed v of semi-physical simulation integrated navigation system of the present invention _eastcurve map;

Fig. 4 is the north orientation speed v of semi-physical simulation integrated navigation system of the present invention _northcurve map;

Fig. 5 is the curve map of the pitch angle θ of semi-physical simulation integrated navigation system of the present invention;

Fig. 6 is the curve map of the roll angle γ of semi-physical simulation integrated navigation system of the present invention;

Fig. 7 is the curve map of the course angle ψ of semi-physical simulation integrated navigation system of the present invention;

Fig. 8 is the auditory localization cues figure of semi-physical simulation integrated navigation system of the present invention.

Embodiment

Below in conjunction with accompanying drawing 1, carry out semi-physical simulation by sport car measured data, detailed description done to the present invention:

Step 1, initialization integrated navigation system, the navigational parameter for system upgrades and provides initial value: use location sensor obtains the initial position parameters of carrier; The linear acceleration information that the angular velocity information recorded according to gyroscope and accelerometer record, and the carrier system speed that speed pickup records, initial attitude Eulerian angle information and the initial navigation system velocity information of carrier is obtained by strapdown inertial navigation system Alignment Algorithm.

Step 2, the navigational parameter differential equation according to strapdown inertial navigation system, inertial sensor model, and with the measurement equation that carrier system speed is measurement amount, selected 10 dimension system state amount and 3 dimension measurement amounts, set up nonlinear continuous model, be specially:

Using sky, northeast (east-north-up) geographic coordinate system as navigational coordinate system (n system), the coordinate system that on right front using carrier, (bx-by-bz) direction vector right-hand rule is formed is as carrier coordinate system (b system).

The velocity differentials equation of strapdown inertial navigation system is

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

In formula, v _east, v _northand v _upbe respectively navigate east orientation, north orientation and sky in being to speed; [.] ^trepresenting matrix transposition; ω _iefor earth rotation angular speed, L is local geographic latitude; r _mfor radius of curvature of meridian, r _nfor radius of curvature in prime vertical; r _m=r _q(1-2 ρ+3 ρ sin ²l), r _n=r _q(ρ sin ²l+1), r _qfor reference ellipsoid equatorial plane radius, ρ is compression of the earth; and $f_{ib}^b={\[{\tilde{f}}_{ibx}^b-{\mathrm{\δf}}_{ibx}^b,{\tilde{f}}_{iby}^b-{\mathrm{\δf}}_{iby}^b,{\tilde{f}}_{ibz}^b-{\mathrm{\δf}}_{ibz}^b\]}^T,$ with be respectively the measured value of carrier system change in coordinate axis direction three accelerometers, with be respectively three corresponding measuring error of accelerometer, measuring error is divided into two parts, and a part is random constant value ▽ _ax, ▽ _ayand ▽ _az, another part is random noise w _axand w _ayand w _az, random noise is all assumed to be zero mean Gaussian white noise,

${\mathrm{\δf}}_{ibx}^b={\▿}_{ax}+w_{ax},{\mathrm{\δf}}_{iby}^b={\▿}_{ay}+w_{ay},{\mathrm{\δf}}_{ibz}^b={\▿}_{az}+w_{az};C_b^n={\left(C_n^b\right)}^T,$ $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right],$ θ is pitch angle, and γ is roll angle, and ψ is course angle; g ⁿ=[0,0 ,-g] ^t, g is acceleration of gravity;

The Eulerian angle differential equation of strapdown inertial navigation system is

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\γ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

In formula, $\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n),$

${\mathrm{\ω}}_{ib}^b={\[{\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\δ\ω}}_{ibx}^b,{\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\δ\ω}}_{iby}^b,{\widetilde{\mathrm{\ω}}}_{ibz}^b-{\mathrm{\δ\ω}}_{ibz}^b\]}^T,$ with for carrier system change in coordinate axis direction three gyroscope survey values, with be three corresponding measuring error of gyroscope, measuring error is divided into two parts, and a part is random constant value ε _gx, ε _gyand ε _gz, another part is random noise w _gx, w _gyand w _gz, random noise is all assumed to be zero mean Gaussian white noise, ${\mathrm{\δ\ω}}_{ibx}^b={\mathrm{\ϵ}}_{gx}+w_{gx},{\mathrm{\δ\ω}}_{iby}^b={\mathrm{\ϵ}}_{gy}+w_{gy},{\mathrm{\δ\ω}}_{ibz}^b={\mathrm{\ϵ}}_{gz}+w_{gz};$

The random constant value department pattern of inertial sensor (gyroscope and the accelerometer) measuring error of strapdown inertial navigation system is

${\stackrel{\·}{\▿}}_{ax}=0,{\stackrel{\·}{\▿}}_{ay}=0,{\stackrel{\·}{\▿}}_{az}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gx}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gy}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gz}=0---\left(3\right)$

Selected 10 maintain system state vector is

x＝[v _east,v _north,θ,γ,ψ,▽ _ax,▽ _ay,ε _gx,ε _gy,ε _gz] ^T

In formula, x is system state vector;

System noise vector is

w＝[w _ax,w _ay,w _gx,w _gy,w _gz,0,0,0,0,0] ^T

In formula, w is system noise vector;

3 dimension amounts are measured as

z＝[v _bx,v _by,v _bz] ^T

In formula, z is for measuring vector; v _bx, v _byand v _bzbe respectively the speed component of carrier system three change in coordinate axis direction;

Measurement noise vector is

η＝[η _vbx,η _vby,η _vbz] ^T

In formula, η is measurement noise vector, η _vbx, η _vby, η _vbzbe respectively speed v _bx, v _byand v _bzmeasurement noise, be all assumed to be zero mean Gaussian white noise;

Measurement equation is

$z=C_n^b{\[v_{east},v_{north},v_{up}\]}^T+\mathrm{\η}---\left(4\right)$

Nonlinear filtering continuous system model is set up as follows according to the velocity differentials equation of strapdown inertial navigation system, the attitude Eulerian angle differential equation, inertial sensor model and measurement equation:

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+\mathrm{\η}\end{array}\right.$

In formula, F (x) is nonlinear state transfer function; H (x) is non-linear measurement function; G (x) is system noise input function; F (x) and G (x) writes out with reference to formula (1)-(3), and H (x) writes out with reference to formula (4).

Step 3, the nonlinear continuous model discretize will set up in step 2, form nonlinear discrete systems model;

Adopt fourth-order Runge-Kutta method by state equation discretize time discrete, after discretize, nonlinear discrete systems model is

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

In formula, x _kand x _k-1be respectively t _kand t _k-1the system state vector in moment; z _kfor t _kthe measurement vector in moment; F (x _k-1) be t _k-1the nonlinear state transfer function in moment; H (x _k) be t _kthe non-linear measurement function in moment; w _kand w _k-1be respectively t _kand t _k-1the system noise vector in moment; η _kfor t _kthe measurement noise vector in moment, and w _kand η _kmeet

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

In formula, δ _kjit is δ function;

Discrete system noise variance intensity battle array is

Q _k＝G(x _k)QG(x _k) ^TT _s

Discrete measuring noise square difference intensity battle array is

R _k＝R/T _f

In formula, Q _kfor discrete system noise w _kvariance intensity battle array; G (x _k) be t _kthe system noise input function in moment; Q is the variance intensity battle array of continuous system noise w; T _sfor update cycle time; R _kfor discrete system noise η _kvariance intensity battle array; R is the variance intensity battle array of continuous system noise η; T _ffor the filtering cycle.

Step 4, be nonlinear feature according to state equation and the measurement equation of the nonlinear discrete systems model set up in step 3, build volume Kalman nonlinear filtering algorithm, in filtering, update cycle time is different from measuring the update cycle;

The filter step of volume Kalman nonlinear filter is

1) initialization system state vector and variance matrix thereof

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

In formula, and P ₀be respectively t ₀the system state vector estimated value and variance matrix thereof in moment;

2) time upgrades

Decompose the variance matrix of back:

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

In formula, P _k-1|k-1and S _k-1|k-1be respectively t _k-1the system state vector variance matrix in moment and square root battle array thereof;

Computing mode volume point:

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k-1|k-1}$

In formula, χ _{i, k-1|k-1}for t _k-1i-th volume point of the system state vector in moment; for t _k-1the system state vector estimated value in moment; ξ _ifor volume Kalman i-th volume point;

Calculate the volume point that nonlinear state function is propagated:

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

In formula, for χ _{i, k-1|k-1}by nonlinear state function F (χ _{i, k-1|k-1}) the volume point propagated;

The one-step prediction value of computing mode:

${\hat{x}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

In formula, for the t of one-step prediction _kmoment system state vector; a _ifor volume point ξ _icorresponding weight; M is the total number of volume point; I is the sequence number of volume point;

Calculate one-step prediction variance matrix:

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

In formula, P _k|k-1for the t of one-step prediction _kmoment system state vector variance matrix; Q _k-1for t _k-1the system noise variance intensity battle array in moment;

More new state and state variance battle array:

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P _k|k-1→P _k|k

In formula, for t _kmoment system state vector estimated value; P _k|kfor t _kmoment system state vector variance matrix;

In formula, A → B represents the value assignment of A to B;

3) arrive if measure time update cycle, then carry out measurement and upgrade

Decompose one-step prediction variance matrix:

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

In formula, S _k|k-1for the t of one-step prediction _kthe square root battle array of moment system state vector variance matrix;

Calculate one-step prediction volume point:

${\mathrm{\χ}}_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

In formula, χ _{i, k|k-1}for the t of one-step prediction _ki-th volume point of moment system state vector;

Calculate the measurement volume point propagated by non-linear measurement function:

Z _i,k|k-1＝H(χ _i,k|k-1)

In formula, Z _{i, k|k-1}for χ _{i, k|k-1}by non-linear measurement function H (χ _{i, k|k-1}) the volume point propagated, i.e. the t of one-step prediction _ki-th volume point of moment system measurements vector;

Calculate one-step prediction measuring value:

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{Z}_{i,k|k-1}{a}_i$

In formula, for the t of one-step prediction _kmoment measures vector;

Calculate one-step prediction and measure variance matrix:

$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$

In formula, P _{zz, k|k-1}for the t of one-step prediction _kmoment measures variance matrix;

Calculate one-step prediction covariance matrix:

$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

In formula, P _{xz, k|k-1}for the t of one-step prediction _kmoment covariance matrix;

The gain of computer card Kalman Filtering:

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

In formula, K _kfor t _kmoment Kalman filtering gain battle array;

More new state:

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

Upgrade state variance battle array:

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

Volume Kalman volume point and respective weights are

$\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\[1\]}_i\\ a_i=\frac{1}{20}\end{array},i=1,...,20$

In formula,

${\[1\]}_i\∈\{\left(\begin{array}{c}1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ .\\ .\\ .\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ .\\ .\\ .\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ .\\ .\\ .\\ -1\end{array}\right)\},$

And each column vector has 10 elements, have 20 column vectors;

Volume point total number is

m＝20；

In volume Kalman nonlinear filtering, update cycle time is different from the measurement update cycle, and update cycle time is consistent with the Data Update cycle of gyroscope and accelerometer, and the cycle is T _s; Realize strap-down navigation parameter in time renewal process to upgrade simultaneously; Measure the update cycle consistent with the measurement amount update cycle, be the filtering cycle, the cycle is T _f.

Step 5, the carrier system speed output of inertial sensor and aiding sensors provided substitute in the filtering algorithm of step 4, carry out nonlinear filtering, directly export the navigational parameter after combination.

Because quantity of state is navigational parameter, therefore filtering exports does not need to carry out error correction, is directly navigational parameter, i.e. v _east, v _north, θ, γ and ψ.

In the description of this instructions, specific features, structure, material or feature that the description of reference term " embodiment ", " some embodiments ", " example ", " concrete example " or " some examples " etc. means to describe in conjunction with this embodiment or example are contained at least one embodiment of the present invention or example.In this manual, identical embodiment or example are not necessarily referred to the schematic representation of above-mentioned term.And the specific features of description, structure, material or feature can combine in an appropriate manner in any one or more embodiment or example.

Although illustrate and describe embodiments of the invention, those having ordinary skill in the art will appreciate that: can carry out multiple change, amendment, replacement and modification to these embodiments when not departing from principle of the present invention and aim, scope of the present invention is by claim and equivalents thereof.

Claims (1)

1., based on the volume Kalman nonlinear combination air navigation aid of carrier system speeds match, it is characterized in that the method comprises the following steps:

Step 1, initialization integrated navigation system, the navigational parameter for system upgrades and provides initial value: use location sensor obtains the initial position parameters of carrier; The linear acceleration information that the angular velocity information recorded according to gyroscope and accelerometer record, and the carrier system speed that speed pickup records, use Alignment Algorithm to obtain initial attitude Eulerian angle information and the initial navigation system velocity information of carrier by strapdown inertial navigation system;

Step 2, the navigational parameter differential equation according to strapdown inertial navigation system, inertial sensor model, and with the measurement equation that carrier system speed is measurement amount, selected 10 dimension system state amount and 3 dimension measurement amounts, set up nonlinear continuous model;

Using the world, northeast reason coordinate system as navigational coordinate system, i.e. n system, the coordinate system formed using carrier front upper right to the vector right-hand rule as carrier coordinate system, i.e. b system;

The velocity differentials equation of strapdown inertial navigation system is

${\stackrel{\·}{v}}_{en}^n=f^n-(2{\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n)\×v_{en}^n+g^n---\left(1\right)$

In formula, v _east, v _northand v _upbe respectively navigate east orientation, north orientation and sky in being to speed; [.] ^trepresenting matrix transposition; ω _iefor earth rotation angular speed, L is local geographic latitude; r _mfor radius of curvature of meridian, r _nfor radius of curvature in prime vertical; r _m=r _q(1-2 ρ+3 ρ sin ²l), r _n=r _q(ρ sin ²l+1), r _qfor reference ellipsoid equatorial plane radius, ρ is compression of the earth; and $f_{ib}^b={\[{\tilde{f}}_{ibx}^b-{\mathrm{\δf}}_{ibx}^b,{\tilde{f}}_{iby}^b-{\mathrm{\δf}}_{iby}^b,{\tilde{f}}_{ibz}^b-{\mathrm{\δf}}_{ibz}^b\]}^T,$ with be respectively the measured value of carrier system change in coordinate axis direction three accelerometers, with be respectively three corresponding measuring error of accelerometer, measuring error is divided into two parts, and a part is random constant value ▽ _ax, ▽ _ayand ▽ _az, another part is random noise w _axand w _ayand w _az, random noise is all assumed to be zero mean Gaussian white noise,

${\mathrm{\δf}}_{ibx}^b={\▿}_{ax}+w_{ax},{\mathrm{\δf}}_{iby}^b={\▿}_{ay}+w_{ay},{\mathrm{\δf}}_{ibz}^b={\▿}_{az}+w_{az};C_b^n={\left(C_n^b\right)}^T,$ $C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\γ}\cos \mathrm{\ψ}-\sin \mathrm{\θ}\sin \mathrm{\γ}\sin \mathrm{\ψ}& \cos \mathrm{\γ}\sin \mathrm{\ψ}+\sin \mathrm{\θ}\sin \mathrm{\γ}\cos \mathrm{\ψ}& -\cos \mathrm{\θ}\sin \mathrm{\γ}\\ -\cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}& -\sin \mathrm{\θ}\\ \sin \mathrm{\γ}\cos \mathrm{\ψ}+\sin \mathrm{\θ}\cos \mathrm{\γ}\sin \mathrm{\ψ}& \sin \mathrm{\γ}\sin \mathrm{\ψ}-\sin \mathrm{\θ}\cos \mathrm{\γ}\cos \mathrm{\ψ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right],$ θ is pitch angle, and γ is roll angle, and ψ is course angle; g ⁿ=[0,0 ,-g] ^t, g is acceleration of gravity;

The Eulerian angle differential equation of strapdown inertial navigation system is

$\left[\begin{array}{c}\stackrel{\·}{\mathrm{\θ}}\\ \stackrel{\·}{\mathrm{\γ}}\\ \stackrel{\·}{\mathrm{\ψ}}\end{array}\right]=\frac{1}{\cos \mathrm{\θ}}\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\γ}& 0& \sin \mathrm{\γ}\cos \mathrm{\θ}\\ \sin \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}& -\cos \mathrm{\γ}\sin \mathrm{\θ}\\ -\sin \mathrm{\ψ}& 0& \cos \mathrm{\γ}\end{array}\right]\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]---\left(2\right)$

In formula, $\left[\begin{array}{c}{\mathrm{\ω}}_{nbx}^b\\ {\mathrm{\ω}}_{nby}^b\\ {\mathrm{\ω}}_{nbz}^b\end{array}\right]={\mathrm{\ω}}_{ib}^b-C_n^b({\mathrm{\ω}}_{ie}^n+{\mathrm{\ω}}_{en}^n),$

${\mathrm{\ω}}_{ib}^b={\[{\widetilde{\mathrm{\ω}}}_{ibx}^b-{\mathrm{\δ\ω}}_{ibx}^b,{\widetilde{\mathrm{\ω}}}_{iby}^b-{\mathrm{\δ\ω}}_{iby}^b,{\widetilde{\mathrm{\ω}}}_{ibz}^b-{\mathrm{\δ\ω}}_{ibz}^b\]}^T,$ with for carrier system change in coordinate axis direction three gyroscope survey values, with be three corresponding measuring error of gyroscope, measuring error is divided into two parts, and a part is random constant value ε _gx, ε _gyand ε _gz, another part is random noise w _gx, w _gyand w _gz, random noise is all assumed to be zero mean Gaussian white noise, ${\mathrm{\δ\ω}}_{ibx}^b={\mathrm{\ϵ}}_{gx}+w_{gx},{\mathrm{\δ\ω}}_{iby}^b={\mathrm{\ϵ}}_{gy}+w_{gy},{\mathrm{\δ\ω}}_{ibz}^b={\mathrm{\ϵ}}_{gz}+w_{gz};$

The random constant value department pattern of inertial sensor (gyroscope and the accelerometer) measuring error of strapdown inertial navigation system is

${\stackrel{\·}{\▿}}_{ax}=0,{\stackrel{\·}{\▿}}_{ay}=0,{\stackrel{\·}{\▿}}_{az}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gx}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gy}=0,{\stackrel{\·}{\mathrm{\ϵ}}}_{gz}=0---\left(3\right)$

Selected 10 maintain system state vector is

x＝[v _east,v _north,θ,γ,ψ,▽ _ax,▽ _ay,ε _gx,ε _gy,ε _gz] ^T

In formula, x is system state vector;

System noise vector is

w＝[w _ax,w _ay,w _gx,w _gy,w _gz,0,0,0,0,0] ^T

In formula, w is system noise vector;

3 dimension amounts are measured as

z＝[v _bx,v _by,v _bz] ^T

In formula, z is for measuring vector; v _bx, v _byand v _bzbe respectively the speed component of carrier system three change in coordinate axis direction;

Measurement noise vector is

η＝[η _vbx,η _vby,η _vbz] ^T

In formula, η is measurement noise vector, η _vbx, η _vby, η _vbzbe respectively speed v _bx, v _byand v _bzmeasurement noise, be all assumed to be zero mean Gaussian white noise;

Measurement equation is

$z=C_n^b{\[v_{east},v_{north},v_{up}\]}^T+\mathrm{\η}---\left(4\right)$

Nonlinear filtering continuous system model is set up as follows according to the velocity differentials equation of strapdown inertial navigation system, the attitude Eulerian angle differential equation, inertial sensor model and measurement equation:

$\left\{\begin{array}{c}\stackrel{\·}{x}=F\left(x\right)+G\left(x\right)w\\ z=H\left(x\right)+\mathrm{\η}\end{array}\right.$

In formula, F (x) is nonlinear state transfer function; H (x) is non-linear measurement function; G (x) is system noise input function; F (x) and G (x) writes out with reference to formula (1)-(3), and H (x) writes out with reference to formula (4);

Step 3, the nonlinear continuous model discretize will set up in step 2, form nonlinear discrete systems model;

Adopt fourth-order Runge-Kutta method by state equation discretize, nonlinear discrete systems model is

$\left\{\begin{array}{c}{x}_k=F\left(x_{k-1}\right)+w_{k-1}\\ z_k=H\left(x_k\right)+{\mathrm{\η}}_k\end{array}\right.$

In formula, x _kand x _k-1be respectively t _kand t _k-1the system state vector in moment; z _kfor t _kthe measurement vector in moment; F (x _k-1) be t _k-1the nonlinear state transfer function in moment; H (x _k) be t _kthe non-linear measurement function in moment; w _kand w _k-1be respectively t _kand t _k-1the system noise vector in moment; η _kfor t _kthe measurement noise vector in moment, and w _kand η _kmeet

$\left\{\begin{array}{c}E\[w_k\]=0,E\[w_k{w}_j^T\]=Q_k{\mathrm{\δ}}_{kj}\\ E\[{\mathrm{\η}}_k\]=0,E\[{\mathrm{\η}}_k{\mathrm{\η}}_j^T\]=R_k{\mathrm{\δ}}_{kj}\\ E\[w_k{\mathrm{\η}}_j^T\]=0\end{array}\right.$

In formula, δ _kjit is δ function;

Discrete system noise variance intensity battle array is

Q _k＝G(x _k)QG(x _k) ^TT _s

Discrete measuring noise square difference intensity battle array is

R _k＝R/T _f

In formula, Q _kfor discrete system noise w _kvariance intensity battle array; G (x _k) be t _kthe system noise input function in moment; Q is the variance intensity battle array of continuous system noise w; T _sfor update cycle time; R _kfor discrete system noise η _kvariance intensity battle array; R is the variance intensity battle array of continuous system noise η; T _ffor the filtering cycle;

Step 4, be nonlinear feature according to state equation and the measurement equation of the nonlinear discrete systems model set up in step 3, build volume Kalman nonlinear filtering algorithm, in filtering, the time upgrades different from measuring the update cycle;

Wherein the filter step of volume Kalman nonlinear filter is

1) initialization system state vector and variance matrix thereof

${\hat{x}}_0=E\left(x_0\right),P_0=E\left(\right({\hat{x}}_0-x_0\left){\left({\hat{x}}_0-x_0\right)}^T\right)$

In formula, and P ₀be respectively t ₀the system state vector estimated value and variance matrix thereof in moment;

2) time upgrades

Decompose the variance matrix of back:

$P_{k-1|k-1}=S_{k-1|k-1}{S}_{k-1|k-1}^T$

In formula, P _k-1|k-1and S _k-1|k-1be respectively t _k-1the system state vector variance matrix in moment and square root battle array thereof;

Computing mode volume point:

${\mathrm{\χ}}_{i,k-1|k-1}=S_{k-1|k-1}{\mathrm{\ξ}}_i+{\hat{X}}_{k-1|k-1}$

In formula, χ _{i, k-1|k-1}for t _k-1i-th volume point of the system state vector in moment; for t _k-1the system state vector estimated value in moment; ξ _ifor volume Kalman i-th volume point;

Calculate the volume point that nonlinear state function is propagated:

${\mathrm{\χ}}_{i,k|k-1}^{*}=F\left({\mathrm{\χ}}_{i,k-1|k-1}\right)$

In formula, for χ _{i, k-1|k-1}by nonlinear state function F (χ _{i, k-1|k-1}) the volume point propagated;

The one-step prediction value of computing mode:

${\hat{X}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{\mathrm{\χ}}_{i,k|k-1}^{*}{a}_i$

In formula, for the t of one-step prediction _kmoment system state vector; a _ifor volume point ξ _icorresponding weight; M is the total number of volume point; I is the sequence number of volume point;

Calculate one-step prediction variance matrix:

$P_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}^{*}{\mathrm{\χ}}_{i,k|k-1}^{*T}-{\hat{x}}_{k|k-1}{\hat{x}}_{k|k-1}^T+Q_{k-1}$

In formula, P _k|k-1for the t of one-step prediction _kmoment system state vector variance matrix; Q _k-1for t _k-1the system noise variance intensity battle array in moment;

More new state and state variance battle array:

${\hat{x}}_{k|k-1}\→{\hat{x}}_{k|k}$

P _k|k-1→P _k|k

In formula, for t _kmoment system state vector estimated value; P _k|kfor t _kmoment system state vector variance matrix;

In formula, A → B represents the value assignment of A to B;

3) arrive if measure time update cycle, then carry out measurement and upgrade

Decompose one-step prediction variance matrix:

$P_{k|k-1}=S_{k|k-1}{S}_{k|k-1}^T$

In formula, S _k|k-1for the t of one-step prediction _kthe square root battle array of moment system state vector variance matrix;

Calculate one-step prediction volume point:

${\mathrm{\χ}}_{i,k|k-1}=S_{k|k-1}{\mathrm{\ξ}}_i+{\hat{x}}_{k|k-1}$

In formula, χ _{i, k|k-1}for the t of one-step prediction _ki-th volume point of moment system state vector;

Calculate the measurement volume point propagated by non-linear measurement function:

Z _i,k|k-1＝H(χ _i,k|k-1)

In formula, Z _{i, k|k-1}for χ _{i, k|k-1}by non-linear measurement function H (χ _{i, k|k-1}) the volume point propagated, i.e. the t of one-step prediction _ki-th volume point of moment system measurements vector;

Calculate one-step prediction measuring value:

${\hat{z}}_{k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{Z}_{i,k|k-1}{a}_i$

In formula, for the t of one-step prediction _kmoment measures vector;

Calculate one-step prediction and measure variance matrix:

$P_{zz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{Z}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{z}}_{k|k-1}{\hat{z}}_{k|k-1}^T+R_k$ In formula, P _{zz, k|k-1}for the t of one-step prediction _kmoment measures variance matrix;

Calculate one-step prediction covariance matrix:

$P_{xz,k|k-1}=\underset{i=1}{\overset{m}{\Σ}}{a}_i{\mathrm{\χ}}_{i,k|k-1}{Z}_{i,k|k-1}^T-{\hat{x}}_{k|k-1}{\hat{z}}_{k|k-1}^T$

In formula, P _{xz, k|k-1}for the t of one-step prediction _kmoment covariance matrix;

The gain of computer card Kalman Filtering:

$K_k=P_{xz,k|k-1}{P}_{zz,k|k-1}^{-1}$

In formula, K _kfor t _kmoment Kalman filtering gain battle array;

More new state:

${\hat{x}}_{k|k}={\hat{x}}_{k|k-1}+K_k(z_k-{\hat{z}}_{k|k-1})$

Upgrade state variance battle array:

$P_{k|k}=P_{k|k-1}-K_k{P}_{zz,k|k-1}{K}_k^T$

Volume Kalman volume point and respective weights are

$\left\{\begin{array}{c}{\mathrm{\ξ}}_i=\sqrt{10}{\[1\]}_i\\ a_i=\frac{1}{20}\end{array}\right.,i=1,...,20$

In formula,

${\[1\]}_i\∈\{\left(\begin{array}{c}1\\ 0\\ \·\\ \·\\ \·\\ 0\end{array}\right),\left(\begin{array}{c}0\\ 1\\ \·\\ \·\\ \·\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ 1\end{array}\right),\left(\begin{array}{c}-1\\ 0\\ \·\\ \·\\ \·\\ 0\end{array}\right),\left(\begin{array}{c}0\\ -1\\ \·\\ \·\\ \·\\ 0\end{array}\right),...,\left(\begin{array}{c}0\\ 0\\ \·\\ \·\\ \·\\ -1\end{array}\right)\},$

And each column vector has 10 elements, have 20 column vectors;

Volume point total number is

m＝20；

In volume Kalman nonlinear filtering, update cycle time is different from the measurement update cycle, and update cycle time is consistent with the Data Update cycle of gyroscope and accelerometer, and the cycle is T _s; Realize strap-down navigation parameter in time renewal process to upgrade simultaneously; Measure the update cycle consistent with the measurement amount update cycle, be the filtering cycle, the cycle is T _f;

Step 5, the carrier system speed output of inertial sensor and aiding sensors provided substitute in the filtering algorithm of step 4, carry out nonlinear filtering, directly export the navigational parameter after combination; Because system state vector is navigational parameter, thus filtering output does not need to carry out error correction, is directly navigational parameter, i.e. v _east, v _north, θ, γ and ψ.