CN101713666B - Single-shaft rotation-stop scheme-based mooring and drift estimating method - Google Patents

Abstract

The invention provides a method for estimating mooring drift based on a single-axis rotation-stop scheme. The initial position parameters of the carrier are determined by GPS; the data output by the fiber optic gyroscope and the accelerometer are collected and processed; the IMU adopts an indexing scheme in which 8 rotation and stop sequences are one rotation cycle; The output updates the strapdown matrix in real time, and at the same time performs coordinate transformation on the acceleration information collected under the IMU rotation state, and converts it to the coordinate system of the mathematical platform; establishes the separation position loop of the carrier in the mooring state according to the error model of the moving base of the carrier Kalman drift estimation model; the constant value deviation of the inertial device obtained after Kalman filtering is processed by the average filtering method to obtain a relatively stable estimated average value. When the carrier is in the mooring state, the drift estimation method provided by the invention can be used to accurately estimate the constant value deviation of the inertial device according to the Kalman filter technology.

Description

A Mooring Drift Estimation Method Based on Single-Axis Spin-to-Stop Scheme

technical field

The present invention relates to a measurement method, in particular to a method for estimating mooring drift based on a single-axis rotation-stop scheme.

Background technique

The strapdown inertial navigation system (SINS) is a completely autonomous navigation system. It uses gyroscopes and accelerometers to measure the angular motion and linear motion parameters of the carrier relative to the inertial space. Provide position, velocity and attitude information accurately. Since SINS completely relies on its own inertial components and does not rely on any external information to measure navigation parameters, it has the advantages of good concealment, no limitation of weather conditions, and no interference. It is a completely autonomous and all-weather navigation system, which has been widely used. Used in aviation, aerospace, navigation and other fields. According to the basic principle of SINS, the existence of the constant value deviation of the inertial device in the navigation process of SINS is the main factor that makes it difficult to improve the positioning accuracy of the inertial navigation system. How to effectively limit the error divergence of inertial navigation and improve the accuracy of inertial navigation system is a very important topic in the field of inertial navigation.

According to the basic principle of SINS, SINS needs to obtain initial information before entering the navigation state, including initial position, velocity and attitude. The accuracy of the initial attitude is the initial alignment accuracy when the SINS enters the navigation state, so the initial attitude must be determined before the SINS enters the navigation state. Since the "platform" is the benchmark for measuring the specific force, the initial alignment of the "platform" is very important; in a strapdown inertial navigation system, the strapdown matrix acts as a mathematical platform. Therefore, the key issue of initial alignment is how to obtain the initial value of the initial strapdown matrix with higher precision in a shorter time. The inertial device deviation is the main factor leading to the deviation of the initial strapdown matrix, so the importance of drift estimation technology is reflected.

The rotational modulation technique is a self-calibration method of the inertial navigation system. It does not need to introduce external correction information, and can automatically average the constant value deviation of the inertial device in the system to offset the influence of drift on the system accuracy. Therefore, the long-time working precision of the inertial navigation system can be improved, and the "autonomous" advantage of the inertial navigation system can be fully utilized. The application of rotational modulation technology can also use lower-precision inertial devices to form a higher-precision inertial navigation system, which is conducive to reducing the cost of the inertial navigation system. At the same time, the introduction of external motion can effectively improve the observability of some parameters of the inertial navigation system. Spend.

When the ship is in the mooring state, due to the influence of sea wave factors, it produces swaying and swaying motions. The interference acceleration and interference angular velocity generated by these interference motions are sensed by the accelerometer and the gyro respectively, and the interference acceleration is solved by the integral link in the inertial navigation system. There is a large error in the calculated carrier velocity, which is compared with the actual linear velocity of the carrier and used as the observation of the system filtering model. The inaccuracy of observations will cause the system to reduce the convergence speed of parameters and reduce the estimation accuracy of parameters when estimating the system.

Public reports related to the present invention in the CNKI database include: 1. "The Error Modulation Analysis of the Rotating Fiber Optic Strapdown Inertial Navigation System", which proposes a novel fiber optic strapdown inertial navigation system with a continuous stable rotating platform, aiming to improve the system While improving the navigation accuracy, the cost of the inertial navigation system is not increased. The structure and working principle of the inertial navigation system are studied, the error modulation process of the rotating optical fiber strapdown inertial navigation system is analyzed, and finally the whole inertial navigation system is simulated. 2. "Using Rotary Modulation Technology to Realize Fast and High-precision Azimuth Alignment", puts forward the technical indicators for developing the rotary modulation system. However, no modulation method and drift estimation method related to the present invention have been proposed.

Contents of the invention

The purpose of the present invention is to provide a method for estimating mooring drift based on a single-axis rotation-stop scheme that can accurately estimate the constant value deviation of an inertial device when the carrier is in a mooring state.

The object of the present invention is achieved like this: comprise the following steps:

(1) Determine the initial position parameters of the carrier through GPS, and bind them into the navigation computer;

(2) The strapdown inertial navigation system is preheated, and the data output by the fiber optic gyroscope and the quartz accelerometer are collected and processed;

(3) The inertial measurement unit (IMU) adopts an indexing scheme in which 8 rotation-stop sequences are one rotation cycle;

(4) Determine the strapdown matrix T _sp ^p according to the output of the gyroscope in the rotating state, and convert the output of the accelerometer to the coordinate system of the mathematical platform;

(5) According to the inertial navigation system moving base error equation to establish the drift estimation model when the carrier is in mooring state, take the carrier velocity v ₀ provided by GPS as the reference, and take the difference between the settlement velocity of the inertial navigation system as the observation quantity, and use Karl Mann filtering technology realizes the estimation of inertial device deviation;

(6) The constant value deviation of the inertial device estimated by the Kalman filter is smoothed by the average filter method, and the influence of small fluctuations is filtered out.

The present invention may also include:

1. The IMU adopts 8 rotation-stop sequences as one rotation period. The transposition scheme is as follows: sequence 1, the IMU starts from point A and rotates 180° clockwise to reach position C, and T _t when it stops; sequence 2, the IMU starts from point C Start and turn 90° clockwise to reach position D, stop time T _t ; Sequence 3, IMU starts from point D and rotate 180° counterclockwise to reach position B, stop time T _t ; Sequence 4, IMU turns 90° counterclockwise from point B Arrive at position A, stop time T _t ; Sequence 5, IMU rotates 180° counterclockwise from point A to reach position C, stop time T _t ; Sequence 6, IMU rotates 90° counterclockwise from point C to position B, stop time T _t ; Sequence 7, IMU starts from point B and turns 180° clockwise to reach position D, and stops at time T _t ; Sequence 8, IMU starts from point D to turn 90° clockwise to reach position A, stops at time T _t ; IMU follows this The rotation sequence is cyclic; the IMU stop points p3, p8 and p4, p7 on the horizontal east axis are symmetrical to the center of the rotation axis; the stop points p1, p5 and p2, p6 on the north axis are symmetrical to the center of the rotation axis.

2. According to the output of the gyroscope in the rotating state, the strapdown matrix T _sp ^p is determined, and the method of converting the output of the accelerometer to the coordinate system of the mathematical platform is:

According to the fourth-order Runge-Kutta method, the quaternion is used to update the strapdown matrix of the inertial navigation system in real time, and the formula is expressed as:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; [</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; is</span>}}^{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; ]</span>$

Among them, ω _is ^s represents the output of the gyroscope; the acceleration value in the coordinate system of the mathematical platform is calculated by using the conversion relationship between the IMU coordinate system and the coordinate system of the mathematical platform, $f^p=T_{\mathrm{the\; s}}^p{f}^{\mathrm{the\; s}}.$

3. According to the inertial navigation system moving base error equation to establish the drift estimation model when the carrier is in mooring state, the carrier velocity v ₀ provided by GPS is used as the benchmark, and the difference with the inertial navigation system settlement speed is used as the observation quantity, using The method of Kalman filtering technology to realize the estimation of inertial device deviation is as follows:

Using the error model that separates the position information of the carrier as the drift estimation model of the system, the Kalman filter state equation with the speed error as the state variable and the measurement equation for the measurement of the speed error are established;

1) Establish the state equation of the Kalman filter:

The state error of the rotating strapdown inertial navigation system is described by a first-order linear differential equation:

$\stackrel{<span\; class="notranslate">\; \&CenterDot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; W</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>$

Where X(t) is the state vector of the system at time t; A(t) and B(t) are the state matrix and noise matrix of the system respectively; W(t) is the system noise vector;

The state vector of the system is:

The white noise vector of the system is:

W＝[a _x a _y ω _x ω _y ω _z 0 0 0 0 0] ^T

分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；Among them, δV _e and δV _n represent the speed errors in the east direction and north direction respectively;

are the zero bias of the IMU coordinate system ox _s , oy _s axis accelerometer; ε _x , ε _y , ε _z are the constant drift of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively; a _x , a _{y are} respectively is the white noise error of the IMU coordinate system ox _s , oy _s axis accelerometer; ω _x , ω _y , ω _z are the white noise errors of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively;

The state transition matrix of the system is:

$\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="f"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}\\ {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}& {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}\end{array}\right]$

in:

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="f"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}& {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}\\ \frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \frac{\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& <span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>\\ <span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="0"\; label="R\; m"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">R</figure-callout></span><figure-callout\; id="0"\; label="R\; m"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">\; </figure-callout>}}_{\mathrm{<span\; class="notranslate">\; </span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

V _e , V _n represent eastward and northward velocities, respectively; ω _ie represents the angular velocity of the earth's rotation; R _m , R _n represent the earth's meridian and the radius of curvature, respectively; L represents the local latitude; f _e , f _n , f _u respectively Expressed as the ratio of east, north, and sky directions in the navigation coordinate system;

The strapdown matrix T _sp ^p is:

${\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 31</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 32</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 33</span>}}\end{array}\right]$

but

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccccc}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="12"\; label="T"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 31</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 32</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 33</span>}}\end{array}\right]<span\; class="notranslate">\; ;</span>$

2) Establish the measurement equation of the Kalman filter:

The measurement equation of the rotating strapdown inertial navigation system is described by the first-order linear differential equation as follows:

Z(t)=H(t)X(t)+v(t)

Among them: Z(t) represents the measurement vector of the system at time t; H(t) represents the measurement matrix of the system; v(t) represents the measurement noise of the system;

The system measurement matrix is:

$\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccccccccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]$

Quantity measured as horizontal velocity error:

v(t)=[v _e v _n ] ^T .

4. The method of smoothing the constant value deviation of the inertial device estimated by the Kalman filter with the method of average filtering, and filtering out the influence of small amplitude fluctuations is: known equidistant sampling points x ₀ <x ₁ <... The observed data on <x _n-1 <... are y ₀ , y ₁ ,..., y _n-1 , and y _i represents the smooth value of y _i ; the weighting coefficient is set to 1, and the value from 400 seconds to 600 after low-pass filtering is taken The second data is smoothed once, then:

${\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="0"\; label="y"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="1"\; label="y"\; filenames="H2009100732329E00031.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; .</span>$

The invention utilizes the intermittent rotation of the inertial measurement unit (IMU) to improve the observability of some parameters in the inertial navigation system; according to the characteristics of the small variation range of the carrier position in the mooring state, the carrier position circuit is separated from the system by using the Kalman filter technology estimation The constant value drift of the gyroscope in the three directions and the zero bias of the accelerometer in the horizontal direction are displayed. Considering that there are small-scale fluctuations in the estimated constant value deviation of the inertial device under the mooring state of the carrier, the estimated data is smoothed by the method of average filtering.

Compared with the prior art, the present invention has the advantages that: under the mooring state of the ship, it is difficult to apply the drift estimation technology under the navigation system due to the interference of the swaying and swaying motion of the ship, and the rotation of the IMU can be used to effectively Improve the observability of some parameters of the system. At the same time, considering the small change of position information in the mooring state of the ship, the coupling relationship between the position loop and the system is ignored, so that the carrier can achieve better estimation accuracy in the mooring state.

The beneficial effects of the present invention are described as follows:

Under Visual C++ simulation conditions, the simulation experiment of this method is carried out:

Set the constant drift of the gyroscope to 0.01°/h, the zero position deviation of the accelerometer to 10 ^-4 g; the initial alignment error of the system to be 0.1°, 0.1°, 0.5°; Axis and yaw axis do three-axis rocking motion, and its mathematical model is:

$\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&theta;</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}_{\mathrm{<span\; class="notranslate">\; \&theta;</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; \&gamma;</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}_{\mathrm{<span\; class="notranslate">\; \&gamma;</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; \&psi;</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; \&psi;</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; \&psi;</span>}}\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; \&phi;</span>}}_{\mathrm{<span\; class="notranslate">\; \&psi;</span>}}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; k</span>}\end{array}\right.$

Among them: θ, γ, ψ represent the roll angle variables of pitch angle, roll angle and heading angle respectively; _{θ m} , _{γ m} , _{ψ m} represent the corresponding swing angle amplitudes; Indicates the corresponding swing angle frequency; φ _θ , φ _γ , φ _ψ respectively represent the corresponding initial phase; ω _i = 2π/T _i , i = θ, γ, ψ, T _i represents the corresponding swing period, and k is the initial heading horn. During simulation, take: θ _m = 6°, γ _m = 12°, ψ _m = 10°, T _θ = 8s, T _γ = 10s, T _ψ = 6s, k = 0°.

The linear velocity caused by the sway, surge and heave of the carrier is:

为[0，2π]上服从均匀分布的随机相位。In the formula, i=x, y, and z are the east, north, and sky directions of the geographic coordinate system. $A_{{\mathrm{D.}}_x}=0.02m,$ $A_{{\mathrm{D.}}_{\mathrm{the\; y}}}=0.03m,$ $A_{{\mathrm{D.}}_z}=0.3m;$ $T_{{\mathrm{D.}}_x}=7\mathrm{the\; s},$ $T_{{\mathrm{D.}}_{\mathrm{the\; y}}}=6\mathrm{the\; s},$ $T_{{\mathrm{D.}}_z}=8\mathrm{the\; s};$

is a random phase that obeys a uniform distribution on [0, 2π].

The initial position of the carrier: 45.7796° north latitude, 126.6705° east longitude;

Initial attitude error angle: the three initial attitude error angles are all zero;

Equatorial radius: R _e = 6378393.0m;

Ellipsoid: e=3.367e-3;

The gravitational acceleration on the earth's surface obtained from the universal gravitation: g ₀ =9.78049;

Earth rotation angular velocity (rad/s): 7.2921158e-5;

Gyroscope constant value drift: 0.01 degrees/hour;

Gyroscope random walk: 0.001 degrees/

Accelerometer zero bias: 10 ^-4 g ₀ ;

Accelerometer noise: 10 ^-6 g ₀ ;

Constant: π = 3.1415926.

Description of drawings

Fig. 1 is a mooring drift estimation method based on a single-axis rotation-stop scheme of the present invention;

Figure 2 is the IMU single-axis four-position rotation stop of the present invention;

Fig. 3 is the estimated gyroscope constant value drift of the present invention;

FIG. 4 is the estimated horizontal accelerometer bias of the present invention.

Detailed ways

The present invention is described in more detail below in conjunction with accompanying drawing example:

(1) Determine the initial position parameters of the carrier through GPS, and bind them into the navigation computer;

(2) The strapdown inertial navigation system is preheated, and the data output by the fiber optic gyroscope and the quartz accelerometer are collected and processed;

(3) The IMU adopts an indexing scheme with 8 rotation-stop sequences as one rotation cycle (as shown in Figure 2);

Sequence 1, IMU starts from point A and rotates 180° clockwise to position C, stop time T _t ; Sequence 2, IMU starts from point C to rotate 90° clockwise to position D, stop time T _t ; Sequence 3, IMU starts from D Start from point B and turn 180° counterclockwise to reach position B, stop time T _t ; Sequence 4, IMU start from point B and turn 90° counterclockwise to reach position A, stop time T _t ; Sequence 5, IMU start from point A and turn counterclockwise 180° ° Arrive at position C, stop time T _t ; Sequence 6, IMU starts from point C and rotates 90° counterclockwise to reach position B, stops at time T _t ; Sequence 7, IMU starts from point B and rotates 180° clockwise to reach position D, stops Time T _t ; Sequence 8, the IMU starts from point D and rotates 90° clockwise to reach position A, and stops at time T _t ; the IMU cycles through this rotation sequence. In order to effectively average the positive and negative inertial device deviations in the horizontal direction at symmetrical positions, define the IMU pause points p3, p8 and p4, p7 on the horizontal east axis to be symmetrical to the center of the rotation axis; the pause points p1, p7 on the north axis p5, p2 and p6 are symmetrical to the center of the rotating shaft. The four-position turn-stop scheme is still carried out at intervals of 180° or 90° of rotation angle.

(4) Determine the strapdown matrix T _sp ^p according to the output of the gyroscope in the rotating state, and convert the output of the accelerometer to the coordinate system of the mathematical platform;

According to the fourth-order Runge-Kutta method, the quaternion is used to update the strapdown matrix of the inertial navigation system in real time, and the formula is expressed as:

${\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; T</span>}}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; [</span><span\; class="notranslate">\; \&times;</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; is</span>}}^{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; ]</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

where ω _is ^s represents the gyroscope output. Using the conversion relationship between the IMU coordinate system and the mathematical platform coordinate system, the acceleration value in the mathematical platform coordinate system is calculated.

${\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}{\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; the\; s</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

(5) According to the error equation of the inertial navigation system's moving base, the drift estimation model of the vehicle in the mooring state is established, and the vehicle velocity v ₀ provided by GPS is used as the reference, and the difference with the settlement velocity of the inertial navigation system is used as the observation. Using Kalman filter technology to realize the estimation of inertial device deviation;

Since the rotation of the IMU can effectively improve the observability of some parameters in the inertial navigation system, and considering that the position of the carrier in the mooring state has a small change range, the error model that separates the position information of the carrier can be used as the drift estimation model of the system . Establish the Kalman filter state equation with the speed error as the state variable and the measurement equation for the measurement of the speed error;

1) Establish the state equation of the Kalman filter:

The state error of the rotating strapdown inertial navigation system is described by a first-order linear differential equation:

$\stackrel{<span\; class="notranslate">\; \&Center\; Dot;</span>}{\mathrm{<span\; class="notranslate">\; x</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\mathrm{<span\; class="notranslate">\; W</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

Where X(t) is the state vector of the system at time t; A(t) and B(t) are the state matrix and noise matrix of the system respectively; W(t) is the system noise vector;

The state vector of the system is:

The white noise vector of the system is:

W＝[a _x a _y ω _x ω _y ω _z 0 0 0 0 0] ^T (5)

分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；ω_x、ω_y、ω_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的白噪声误差；Among them, δV _e and δV _n represent the speed errors in the east direction and north direction respectively;

are the zero bias of the IMU coordinate system ox _s , oy _s axis accelerometer; ε _x , ε _y , ε _z are the constant drift of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively; a _x , a _{y are} respectively is the white noise error of the IMU coordinate system ox _s , oy _s axis accelerometer; ω _x , ω _y , ω _z are the white noise errors of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively;

The state transition matrix of the system is:

$\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="f"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}\\ {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}& {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

in:

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="f"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}& {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 2</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="f"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}& {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; u</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 9</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{cc}\mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}\\ \frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \frac{\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& <span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>\\ <span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\mathrm{<span\; class="notranslate">\; the\; tan</span>}\mathrm{<span\; class="notranslate">\; L</span>}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}\\ {\mathrm{<span\; class="notranslate">\; \&omega;</span>}}_{\mathrm{<span\; class="notranslate">\; ie</span>}}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; L</span>}<span\; class="notranslate">\; +</span>\frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}& \frac{{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}}{{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; m</span>}}}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

V _e , V _n represent eastward and northward velocities, respectively; ω _ie represents the angular velocity of the earth's rotation; R _m , R _n represent the earth's meridian and the radius of curvature, respectively; L represents the local latitude; f _e , f _n , f _u respectively It is expressed as the ratio of east, north, and sky directions in the navigation coordinate system.

Let the strapdown matrix T _sp ^p be:

${\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; the\; s</span>}}^{\mathrm{<span\; class="notranslate">\; p</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 31</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 32</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 33</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 12</span>}<span\; class="notranslate">\; )</span>$

but

${\mathrm{<span\; class="notranslate">\; <figure-callout\; id="5"\; label="T"\; filenames="H2009100732329E00021.png,H2009100732329E00022.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 5</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 5</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{ccccc}{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& {\mathrm{<span\; class="notranslate">\; <figure-callout\; id="12"\; label="T"\; filenames="H2009100732329E00022.png,H2009100732329E00031.png"\; state="\{\{state\}\}">T</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& {\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 11</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 12</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 13</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; one</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; two</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; twenty\; three</span>}}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 31</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 32</span>}}& <span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; T</span>}}_{\mathrm{<span\; class="notranslate">\; 33</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

2) Establish the measurement equation of the Kalman filter:

The measurement equation of the rotating strapdown inertial navigation system is described by the first-order linear differential equation as follows:

Z(t)＝H(t)X(t)+v(t) (14)

Among them: Z(t) represents the measurement vector of the system at time t; H(t) represents the measurement matrix of the system; v(t) represents the measurement noise of the system;

The system measurement matrix is:

$\mathrm{<span\; class="notranslate">\; h</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cccccccccc}\mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\\ \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 1</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

Quantity measured as horizontal velocity error:

v(t)=[v _e v _n ] ^T (16)

(6) The constant value deviation of the inertial device estimated by the Kalman filter is smoothed by the method of average filtering, and the influence of small fluctuations is filtered out;

Considering that there are small-scale fluctuations in the estimated constant value deviation of the inertial device under the mooring state of the carrier, such a signal is not conducive to the correction of the constant value deviation of the inertial device, so the weighted average filtering algorithm is used to analyze the The constant value deviation information is de-noised again. This method is the simplest signal processing method, and the algorithm formula is as follows: Let the observed data on the known equidistant sampling point x ₀ <x ₁ <...<x _n-1 <... be y ₀ , y ₁ ,... , y _n-1 , y _i represents the smooth value of y _i . We take the weighting coefficient as 1, and take the data from 400 seconds to 600 seconds after low-pass filtering to do a smoothing, then:

${\stackrel{<span\; class="notranslate">\; \&OverBar;</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}_{\mathrm{<span\; class="notranslate">\; i</span>}}<span\; class="notranslate">\; =</span>\frac{{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; \&CenterDot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="0"\; label="y"\; filenames="H2009100732329E00022.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; <figure-callout\; id="1"\; label="y"\; filenames="H2009100732329E00031.png"\; state="\{\{state\}\}">y</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; +</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; \&Center\; Dot;</span><span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; i</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; no</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}}{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 17</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; .</span>$

Claims (3)

1. mooring and drift estimating method based on single-shaft rotation-stop scheme is characterized in that may further comprise the steps:

(1) determines the initial position parameters of carrier by GPS, and bind to navigational computer;

(2) strapdown inertial navitation system (SINS) is carried out preheating and is prepared, and gathers the data of fibre optic gyroscope and quartz accelerometer output and data are handled;

(3) IMU adopts 8 commentaries on classics to stop the transposition scheme that order is a swing circle;

(4) determine the strapdown matrix according to gyrostatic output under the rotation status

And the output of degree of will speed up meter is transformed into the mathematical platform coordinate system;

Model is floated in estimating when (5) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize that inertia device often is worth the estimation of deviation;

(6) inertia device that Kalman Filter Estimation is gone out often is worth deviation and adopts the method for average filter it to be carried out smoothly the influence of filtering fluctuation within a narrow range;

It is that the transposition scheme of a swing circle is that described IMU adopts 8 commentaries on classics to stop order: order 1, IMU clockwise rotates 180 ° of in-position C, stand-by time T from the A point _tOrder 2, IMU clockwise rotates 90 ° of in-position D, stand-by time T from the C point _tOrder 3, IMU rotates counterclockwise 180 ° of in-position B, stand-by time T from the D point _tOrder 4, IMU rotates counterclockwise 90 ° of in-position A, stand-by time T from the B point _tOrder 5, IMU rotates counterclockwise 180 ° of in-position C, stand-by time T from the A point _tOrder 6, IMU rotates counterclockwise 90 ° of in-position B, stand-by time T from the C point _tOrder 7, IMU clockwise rotates 180 ° of in-position D, stand-by time T from the B point _tOrder 8, IMU clockwise rotates 90 ° of in-position A, stand-by time T from the D point _tIMU rotates sequential loop according to this to carry out; IMU pause point p3, p8 and p4, p7 on the wherein horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1 on the north orientation axle, p5 and p2, p6 are symmetrical in the rotating shaft center.

2. a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme according to claim 1 is characterized in that describedly determining the strapdown matrix according to gyrostatic output under the rotation status

And the method that the output of degree of will speed up meter is transformed into the mathematical platform coordinate system is:

According to the quadravalence runge kutta method, the application hypercomplex number is carried out real-time update to the strapdown matrix of inertial navigation system, and its formulate is:

${\stackrel{\&CenterDot;}{T}}_s^p=T_s^p[\&times;{\mathrm{\&omega;}}_{\mathrm{is}}^s]$

Wherein

The output of expression gyroscope; Utilize the transformational relation of IMU coordinate system and mathematical platform coordinate system, calculate the accekeration under the mathematical platform coordinate system,

3. a kind of mooring and drift estimating method based on single-shaft rotation-stop scheme according to claim 2 is characterized in that described estimating when setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system floats model, the bearer rate v that provides with GPS ₀Be benchmark, and with the difference of inertial navigation system clearing speed as observed quantity, utilize Kalman Filter Technology to realize that the estimation approach of inertia device deviation is:

Adopt carrier of separating error of position information model to float model as estimating of system, foundation is that the Kalman filtering state equation and the velocity error of state variable is the measurement equation of measurement amount with the velocity error;

1) set up the state equation of Kalman filtering:

The state error of rotation strapdown inertial navitation system (SINS) is described with linear first-order differential equation:

$\stackrel{\&CenterDot;}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)$

The state vector of etching system when wherein X (t) is t; A (t) and B (t) are respectively the state matrix and the noise matrix of system;

The state vector of system is:

The white noise vector of system is:

$W={\left[\begin{array}{cccccccccc}{a}_x& a_y& {\mathrm{\&omega;}}_x& {\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_z& 0& 0& 0& 0& 0\end{array}\right]}^T$

δ V wherein _e, δ V _nThe velocity error of representing east orientation, north orientation respectively;

Be respectively IMU coordinate system ox _s, oy _sAxis accelerometer zero partially; ε _x, ε _y, ε _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe constant value drift of axle gyro; a _x, a _yBe respectively IMU coordinate system ox _s, oy _sThe white noise error of axis accelerometer; ω _x, ω _y, ω _zBe respectively IMU coordinate system ox _s, oy _s, oz _sThe white noise error of axle gyro;

The state matrix of system is:

$A\left(t\right)=\left[\begin{array}{cc}{F}_{5\&times;5}& T_{5\&times;5}\\ 0_{5\&times;5}& 0_{5\&times;5}\end{array}\right]$

Wherein: $F_{5\&times;5}=\left[\begin{array}{cc}{F}_{2\&times;2}& F_{2\&times;3}\\ F_{3\&times;2}& F_{3\&times;3}\end{array}\right]$

$F_{2\&times;2}=\left[\begin{array}{cc}\frac{V_n\tan L}{R_n}& 2{\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}\\ -2({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0\end{array}\right]$

$F_{2\&times;3}=\left[\begin{array}{ccc}0& -f_u& f_n\\ f_u& 0& -f_e\end{array}\right]$

$F_{3\&times;2}=\left[\begin{array}{cc}0& -\frac{1}{R_m}\\ \frac{1}{R_n}& 0\\ \frac{\tan L}{R_n}& 0\end{array}\right]$

$F_{3\&times;3}=\left[\begin{array}{ccc}0& {\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n}& -({\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n})\\ -({\mathrm{\&omega;}}_{\mathrm{ie}}\sin L+\frac{V_e\tan L}{R_n})& 0& -\frac{\mathrm{Vn}}{R_m}\\ {\mathrm{\&omega;}}_{\mathrm{ie}}\cos L+\frac{V_e}{R_n}& \frac{V_n}{R_m}& 0\end{array}\right]$

V _e, V _nThe speed of representing east orientation, north orientation respectively; ω _IeThe expression rotational-angular velocity of the earth; R _m, R _nRepresent earth meridian, fourth of the twelve Earthly Branches radius-of-curvature at the tenth of the twelve Earthly Branches respectively; L represents local latitude; f _e, f _n, f _uBe expressed as respectively navigation coordinate system down east orientation, north orientation, day to specific force;

The strapdown matrix

For:

$T_s^p=\left[\begin{array}{ccc}{T}_{11}& T_{12}& T_{13}\\ T_{21}& T_{22}& T_{23}\\ T_{31}& T_{32}& T_{33}\end{array}\right]$

Then

$T_{5\&times;5}=\left[\begin{array}{ccccc}{T}_{11}& T_{12}& 0& 0& 0\\ T_{21}& T_{22}& 0& 0& 0\\ 0& 0& -T_{11}& -T_{12}& -T_{13}\\ 0& 0& -T_{21}& -T_{22}& -T_{23}\\ 0& 0& -T_{31}& -T_{32}& -T_{33}\end{array}\right];$

2) set up the measurement equation of Kalman filtering:

The measurement equation of describing the rotation strapdown inertial navitation system (SINS) with linear first-order differential equation is as follows:

Z(t)＝H(t)X(t)+v(t)

Wherein: the measurement vector of etching system during Z (t) expression t; The measurement matrix of H (t) expression system;

The system measurements matrix is:

$H\left(t\right)=\left[\begin{array}{cccccccccc}1& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

Amount is measured as the horizontal velocity error:

v(t)＝[v _e?v _n] ^T。

Images