CN104061930A - Navigation method based on strapdown inertial guidance and Doppler log - Google Patents

Abstract

The invention discloses a navigation method based on strapdown inertial guidance and a Doppler log. The navigation method is characterized in that the navigation method is applied to a navigation system based on strapdown inertial guidance and the Doppler log, wherein the navigation system comprises the Doppler log, a strapdown inertial measurement unit, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four energy transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerator; the strapdown inertial measurement unit is connected with the strapdown inertial measurement unit processing module; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises a plurality of steps, and the method overcomes the defects that an AUV (autonomous underwater vehicle) navigation system is easily influenced by environment and the navigation and positioning error of a strapdown inertial navigation system cannot reach the requirements of accuracy with continued accumulation of time.

Description

Navigation method based on strapdown inertial guidance and Doppler log

Technical Field

The invention relates to a navigation method based on strapdown inertial guidance and a Doppler log, belonging to the field of navigation, positioning and tracking of underwater and water surface aircrafts.

Background

AUV (Autonomous Underwater Vehicle) is an important tool for exploring and developing and utilizing marine environmental resources, and plays an important role in marine military application. The problem of navigation of AUVs remains one of the major technical challenges facing today due to their remoteness, concealment, and autonomy. Because the underwater environment is complex, an integrated navigation mode is often adopted to perform navigation positioning on the AUV, and the design of a filter of an integrated navigation system is the key for ensuring the navigation precision. The navigation positioning precision of the conventional AUV can reach about <0.8 nmile/h at present, and the accuracy requirement of AUV navigation positioning is met to a certain extent.

The SINS (Strapdown Inertial Navigation system) system has autonomous Navigation capability, is not influenced by environment, carrier maneuvering and radio interference, can continuously provide Navigation positioning information such as carrier position, speed and attitude, has high data updating frequency and higher relative precision in a short time. However, as the operating time of the system is prolonged, the navigation error of the strapdown inertial navigation system is accumulated and increased, and at this time, the error accumulated over time needs to be suppressed by correcting and compensating the strapdown inertial navigation system through a filtering algorithm by using the observation information of the external sensor.

The DVL (Doppler Velocity Log Doppler Log) is a speed measuring device widely used in combined systems of underwater and surface navigation systems, and has an acoustic navigation positioning system that outputs the three-dimensional speed and range capability of a carrier in real time. The Doppler system obtains the acoustic wave frequency shift through the information of four transducers arranged at the bottom of the AUV, and then calculates to obtain the three-dimensional speed information of the carrier in the carrier system, and has the advantages of high precision, reliability, real-time performance and the like. However, due to the complex underwater environment, the accumulation of the position error of the dead reckoning system along with time and the long endurance and concealment requirements of the AUV, the precision of the conventional filtering algorithm of the strapdown inertial navigation/Doppler combined system is difficult to meet the requirement of a high-precision navigation positioning function.

The conventional H-infinity filtering algorithm does not make any assumption on the statistical characteristics of the system, has the advantages of simple filtering model and good robustness, and is often applied to the AUV navigation system. However, the conventional H-infinity filter has weak capability of tracking a maneuvering process, and the instantaneity is difficult to guarantee. When the state estimation value of the filter deviates from the system state due to the influence of factors such as model uncertainty, the state estimation value of the filter is necessarily expressed on the mean value and the amplitude of the output residual sequence, and at the moment, if the time-varying gain matrix is adjusted on line, the system estimation error variance is minimum, and the residual sequences are still mutually orthogonal, namely, the orthogonality principle is satisfied, the filter can be forced to keep high-precision tracking on the actual system state. The invention implements the fading on the past data by introducing the suboptimal fading factor into the H-infinity filtering algorithm to weaken the influence of old data on the current filtering estimation value, namely, the filter is forced to track the actual system state with high precision by adjusting the estimation error variance matrix and the corresponding gain matrix in real time to meet the orthogonality principle, and all effective information in the output residual error is extracted to the greatest extent to obtain the filtering algorithm with high tracking performance. The filtering algorithm has strong robustness about model uncertainty and the tracking capability of the system maneuvering process, keeps the advantage of simplicity of an H-infinity filtering model, realizes the tracking of the filter on the system state of the AUV maneuvering process, and has important theoretical significance and engineering practical value.

The AUV integrated navigation system based on the strapdown inertial navigation/Doppler combines the advantages of the strapdown inertial navigation, Doppler and the filtering algorithm provided by the invention, can provide real-time navigation tracking parameters with high precision of a carrier, and solves the problems that the AUV navigation system is easily influenced by the environment and the navigation positioning error of the strapdown inertial navigation continuously accumulates along with the time and cannot meet the precision requirement.

Disclosure of Invention

The purpose of the invention is as follows: the invention aims to overcome the defects of the prior art and provides a filtering method for an AUV (autonomous underwater vehicle) integrated navigation system for strapdown inertial navigation/Doppler (AUV). And the designed filter is utilized to introduce Doppler velocity measurement information into an AUV combined system so as to assist strapdown inertial navigation to carry out navigation positioning. The method can integrate a plurality of sub navigation information, and overcomes the defects that an AUV navigation system is easily influenced by the environment and the navigation positioning error of a strapdown inertial navigation system continuously accumulates along with the time and cannot meet the precision requirement.

The technical scheme is as follows: the invention relates to a navigation method based on a strapdown inertial guidance and a Doppler log, which is applied to a navigation system based on the strapdown inertial guidance and the Doppler log, wherein the navigation system comprises the Doppler log, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit is connected with the processing module of the strapdown inertial measurement unit; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises the following steps:

(1) measuring triaxial angular velocity information through a triaxial gyroscope and triaxial acceleration information through an accelerometer in the strapdown inertial measurement unit assembly, receiving navigation information output by the strapdown inertial measurement unit through a processing module of the strapdown inertial measurement unit, and obtaining navigation information such as carrier position, velocity and attitude through navigation integral calculation;

(2) transmitting an electric oscillation signal to the transducer through a transceiver of the Doppler log;

(3) transmitting ultrasonic waves and receiving reflected echoes with frequency shift characteristics through four transducers of a Doppler log;

(4) by usingThe receiving system in the transceiver amplifies the echo signal sent by the transducer, then obtains Doppler frequency shift, converts the echo signal into a navigational speed analog signal and sends the navigational speed analog signal to the interface unit. If the carrier navigation speed is V, the beam launching depression angle is theta, and the sound velocity C is approximately equal to 1500 m/s, the single-beam frequency shift calculation formula is as follows: Δ f 2Vf₀cos θ/C, the velocity calculation from which is derived is: v ═ Δ fC/(2 f)₀cosθ)；

(5) Converting the four navigational speed analog signals sent by the transceiver into navigational speed by using an interface unit in the transceiver, and outputting the navigational speed analog signals to the central processing unit in a digital mode;

(6) an arithmetic unit in the central processing unit periodically calculates the real-time three-dimensional carrier system speed of the carrier according to the speed information output by the interface unit of the Doppler log;

(7) a filtering module in the central processing unit carries out filtering fusion calculation on the received strapdown inertial navigation information and the three-dimensional speed information of the Doppler log to obtain the correction value of the inertial strapdown measurement system at the moment kAfter correction, the final high-precision navigation positioning information is obtained;

the method for obtaining the correction value of the inertia strapdown measurement system through filtering fusion calculation comprises the following steps:

taking strapdown inertial navigation as a reference system, and taking a speed error by a state variable XAttitude angle error (phi)_e，φ_n，φ_u) Accelerometer random constant biasAnd gyro random constant drift (epsilon)_x，ε_y，ε_z) And 10 dimensions in total: <math><mrow> <mi>X</mi> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msubsup> <mi>&delta;V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msubsup> <mi>&delta;V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>e</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>n</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>u</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>x</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>x</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>z</mi> </msub> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow></math> W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^Tis a system noise vector;

the difference between the velocity solved by the strapdown inertial navigation and the velocity of the carrier system measured by the Doppler log after being converted into the navigation system is used as the measurement value of the system, and then the measurement equation is expressed as follows:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

wherein the observation vector isMeasurement noise vector V ═ w_vx，w_vy]^TThe system measurement matrix is H, and a discrete system filter equation can be obtained by discretizing a system state equation and a measurement equation; using initial valuesAnd P₀According to the measurement of time k_kThe state estimate up to time k can be deduced ${\hat{X}}_k(k=\mathrm{1,2,3},...):$

<math><mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow></math>

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein <math><mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>&Lambda;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow></math>

Wherein: Λ (k) = diag [ λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k) Not less than 1; i is 1, 2, …, n. In the sub-optimization method, λ is roughly determined based on a priori knowledge of the system₁(k)：λ₂(k)：…：λ_n(k) The initial values of (a) are: lambda [ alpha ]₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_nIn the formula, α_iIs not less than 1(i =1, 2, …, n), then λ is obtained_i(k) The approximation algorithm of (d) is as follows:

<math><mrow> <msub> <mi>&lambda;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&le;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

wherein,

<math><mrow> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tr</mi> <mo>[</mo> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>]</mo> </mrow> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>M</mi> <mi>ii</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow></math>

<math><mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&Gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>-</mo> <mi>&beta;</mi> <msub> <mi>I</mi> <mi>k</mi> </msub> </mrow></math>

<math><mrow> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow></math>

tr (A) represents the trace-finding operation for matrix A, where V is_0，kIs calculated by the following formula:

<math><mrow> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&gamma;</mi> <mn>1</mn> </msub> <msubsup> <mi>&gamma;</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>&rho;V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>&rho;</mi> </mrow> </mfrac> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

in the above equation, residual errorRho is more than or equal to 0.95 and less than or equal to 1, which is taken as a forgetting factor, and rho is generally 0.95; β ≧ 1 is a selected attenuation factor, typically β = 1.

Furthermore, the three-axis gyroscope of the strapdown inertial measurement unit is orthogonally installed in three axes, and the three-axis accelerometer is orthogonally installed in three axes.

Furthermore, the three-axis gyroscope of the strapdown inertial measurement unit is an optical fiber gyroscope.

Further, the triaxial accelerometer is a quartz flexible accelerometer.

Further, the transceiver of the doppler log comprises a transmitting system, a receiving system and a calculation compensation circuit.

Furthermore, the navigation system also comprises a display, wherein the display is connected with the arithmetic unit and is used for displaying the navigation information obtained by the operation of the central processing unit.

The navigation system includes: the system comprises a strapdown inertial measurement unit, a processing module of the strapdown inertial measurement unit, a Doppler log, a transceiver, a transducer, an interface unit and a central processing unit, wherein the transceiver, the transducer, the interface unit and the central processing unit are arranged on an AUV. The strapdown inertial measurement unit is used for outputting inertial measurement data; the strapdown inertial measurement unit processing module is used for receiving the digital signals output by the strapdown inertial measurement unit and obtaining carrier position, speed and attitude data through navigation integral calculation; the Doppler log is used for measuring the longitudinal speed and the transverse speed of the carrier relative to the sea bottom; the transducer which can be arranged at the bottom end of the AUV is used for transmitting ultrasonic waves and receiving reflected echoes; the transceiver arranged at the bottom end of the AUV comprises a transmitting system, a receiving system, a calculation compensation circuit and other units; the interface unit is used for converting the navigational speed signal sent by the transceiver into navigational speed and outputting data to external equipment in a digital mode or an analog mode; the central processing unit comprises an arithmetic unit and a filtering module, can receive the navigation information of the processing module of the strapdown inertial measurement unit and the Doppler log, and corrects the parameters of the processing module of the strapdown inertial measurement unit by utilizing the estimated system state through real-time filtering calculation to obtain final combined navigation information data.

The invention can be applied to AUV system, AUV is an important tool for exploring and developing and utilizing marine environment resources, and plays an important role in marine military application. Because of its long-range, disguise and complicated underwater environment, often adopt the integrated navigation mode to navigate the location to AUV. The strapdown inertial navigation system has autonomous navigation capability, is not influenced by environment, carrier maneuvering and radio interference, can continuously provide navigation positioning information such as carrier position, speed and attitude, has high data updating frequency and has higher relative precision in a short time. However, as the operating time of the system is prolonged, the navigation error of the strapdown inertial navigation system is accumulated and increased, and at this time, the error accumulated over time needs to be suppressed by correcting and compensating the strapdown inertial navigation system through a filtering algorithm by using the observation information of the external sensor. Doppler is a speed measuring device widely applied to an underwater and water surface navigation combined system, and has an acoustic navigation positioning system capable of outputting the three-dimensional speed and range capability of a carrier in real time. The method has the advantages of high precision, reliability, real-time performance and the like. However, due to the complex underwater environment, the accumulation of the position error of the dead reckoning system along with time and the long endurance and concealment requirements of the AUV, the precision of the conventional filtering algorithm of the strapdown inertial navigation/Doppler combined system is difficult to meet the requirement of a high-precision navigation positioning function. The conventional H-infinity filtering algorithm does not make any assumption on the statistical characteristics of the system, and has the advantages of simple filtering model and good robustness. However, the conventional H-infinity filter has weak capability of tracking a maneuvering process, and the instantaneity is difficult to guarantee. The invention implements the fading on the past data by introducing the suboptimal fading factor into the H-infinity filtering algorithm to weaken the influence of old data on the current filtering estimation value, namely, the filter is forced to keep the high-precision tracking on the actual system state by adjusting the estimation error variance matrix and the corresponding gain matrix in real time to meet the orthogonality principle, and all effective information in the output residual error is extracted to the maximum extent to obtain the filtering algorithm with high tracking performance. The filtering algorithm has stronger robustness about model uncertainty and the tracking capability of the system maneuvering process, keeps the advantage of simplicity of an H ∞ filtering model, and realizes the tracking of the filter on the system state of the AUV maneuvering process. The invention utilizes the filtering technology to perform data fusion calculation on the strapdown inertial navigation information and the three-dimensional speed information of the Doppler log to obtain a final navigation positioning result with high precision, strong real-time performance and good continuity.

The concrete description is as follows:

(1) geometric model of the earth and physical parameters

Description of the shape of the Earth

WGS-84 reference ellipsoid is used as an earth model in inertial navigation solution, and the equator radius major semi-axis R of the earth model is_e6378137.0m, and 1/298.257223563. Meridian plane radius of curvature R_NCurvature radius R of unitary and quartile ring_ECan be obtained from the following equation:

$\left\{\begin{array}{c}{R}_N=\frac{R_e{(1-e)}^2}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{3}{2}}}\\ R_E=\frac{R_e}{{[{(1-e)}^2{\sin }^{2}L+{\cos }^{2}L]}^{\frac{1}{2}}}\end{array}\right.$

② the rotation angular rate (ω) of the earth_ie)

In an inertial navigation system, the rotational angular velocity of the earth is:

ω_ie=15.0411°/h=7.29211585×10^-5rad/s

③ acceleration of gravity of the earth (g)

The gravitational acceleration of the WGS-84 reference ellipsoid is:

<math><mrow> <mi>g</mi> <mo>=</mo> <mn>9.7803267714</mn> <mo>&times;</mo> <mfrac> <mrow> <mn>1</mn> <mo>+</mo> <mn>0.00193185138639</mn> <msup> <mi>sin</mi> <mn>2</mn> </msup> <mi>L</mi> </mrow> <msqrt> <mn>1</mn> <mo>-</mo> <mn>0.00669437999013</mn> <msup> <mi>sin</mi> <mn>2</mn> </msup> <mi>L</mi> </msqrt> </mfrac> <msup> <mrow> <mo>(</mo> <mn>1</mn> <mo>+</mo> <mfrac> <mi>h</mi> <msub> <mi>R</mi> <mi>e</mi> </msub> </mfrac> <mo>)</mo> </mrow> <mrow> <mo>-</mo> <mn>2</mn> </mrow> </msup> </mrow></math>

wherein L represents the local latitude, h represents the altitude, R_eThe equator radius major axis.

(2) Definition of coordinate system

Earth's center inertial coordinate system (i series)

With ox_iy_iz_iIndicating that the origin is at the center of the earth, ox_iAnd oy_iAxis in the equatorial plane of the earth, ox_iAxis pointing to spring equinox, oz_iAxis coincident with the earth's axis of rotation, oy_iWith ox_i、oz_iAnd forming a right-hand rectangular coordinate system. The orientation of the three coordinate axes in the inertial space is fixed. The output of the strapdown inertial measurement unit takes i as a reference.

② terrestrial coordinate system (e series)

With ox_ey_ez_eIndicating that the origin is at the center of the earth, oz_eAxis coincident with the earth's axis of rotation, ox_eAxis along the intersection of the Greenwich meridian plane and the equatorial plane of the Earth, oy_eAxis in equatorial plane, ox_e、oy_e、oz_eThe three axes form a right-hand rectangular coordinate system. The earth coordinate system is fixedly connected with the earth, and the angular rate of the e system rotating relative to the i system is the rotational angular speed omega of the earth_ie。

③ geographical coordinate system (g series)

With ox_gy_gz_gMeaning that the origin is at the center of gravity of the carrier, typically using the northeast sky coordinate system as the geographic coordinate system, ox_gThe axis points to east (E), oy_gThe axis pointing to north (N), oz_gThe axis points to the day (U).

Navigation coordinate system (n series)

With ox_ny_nz_nIndicating that the northeast geographic coordinate system (ENU) is used as the navigation coordinate system.

Vehicle coordinate system (b series)

With ox_by_bz_bIndicating that the origin is generally taken as the geometric center, ox, of the strapdown inertial measurement unit_bAxis to the right along the transverse axis of the carrier, oy_bAxis forward along the longitudinal axis of the carrier, oz_bThe axis is vertical to the carrier axis.

(3) Conversion relation between attitude angle and attitude matrix

Defining of attitude angle

Course angle: longitudinal axis of the carrier oy_bThe angle between the projection on the horizontal plane and the north of the geographic meridian, called the heading angle, is psi. The heading angle value is calculated clockwise by taking the geographic north direction as a starting point, and the definition range of the heading angle value is 0-360 degrees.

Pitch angle: the carrier being wound about a transverse axis ox_bThe angle between the longitudinal axis of the carrier and the horizontal plane when rotating, called pitch angle, is denoted as θ. The pitch angle is positive when the pitch angle is up and negative when the pitch angle is down from the horizontal plane, and the range of the pitch angle is-90 degrees to +90 degrees.

Roll angle: the included angle between the longitudinal symmetric plane of the carrier and the longitudinal plumb plane is called the roll angle and is marked as gamma. The roll angle is positive when the right inclination is positive and negative when the left inclination is negative from the vertical plane, and the definition range is-180 DEG to +180 deg.

Matrix of the posture 2

The transformation from the carrier coordinate system b to the geographic coordinate system (navigation system n system) can be realized by three rotations in the following sequenceThe specific sequence is as follows: around-oz_gAngle phi of axis rotation, axis ox₁Angle of rotation of axis theta, rewind of axis oy₂The shaft rotates by an angle gamma. The conversion relationship between them is shown as follows.

<math><mrow> <mi>o</mi> <msub> <mi>x</mi> <mi>g</mi> </msub> <msub> <mi>y</mi> <mi>g</mi> </msub> <msub> <mi>z</mi> <mi>g</mi> </msub> <munderover> <mo>&RightArrow;</mo> <mi>y</mi> <mrow> <mo>-</mo> <mi>o</mi> <msub> <mi>z</mi> <mi>g</mi> </msub> </mrow> </munderover> <mi>o</mi> <msub> <mi>x</mi> <mn>1</mn> </msub> <msub> <mi>y</mi> <mn>1</mn> </msub> <msub> <mi>z</mi> <mn>1</mn> </msub> <munderover> <mo>&RightArrow;</mo> <mi>q</mi> <mrow> <mi>o</mi> <msub> <mi>x</mi> <mn>1</mn> </msub> </mrow> </munderover> <mi>o</mi> <msub> <mi>x</mi> <mn>2</mn> </msub> <msub> <mi>y</mi> <mn>2</mn> </msub> <msub> <mi>z</mi> <mn>2</mn> </msub> <munderover> <mo>&RightArrow;</mo> <mi>g</mi> <mrow> <mi>o</mi> <msub> <mi>y</mi> <mn>2</mn> </msub> </mrow> </munderover> <mi>o</mi> <msub> <mi>x</mi> <mi>b</mi> </msub> <msub> <mi>y</mi> <mi>b</mi> </msub> <msub> <mi>z</mi> <mi>b</mi> </msub> </mrow></math>

The transformation matrices corresponding to the three rotations are respectively

<math><mrow> <msub> <mi>C</mi> <mi>&psi;</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>cos</mi> <mi>&psi;</mi> </mtd> <mtd> <mo>-</mo> <mi>sin</mi> <mi>&psi;</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mi>sin</mi> <mi>&psi;</mi> </mtd> <mtd> <mi>cos</mi> <mi>&psi;</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

<math><mrow> <msub> <mi>C</mi> <mi>&theta;</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mi>cos</mi> <mi>&theta;</mi> </mtd> <mtd> <mi>sin</mi> <mi>&theta;</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <mi>sin</mi> </mtd> <mtd> <mi>cos</mi> <mi>&theta;</mi> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

<math><mrow> <msub> <mi>C</mi> <mi>&gamma;</mi> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>cos</mi> <mi>&gamma;</mi> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <mi>sin</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mi>sin</mi> <mi>&gamma;</mi> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mi>cos</mi> <mi>&gamma;</mi> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

Therefore, the transformation matrix from the geographic coordinate system to the carrier coordinate system is:

<math><mrow> <msubsup> <mi>C</mi> <mi>g</mi> <mi>b</mi> </msubsup> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>cos</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&psi;</mi> <mo>+</mo> <mi>sin</mi> <mi></mi> <mi>&gamma;</mi> <mi>sin</mi> <mi></mi> <mi>&psi;</mi> <mi>sin</mi> <mi>&theta;</mi> </mtd> <mtd> <mo>-</mo> <mi>cos</mi> <mi></mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&psi;</mi> <mo>+</mo> <mi>sin</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi></mi> <mi>&psi;</mi> <mi>sin</mi> <mi>&theta;</mi> </mtd> <mtd> <mo>-</mo> <mi>sin</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&theta;</mi> </mtd> </mtr> <mtr> <mtd> <mi>sin</mi> <mi></mi> <mi>&psi;</mi> <mi>cos</mi> <mi>&theta;</mi> </mtd> <mtd> <mi>cos</mi> <mi></mi> <mi>&psi;</mi> <mi>cos</mi> <mi>&theta;</mi> </mtd> <mtd> <mi>sin</mi> <mi>&theta;</mi> </mtd> </mtr> <mtr> <mtd> <mi>sin</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&psi;</mi> <mo>-</mo> <mi>cos</mi> <mi></mi> <mi>&gamma;</mi> <mi>sin</mi> <mi></mi> <mi>&psi;</mi> <mi>sin</mi> <mi>&theta;</mi> </mtd> <mtd> <mo>-</mo> <mi>sin</mi> <mi></mi> <mi>&gamma;</mi> <mi>sin</mi> <mi>&psi;</mi> <mo>-</mo> <mi>cos</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi></mi> <mi>&psi;</mi> <mi>sin</mi> <mi>&theta;</mi> </mtd> <mtd> <mi>cos</mi> <mi></mi> <mi>&gamma;</mi> <mi>cos</mi> <mi>&theta;</mi> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

the geographic coordinate system is used as the navigation coordinate system, and the strapdown inertial navigation system attitude matrix is

$C_b^n={\left(C_g^b\right)}^T$

(iii) conversion relationship between attitude angle and attitude matrix

<math><mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>=</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>g</mi> <mi>b</mi> </msubsup> <mo>)</mo> </mrow> <mi>T</mi> </msup> <mo>=</mo> <msub> <mrow> <mo>[</mo> <msub> <mi>T</mi> <mi>ij</mi> </msub> <mo>]</mo> </mrow> <mrow> <mn>3</mn> <mo>&times;</mo> <mn>3</mn> </mrow> </msub> </mrow></math>

The conversion relationship between the attitude matrix and the attitude angle of the main value interval can be solved as follows:

(4) basic principle of strapdown inertial navigation system

Differential equation of strapdown inertial navigation system

The attitude matrix differential equation is:

wherein, is the angular velocity of the carrier, is measured by a gyroscope,and has the following components:

<math><mrow> <msubsup> <mi>&omega;</mi> <mi>ie</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&omega;</mi> <mi>ie</mi> </msub> <mi>cos</mi> <mi>L</mi> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&omega;</mi> <mi>ie</mi> </msub> <mi>sin</mi> <mi>L</mi> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msubsup> <mi>&omega;</mi> <mi>en</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mo>-</mo> <mfrac> <msubsup> <mi>V</mi> <mi>N</mi> <mi>n</mi> </msubsup> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <msubsup> <mi>V</mi> <mi>E</mi> <mi>n</mi> </msubsup> <mrow> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> </mtd> </mtr> <mtr> <mtd> <mfrac> <msubsup> <mi>V</mi> <mi>E</mi> <mi>n</mi> </msubsup> <mrow> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>tan</mi> <mi>L</mi> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

in the formula, $V^n={\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]}^T$ the speed of the carrier to the ground is the size of the navigation system.

The specific force equation of the carrier in the navigation system is as follows: <math><mrow> <msup> <mover> <mi>V</mi> <mo>&CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <msup> <mi>f</mi> <mi>n</mi> </msup> <mo>-</mo> <mrow> <mo>(</mo> <msubsup> <mrow> <mn>2</mn> <mi>&omega;</mi> </mrow> <mi>ie</mi> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&omega;</mi> <mi>en</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>+</mo> <msup> <mi>g</mi> <mi>n</mi> </msup> </mrow></math>

in the formulaf^bSpecific force, g, measured for an accelerometerⁿ=[0 0 -g]^T。

Position update differential equation: in a system with a geographic coordinate system as a navigation coordinate system, the differential equations for latitude L, longitude λ, and altitude h are:

<math><mrow> <mover> <mi>L</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>N</mi> <mi>n</mi> </msubsup> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>,</mo> <mover> <mi>&lambda;</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>E</mi> <mi>n</mi> </msubsup> <mrow> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mi>cos</mi> <mi>L</mi> </mrow> </mfrac> <mo>,</mo> <mover> <mi>h</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <msubsup> <mi>V</mi> <mi>U</mi> <mi>n</mi> </msubsup> </mrow></math>

error model of strapdown inertial navigation system

Gyroscope error model: epsilon_i=ε_bi+ε_wi(i=x，y，z)，ε_biIs a random constant, epsilon_wiIs white noise;

an accelerometer error model:(i=x，y，z)，is a random constant and is a function of time,is white noise;

the attitude error equation is: <math><mrow> <msup> <mover> <mi>&phi;</mi> <mo>&CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <mo>-</mo> <msubsup> <mi>&omega;</mi> <mi>in</mi> <mi>n</mi> </msubsup> <mo>&times;</mo> <msup> <mi>&phi;</mi> <mi>n</mi> </msup> <mo>+</mo> <mi>&delta;</mi> <msubsup> <mi>&omega;</mi> <mi>ie</mi> <mi>n</mi> </msubsup> <mo>+</mo> <mi>&delta;</mi> <msubsup> <mi>&omega;</mi> <mi>en</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msup> <mi>&epsiv;</mi> <mi>b</mi> </msup> <mo>,</mo> <msup> <mi>&phi;</mi> <mi>n</mi> </msup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>&phi;</mi> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mi>&phi;</mi> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mi>&phi;</mi> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow></math> as attitude error,. epsilon^b=[ε_x ε_y ε_z]^TIs the gyroscope error.

The velocity error equation is:

<math><mrow> <mi>&delta;</mi> <msup> <mover> <mi>V</mi> <mo>&CenterDot;</mo> </mover> <mi>n</mi> </msup> <mo>=</mo> <mo>-</mo> <mi>&phi;</mi> <mo>&times;</mo> <msup> <mi>f</mi> <mi>n</mi> </msup> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&delta;&omega;</mi> <mi>ie</mi> <mi>n</mi> </msubsup> <mo>+</mo> <mi>&delta;</mi> <msubsup> <mi>&omega;</mi> <mi>en</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>-</mo> <mrow> <mo>(</mo> <mn>2</mn> <msubsup> <mi>&omega;</mi> <mi>ie</mi> <mi>n</mi> </msubsup> <mo>+</mo> <msubsup> <mi>&omega;</mi> <mi>en</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mo>&times;</mo> <msup> <mi>&delta;V</mi> <mi>n</mi> </msup> <mo>+</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <msup> <mo>&dtri;</mo> <mi>b</mi> </msup> <mo>,</mo> </mrow></math>

<math><mrow> <mi>&delta;</mi> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>&delta;</mi> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> </mtd> <mtd> <mi>&delta;</mi> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> </mtd> <mtd> <mi>&epsiv;</mi> <msubsup> <mi>V</mi> <mi>u</mi> <mi>n</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow></math> in order to be able to determine the speed error, <math><mrow> <msup> <mo>&dtri;</mo> <mi>b</mi> </msup> <mo>=</mo> <msup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mo>&dtri;</mo> <mi>x</mi> </msub> </mtd> <mtd> <msub> <mo>&dtri;</mo> <mi>y</mi> </msub> </mtd> <mtd> <msub> <mo>&dtri;</mo> <mi>z</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> </mrow></math> is the accelerometer error.

The position error equation is:

<math><mrow> <mi>&delta;</mi> <mover> <mi>L</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <mi>&delta;</mi> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> </mrow> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mo>-</mo> <mfrac> <msubsup> <mi>V</mi> <mi>N</mi> <mi>n</mi> </msubsup> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mfrac> <mi>&delta;h</mi> </mrow></math>

<math><mrow> <mi>&delta;</mi> <mover> <mi>&lambda;</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mfrac> <mrow> <mi>sec</mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>&delta;</mi> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>+</mo> <mfrac> <mrow> <msubsup> <mi>V</mi> <mi>E</mi> <mi>n</mi> </msubsup> <mi>sec</mi> <mi>L</mi> <mi>tan</mi> <mi>L</mi> </mrow> <mrow> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> </mfrac> <mi>&delta;L</mi> <mo>-</mo> <mfrac> <mrow> <msubsup> <mi>V</mi> <mi>E</mi> <mi>n</mi> </msubsup> <mi>sec</mi> <mi>L</mi> </mrow> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>E</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mfrac> <mi>&delta;h</mi> </mrow></math>

<math><mrow> <mi>&delta;</mi> <mover> <mi>h</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>&delta;</mi> <msubsup> <mi>V</mi> <mi>u</mi> <mi>n</mi> </msubsup> </mrow></math>

(5) filtering algorithm

Introduction of H infinity filtering algorithm

Consider the following Krein-based spatial discrete state space system model:

<math><mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>X</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>W</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>Z</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>y</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>X</mi> <mi>k</mi> </msub> <mo>+</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced></math>

X_kis t_kA state vector of a system to be estimated at a moment;

Φ_k，k-1is t_k-1Time to t_kA one-step transition matrix of time;

Γ_k-1as system noiseA drive array;

Z_kis a measurement state vector;

H_kis a measuring array;

W_k-1exciting a noise sequence for the system;

V_kto measure the noise sequence;

X_k，k-1predicting the system state in one step;

P_kan estimation error variance matrix is obtained;

K_kis a filter gain matrix;

y_kis a given system state X_kLinear combination observations of (2).

Wherein, W_kAnd V_kIs I₂Energy-bounded noise, i.e.No assumptions are made about its statistical properties. Let the initial state of the system be X₀，Represents the initial state X of the system₀Defining an initial estimation error variance matrix as:

order toExpressed at a given observation value Z_kFor y under the condition_kDefining the filtering error as follows $e_k={\hat{y}}_k-L_k{X}_k.$

Let T_k(F_f) Indicating unknown interferenceMapping to Filter error e_kThe transfer function of. Sub-optimal H ∞ estimation problem description: given a positive number gamma>0, finding a suboptimal H-infinity estimate ${\hat{y}}_k=F_f(Z_0,Z_1,...,Z_k),$ So that | | T_k(F_f)||_∞<Gamma, i.e. satisfy

<math><mrow> <munder> <mi>inf</mi> <msub> <mi>F</mi> <mi>f</mi> </msub> </munder> <munder> <mi>sup</mi> <mrow> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>,</mo> <mi>W</mi> <mo>,</mo> <mi>V</mi> <mo>&Element;</mo> <msub> <mi>h</mi> <mn>2</mn> </msub> </mrow> </munder> <mfrac> <msubsup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>e</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> <mn>2</mn> </msubsup> <mrow> <msubsup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>X</mi> <mn>0</mn> </msub> <mo>-</mo> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mn>0</mn> </msub> <mo>|</mo> <mo>|</mo> </mrow> <msubsup> <mi>P</mi> <mn>0</mn> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>W</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mrow> <mo>|</mo> <mo>|</mo> <msub> <mi>V</mi> <mi>k</mi> </msub> <mo>|</mo> <mo>|</mo> </mrow> <mn>2</mn> <mn>2</mn> </msubsup> </mrow> </mfrac> <mo><</mo> <msup> <mi>&gamma;</mi> <mn>2</mn> </msup> </mrow></math>

For a given gamma>0, if [ phi ]_k，k-1，Γ_k，k-1]Is full rank, the condition | | | T is satisfied_k(F_f)||_∞Gamma filters exist, if and only if for all k, there is

<math><mrow> <msubsup> <mi>P</mi> <mi>k</mi> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mo>+</mo> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> <mo>-</mo> <msup> <mi>&gamma;</mi> <mrow> <mo>-</mo> <mn>2</mn> </mrow> </msup> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>L</mi> <mi>k</mi> </msub> <mo>></mo> <mn>0</mn> </mrow></math>

Wherein, P_kSatisfy the following recursive Riccati equation

<math><mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>.</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> </mrow></math>

<math><mrow> <msub> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <mi>I</mi> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mo>-</mo> <msup> <mi>&gamma;</mi> <mn>2</mn> </msup> <mi>I</mi> </mtd> </mtr> </mtable> </mfenced> <mo>+</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mi>k</mi> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

If the above inequality holds, the H ∞ filtering recursion algorithm can be obtained as follows:

${\hat{y}}_k=L_k{\hat{X}}_k$

<math><mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow></math>

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

suboptimal H-infinity filtering algorithm at P_kGiven a positive number gamma, through R_e，kFurther adjust P_kThe robustness of the filtering algorithm is better. As long as the initial value is givenAnd P₀According to the measurement of time k_kThe state estimate up to time k can be deduced ${\hat{X}}_k(k=\mathrm{1,2,3},...).$

Improved algorithm

The filter state estimate has the following general structure:

in the formula <math><mrow> <mfenced open='' close=''> <mtable> <mtr> <mtd> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>&gamma;</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow></math> γ_kA residual vector representing time k;

on-line selection of an appropriate time-varying gain matrix K_kThe following conditions, namely, the orthogonality principle, are satisfied simultaneously:

$E\left[(X_k-{\hat{X}}_k){(X_k-{\hat{X}}_k)}^T\right]=\min$

<math><mrow> <mi>E</mi> <mo>[</mo> <msub> <mi>&gamma;</mi> <mrow> <mi>k</mi> <mo>+</mo> <mi>j</mi> </mrow> </msub> <msubsup> <mi>&gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>]</mo> <mo>=</mo> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>=</mo> <mn>0,1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> <mo>,</mo> <mi>j</mi> <mo>=</mo> <mn>1,2</mn> <mo>,</mo> <mo>.</mo> <mo>.</mo> <mo>.</mo> </mrow></math>

when the state estimation value of the filter deviates from the system state due to the influence of factors such as model uncertainty, the state estimation value will inevitably show on the mean value and amplitude of the output residual sequence, and at this time, if the gain array K is adjusted on line_kSo that the residual sequences remain mutually orthogonal, the filter can be forced to keep track of the actual system state.

Introducing a suboptimal fading factor into an H-infinity filtering algorithm to implement fading on past data so as to weaken the influence of old data on the current filtering estimation value, and adjusting an estimation error variance matrix P in real time_kAnd a corresponding gain matrix K_kThe output residual error is forced to be approximate to Gaussian white noise, all effective information in the output residual error can be extracted to the maximum extent, and the filtering algorithm with strong tracking performance is obtained.

Will P_kWritten as follows:

<math><mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>&Lambda;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow></math>

wherein: Λ (k) = diag [ λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k)≥1；i=1，2, …, n. In the sub-optimization method, λ is roughly determined based on a priori knowledge of the system₁(k)：λ₂(k)：…：λ_n(k) The initial values of (a) are: lambda [ alpha ]₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_nIn the formula, α_iIs not less than 1(i =1, 2, …, n), then λ is obtained_i(k) The approximation algorithm of (d) is as follows:

<math><mrow> <msub> <mi>&lambda;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&le;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

wherein,

<math><mrow> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tr</mi> <mo>[</mo> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>]</mo> </mrow> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>M</mi> <mi>ii</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow></math>

<math><mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&Gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>-</mo> <mi>&beta;</mi> <msub> <mi>I</mi> <mi>k</mi> </msub> </mrow></math>

<math><mrow> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow></math>

tr (A) represents a trace-finding operation for an arbitrary matrix A, wherein V is_0，kIs calculated by the following formula:

<math><mrow> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&gamma;</mi> <mn>1</mn> </msub> <msubsup> <mi>&gamma;</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>&rho;V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>&rho;</mi> </mrow> </mfrac> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

in the above equation, residual errorRho is more than or equal to 0.95 and less than or equal to 1, which is taken as a forgetting factor, and rho is generally 0.95; beta.gtoreq.1 is a weakening factor, and the purpose of introducing the weakening factor is to make the state estimation value smoother.

The method is based on an H-infinity filtering algorithm, and the orthogonality principle is satisfied by adjusting an estimation error variance matrix and a corresponding gain matrix in real time, so that a filter is forced to keep high-precision tracking of the actual system state, and a filtering algorithm with high tracking performance is obtained. The filtering algorithm has stronger robustness about model uncertainty and the tracking capability of the system maneuvering process, keeps the advantage of simplicity of an H-infinity filtering model, and realizes the high-precision tracking of the filter on the system state of the carrier maneuvering process.

The filtering algorithm is applied to the AUV integrated navigation system, and data fusion is carried out on the navigation information of the inertial strapdown navigation system and the three-dimensional speed information of the Doppler log, so that a navigation positioning result with high precision, strong real-time performance and good continuity is obtained, and the navigation positioning performance of the integrated system is effectively improved.

Compared with the prior art, the invention has the beneficial effects that: the method can integrate information of a plurality of sub navigation sensors, and has complementary advantages to obtain navigation information with higher precision and more reliability.

Drawings

FIG. 1 is a main block diagram of a strapdown inertial guidance and Doppler log-based navigation system in the strapdown inertial guidance and Doppler log-based navigation method of the present invention;

FIG. 2 is a block diagram of a combined model of the navigation method based on strapdown inertial guidance and a Doppler log according to the present invention;

FIG. 3 is a structural schematic diagram of a navigation method based on strapdown inertial guidance and a Doppler log according to the present invention;

FIG. 4 is a flow chart of a filtering fusion calculation algorithm in the navigation method based on strapdown inertial guidance and a Doppler log according to the present invention;

FIG. 5 is a general flow chart of the strapdown inertial navigation and Doppler system in the strapdown inertial guidance and Doppler log based navigation method of the present invention;

fig. 6 is a simulation analysis effect diagram of the navigation method based on strapdown inertial guidance and doppler log in embodiment 1 of the present invention.

Detailed Description

The technical solution of the present invention is described in detail below, but the scope of the present invention is not limited to the embodiments.

Example (b):

in this embodiment, the navigation method is applied to a navigation system based on a strapdown inertial guidance and doppler log, and the navigation system includes a doppler log, a strapdown inertial measurement unit processing module, and a central processing unit; the Doppler log comprises a transceiver, four transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit is connected with the processing module of the strapdown inertial measurement unit; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises the following steps:

(1) measuring triaxial angular velocity information through a triaxial gyroscope in the strapdown inertial measurement unit assembly and triaxial acceleration information through an accelerometer, receiving navigation information output by the strapdown inertial measurement unit through a processing module of the strapdown inertial measurement unit, and obtaining navigation information such as carrier position, velocity and attitude through navigation integral calculation;

(2) transmitting an electric oscillation signal to the transducer through a transceiver of the Doppler log;

(3) transmitting ultrasonic waves and receiving reflected echoes with frequency shift characteristics through four transducers of a Doppler log;

(4) the receiving system in the transceiver is used for amplifying the echo signal sent by the transducer, obtaining Doppler frequency shift, converting the echo signal into an analog signal of navigational speed and sending the analog signal to the interface unit. If the carrier navigation speed is V, the beam launching depression angle is theta, and the sound velocity C is approximately equal to 1500 m/s, the single-beam frequency shift calculation formula is as follows: Δ f 2Vf₀cos θ/C, the velocity calculation from which is derived is: v ═ Δ fC/(2 f)₀cosθ)；

(5) Converting the four navigational speed analog signals sent by the transceiver into navigational speed by using an interface unit in the transceiver, and outputting the navigational speed analog signals to the central processing unit in a digital mode;

(6) an arithmetic unit in the central processing unit periodically calculates the real-time three-dimensional carrier system speed of the carrier according to the speed information output by the interface unit of the Doppler log;

(7) a filtering module in the central processing unit carries out filtering fusion calculation on the received strapdown inertial navigation information and the three-dimensional speed information of the Doppler log to obtain the correction value of the inertial strapdown measurement system at the moment kAfter correction, the final high-precision navigation positioning information is obtained;

the method for obtaining the correction value of the inertia strapdown measurement system through filtering fusion calculation comprises the following steps:

taking strapdown inertial navigation as a reference system, and taking a speed error by a state variable XAttitude angle error (phi)_e，φ_n，φ_u) Accelerometer random constant biasAnd gyro random constant drift (epsilon)_x，ε_y，ε_z) And 10 dimensions in total: <math><mrow> <mi>X</mi> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msubsup> <mi>&delta;V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msubsup> <mi>&delta;V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>e</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>n</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>u</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>x</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>x</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>z</mi> </msub> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow></math> W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^Tis a system noise vector;

the difference between the velocity solved by the strapdown inertial navigation and the velocity of the carrier system measured by the Doppler log after being converted into the navigation system is used as the measurement value of the system, and then the measurement equation is expressed as follows:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

wherein the observation vector isMeasurement noise vector V = [ w = [)_vx，w_vy]^TThe system measurement matrix is H, and a discrete system filter equation can be obtained by discretizing a system state equation and a measurement equation; using initial valuesAnd P₀According to the measurement of time k_kThe state estimation to the k time can be deduced ${\hat{X}}_k(k=\mathrm{1,2,3},...):$

<math><mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow></math>

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein <math><mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>&Lambda;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow></math>

Wherein: Λ (k) = diag [ λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k) Not less than 1; i =1, 2, …, n. In the sub-optimization method, λ is roughly determined based on a priori knowledge of the system₁(k)：λ₂(k)：…：λ_n(k)The initial values of (a) are: lambda [ alpha ]₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_nIn the formula, α_iIs not less than 1(i =1, 2, …, n), then λ is obtained_i(k) The approximation algorithm of (d) is as follows:

<math><mrow> <msub> <mi>&lambda;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&le;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

wherein,

<math><mrow> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tr</mi> <mo>[</mo> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>]</mo> </mrow> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>M</mi> <mi>ii</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow></math>

<math><mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&Gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>-</mo> <mi>&beta;</mi> <msub> <mi>I</mi> <mi>k</mi> </msub> </mrow></math>

<math><mrow> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow></math>

tr (A) represents the trace-finding operation for matrix A, where V is_0，kIs calculated by the following formula:

<math><mrow> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&gamma;</mi> <mn>1</mn> </msub> <msubsup> <mi>&gamma;</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>&rho;V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>&rho;</mi> </mrow> </mfrac> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow></math>

in the above equation, residual errorRho is more than or equal to 0.95 and less than or equal to 1, which is taken as a forgetting factor, and rho is generally 0.95; β ≧ 1 is a selected attenuation factor, typically β = 1.

In the strapdown inertial measurement unit of the embodiment, the triaxial gyroscope is orthogonally installed in triaxial, and the triaxial accelerometer is orthogonally installed in triaxial.

The three-axis gyroscope of the strapdown inertial measurement unit of the embodiment is an optical fiber gyroscope.

The three-axis accelerometer of this embodiment is a silicon micro-accelerometer.

The transceiver of the doppler log of the present embodiment includes a transmitting system, a receiving system, and a calculation compensation circuit.

The navigation system of the embodiment further comprises a display, wherein the display is connected with the arithmetic unit and displays the navigation information obtained by the operation of the central processing unit.

The conventional navigation method (comparative example) based on the H infinity filtering algorithm and the navigation method (embodiment) based on the inertial strapdown navigation and the Doppler log of the embodiment are subjected to simulation analysis on the filtering performance in the AUV integrated navigation system, and the AUV track curve tracking and comparing effect is shown in fig. 6.

As noted above, while the present invention has been shown and described with reference to certain preferred embodiments, it is not to be construed as limited thereto. Various changes in form and detail may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (6)

1. A navigation method based on strapdown inertial guidance and a Doppler log is characterized in that the navigation method is applied to a navigation system based on the strapdown inertial guidance and the Doppler log, and the navigation system comprises the Doppler log, a strapdown inertial measurement unit processing module and a central processing unit; the Doppler log comprises a transceiver, four transducers and an interface unit; the Doppler log is connected with the central processing unit through the interface unit; the strapdown inertial measurement unit comprises a three-axis gyroscope and a three-axis accelerometer; the strapdown inertial measurement unit is connected with the processing module of the strapdown inertial measurement unit; the strapdown inertial measurement unit processing module is connected with the central processing unit; the navigation method comprises the following steps:

(1) measuring triaxial angular velocity information through a triaxial gyroscope and triaxial acceleration information through an accelerometer in the strapdown inertial measurement unit assembly, receiving navigation information output by the strapdown inertial measurement unit through a processing module of the strapdown inertial measurement unit, and obtaining navigation information such as carrier position, velocity and attitude through navigation integral calculation;

(2) transmitting an electric oscillation signal to the transducer through a transceiver of the Doppler log;

(3) transmitting ultrasonic waves and receiving reflected echoes with frequency shift characteristics through four transducers of a Doppler log;

(4) the receiving system in the transceiver is used for amplifying the echo signal sent by the transducer, obtaining Doppler frequency shift, converting the echo signal into an analog signal of navigational speed and sending the analog signal to the interface unit. If the carrier navigation speed is V, the beam launching depression angle is theta, and the sound velocity C is approximately equal to 1500 m/s, the single-beam frequency shift calculation formula is as follows: Δ f 2Vf₀cos θ/C, the velocity calculation from which is derived is: v ═ Δ fC/(2 f)₀cosθ)；

(5) Converting the four navigational speed analog signals sent by the transceiver into navigational speed by using an interface unit in the transceiver, and outputting the navigational speed analog signals to the central processing unit in a digital mode;

(6) an arithmetic unit in the central processing unit periodically calculates the real-time three-dimensional carrier system speed of the carrier according to the speed information output by the interface unit of the Doppler log;

(7) a filtering module in the central processing unit carries out filtering and platform-fusing calculation on the received strapdown inertial navigation information and the three-dimensional speed information of the Doppler log to obtain the correction value of the inertial strapdown measurement system at the moment kAfter correction, the final high-precision navigation positioning information is obtained;

the method for obtaining the correction value of the inertia strapdown measurement system through filtering fusion calculation comprises the following steps:

taking strapdown inertial navigation as a reference system, and taking a speed error by a state variable XAttitude angle error (phi)_e，φ_n，φ_u) Accelerometer random constant biasAnd gyro random constant drift (epsilon)_x，ε_y，ε_z) And 10 dimensions in total: <math> <mrow> <mi>X</mi> <mo>=</mo> <msup> <mrow> <mo>[</mo> <msubsup> <mi>&delta;V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msubsup> <mi>&delta;V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>e</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>n</mi> </msub> <mo>,</mo> <msub> <mi>&phi;</mi> <mi>u</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>x</mi> </msub> <mo>,</mo> <msub> <mo>&dtri;</mo> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>x</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>y</mi> </msub> <mo>,</mo> <msub> <mi>&epsiv;</mi> <mi>z</mi> </msub> <mo>]</mo> </mrow> <mi>T</mi> </msup> <mo>;</mo> </mrow> </math>

W=[w_ax，w_ay，w_gx，w_gy，w_gz，0，0，0，0，0]^Tis a system noise vector;

the difference between the velocity solved by the strapdown inertial navigation and the velocity of the carrier system measured by the Doppler log after being converted into the navigation system is used as the measurement value of the system, and then the measurement equation is expressed as follows:

$Z=\left[\begin{array}{c}{V}_{\mathrm{SINSe}}^n-C_b^n{V}_{\mathrm{DVLe}}\\ V_{\mathrm{SINSn}}^n-C_b^n{V}_{\mathrm{DVLn}}\end{array}\right]=\mathrm{HX}+V$

wherein the observation vector isMeasurement noise vector V = [ w = [)_vx，w_vy]^TThe system measurement matrix is H, and a discrete system filter equation can be obtained by discretizing a system state equation and a measurement equation; using initial valuesAnd P₀According to the measurement of time k_kThe state estimate up to time k can be deduced ${\hat{X}}_k(k=\mathrm{1,2,3},...):$

<math> <mrow> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>K</mi> <mi>k</mi> </msub> <mrow> <mo>(</mo> <msub> <mi>Z</mi> <mi>k</mi> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mover> <mi>X</mi> <mo>^</mo> </mover> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>)</mo> </mrow> </mrow> </math>

$K_k=P_k{H}_k^T{(H_k{P}_k{H}_k^T+I)}^{-1}$

Wherein <math> <mrow> <msub> <mi>P</mi> <mi>k</mi> </msub> <mo>=</mo> <msub> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Gamma;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>+</mo> <mi>&Lambda;</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow> </math>

Wherein: Λ (k) = diag [ λ₁(k)，λ₂(k)，…λ_n(k)]，λ_i(k) Not less than 1; 1, 2, …, n; in the sub-optimization method, λ is roughly determined based on a priori knowledge of the system₁(k)：λ₂(k)：…：λ_n(k) The initial values of (a) are: lambda [ alpha ]₁(k)：λ₂(k)：…：λ_n(k)=α₁：α₂：…：α_nIn the formula, α_i1(i ═ 1, 2, …, n), then λ is obtained_i(k) The approximation algorithm of (d) is as follows:

<math> <mrow> <msub> <mi>&lambda;</mi> <mi>i</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>&le;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

wherein,

<math> <mrow> <msub> <mi>&lambda;</mi> <mrow> <mn>0</mn> <mi>i</mi> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tr</mi> <mo>[</mo> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>]</mo> </mrow> <mrow> <munderover> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </munderover> <msub> <mi>&alpha;</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msub> <mi>M</mi> <mi>ii</mi> </msub> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> </mrow> </mfrac> </mrow> </math>

<math> <mrow> <mi>N</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>-</mo> <msub> <mi>H</mi> <mi>k</mi> </msub> <msub> <mi>&Gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&Gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <mo>-</mo> <mi>&beta;</mi> <msub> <mi>I</mi> <mi>k</mi> </msub> </mrow> </math>

<math> <mrow> <mrow> <mi>M</mi> <mrow> <mo>(</mo> <mi>k</mi> <mo>)</mo> </mrow> <mo>=</mo> <mo>{</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>-</mo> <msub> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>L</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <msubsup> <mi>R</mi> <mrow> <mi>e</mi> <mo>,</mo> <mi>k</mi> </mrow> <mrow> <mo>-</mo> <mn>1</mn> </mrow> </msubsup> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>H</mi> <mi>k</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>L</mi> <mi>k</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <msub> <mi>P</mi> <mrow> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <msubsup> <mi>&Phi;</mi> <mrow> <mi>k</mi> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> <mi>T</mi> </msubsup> <mo>}</mo> </mrow> <msubsup> <mi>H</mi> <mi>k</mi> <mi>T</mi> </msubsup> <msub> <mi>H</mi> <mi>k</mi> </msub> </mrow> </math>

tr (A) represents a trace-finding operation for an arbitrary matrix A, in the formulaV_0，kIs calculated by the following formula:

<math> <mrow> <msub> <mi>V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> </mrow> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>&gamma;</mi> <mn>1</mn> </msub> <msubsup> <mi>&gamma;</mi> <mn>1</mn> <mi>T</mi> </msubsup> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>=</mo> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mfrac> <mrow> <msub> <mi>&rho;V</mi> <mrow> <mn>0</mn> <mo>,</mo> <mi>k</mi> <mo>-</mo> <mn>1</mn> </mrow> </msub> <mo>+</mo> <msub> <mi>&gamma;</mi> <mi>k</mi> </msub> <msubsup> <mi>&gamma;</mi> <mi>k</mi> <mi>T</mi> </msubsup> </mrow> <mrow> <mn>1</mn> <mo>+</mo> <mi>&rho;</mi> </mrow> </mfrac> <mo>,</mo> </mtd> <mtd> <mi>k</mi> <mo>&GreaterEqual;</mo> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

in the above equation, residual errorRho is more than or equal to 0.95 and less than or equal to 1 and is a forgetting factorGenerally, ρ is 0.95; beta.gtoreq.1 is a weakening factor, and generally beta =1 is taken.

2. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the three-axis gyroscope of the strapdown inertial measurement unit is orthogonally mounted in three axes, and the three-axis accelerometer is orthogonally mounted in three axes.

3. The strapdown inertial guidance and doppler velocity log based navigation method of claim 1, wherein the three-axis gyroscope of the strapdown inertial measurement unit is a fiber optic gyroscope.

4. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the three-axis accelerometer is a silicon micro-accelerometer.

5. The strapdown inertial guidance and doppler log based navigation method of claim 1, wherein the transceiver of the doppler log comprises a transmitting system, a receiving system and a computational compensation circuit.

6. The strapdown inertial guidance and doppler log-based navigation method of claim 1, wherein the navigation system further comprises a display, the display is connected to the operator, and the display displays the navigation information obtained by the operation of the cpu.