CN101706287B

CN101706287B - Rotating strapdown system on-site proving method based on digital high-passing filtering

Info

Publication number: CN101706287B
Application number: CN2009100732422A
Authority: CN
Inventors: 孙枫; 孙伟; 袁俊佳; 薛媛媛; 王武剑; 李国强
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2009-11-20
Filing date: 2009-11-20
Publication date: 2012-01-04
Anticipated expiration: 2029-11-20
Also published as: CN101706287A

Abstract

The invention provides a rotating strapdown system on-site proving method based on digital high-passing filtering, which comprises the steps: (1) determining an initial position parameter of a carrier by a GPS; (2) collecting data output by an optical fiber gyroscope and an acceleration meter, and processing the data; (3) rotating and stopping at four positions of a single axis of an inertia measurement unit; (4) analyzing biased observability degree of an inertia device by using a spectral condition number method; (5) filtering the Schuler period contained in velocity information of a navigation system by adopting an IIR high-passing digital filter; and (6) taking filtered velocity information as observed quantity, and estimating the deviation of the inertia device by adopting a Kalman filtering technique. When the carrier is in anchoring state, the adoption of the method can obtain higher on-site proving precision.

Description

A Field Calibration Method for Rotating Strapdown System Based on Digital High-pass Filter

技术领域 technical field

本发明涉及的是一种旋转捷联系统现场标定方法，尤其涉及的是一种基于数字高通滤波的旋转捷联系统现场标定方法。The invention relates to a method for on-site calibration of a rotary strapdown system, in particular to a method for on-site calibration of a rotary strapdown system based on digital high-pass filtering.

背景技术 Background technique

捷联惯性导航系统(SINS)是将陀螺仪、加速度计等惯性元件固连在载体上，根据牛顿力学定律，通过对这些惯性元件采集的信息进行处理，得到载体的姿态、速度、位置、加速度、角速度和角加速度等全导航信息的完全自主导航设备。由于SINS完全依靠自身的惯性元件，不依靠任何外界信息测量导航参数，因此，它具有隐蔽性好，不受气候条件限制，不受干扰等优点，是一种完全自主式、全天候的导航系统，已广泛应用于航空、航天、航海等领域。根据SINS的基本原理，SINS在导航过程中惯性器件常值偏差的存在是导致惯导系统导航精度难以提高的主要因素。如何有效限制惯性导航误差发散、提高惯性导航系统精度是惯性导航领域一项非常重要的课题。Strapdown Inertial Navigation System (SINS) is to connect inertial components such as gyroscopes and accelerometers to the carrier. According to Newton’s laws of mechanics, the attitude, velocity, position and acceleration of the carrier can be obtained by processing the information collected by these inertial components. , angular velocity and angular acceleration and other full navigation information of fully autonomous navigation equipment. 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. Has been widely 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 navigation accuracy of the inertial navigation system. How to effectively limit the divergence of inertial navigation errors and improve the accuracy of inertial navigation systems is a very important topic in the field of inertial navigation.

惯导系统的误差抑制，不是依赖于外部辅助对误差状态进行估计，而是研究惯性导航误差在特定运动条件下的传播规律，并依据此规律限制误差发散，提高导航精度的方法。转动抑制是最典型的误差抑制方法：通过绕一个轴或多个轴转动惯性测量单元(IMU)，对导航误差进行调制，达到控制导航误差发散、提高导航精度的目的。The error suppression of the inertial navigation system does not rely on external assistance to estimate the error state, but to study the propagation law of inertial navigation errors under specific motion conditions, and based on this law to limit the error divergence and improve navigation accuracy. Rotation suppression is the most typical error suppression method: by rotating the inertial measurement unit (IMU) around one or more axes, the navigation error is modulated to control the divergence of navigation errors and improve navigation accuracy.

标定技术就是一种从软件方面来提高惯导实际使用精度的方法。标定技术本质上也是一种误差补偿技术。所谓误差补偿技术就是建立惯性元件和惯导系统的误差数学模型，通过一定的试验来确定模型系数，进而通过软件算法来消除误差。惯性元件和惯导系统在出厂之前，必须通过标定来确定基本的误差数学模型参数，以保证元件和系统的正常工作。而且惯性元件高阶误差项的研究、惯导系统恶劣动态环境下的误差补偿都是在标定的基础上进行的，可以说标定工作是整个误差补偿技术的基础。惯性测量单元逐次开机误差(惯性元件的漂移与刻度因数误差)是不同的，且随着时间增加IMU输出误差随时间发生漂移，实现现场在线标定对于提高系统精度具有重大意义。Calibration technology is a method to improve the accuracy of inertial navigation from the aspect of software. Calibration technology is essentially an error compensation technology. The so-called error compensation technology is to establish the error mathematical model of the inertial components and the inertial navigation system, determine the model coefficients through certain experiments, and then eliminate the errors through software algorithms. Before leaving the factory, inertial components and inertial navigation systems must be calibrated to determine the basic error mathematical model parameters to ensure the normal operation of components and systems. Moreover, the research on the high-order error terms of the inertial components and the error compensation of the inertial navigation system in the harsh dynamic environment are all carried out on the basis of calibration. It can be said that the calibration work is the basis of the entire error compensation technology. The successive start-up errors of the inertial measurement unit (the drift of the inertial element and the scale factor error) are different, and the IMU output error drifts with time as time increases. Realizing on-site calibration is of great significance for improving the system accuracy.

在CNKI库中与本发明相关的公开报道有：1、《水平初始对准误差对旋转IMU导航系统的精度影响》(刘峰，徐策，尚克军，王子静；中国惯性技术学报；2008年第6期)，该文章主要讲述了水平不对准对旋转捷联惯导系统的影响，其中文中讲述了IMU单轴单向旋转，但是并没有提及现场标定的内容。2、《旋转IMU在光纤捷联航姿系统中的应用》(王其、徐晓苏；中国惯性技术学报；2007年第3期)，该文章主要介绍了单轴、双轴旋转方式，并在理论上给予证明。3、《光纤陀螺IMU的六位置旋转现场标定新方法》(刘百奇，房建成；光电工程；2008年第1期)，该文章主要介绍了在使用现场将光纤陀螺IMU在六个位置上进行十二次旋转，然后根据光纤陀螺IMU的误差模型建立42个非线性输入输出方程，通过旋转积分和对称位置误差相消，消除方程中的非线性项，最终求解出陀螺标度因数、陀螺常值漂移、陀螺安装误差和加速度计常值偏置等15个误差系数。Public reports relevant to the present invention in the CNKI library include: 1. "Initial Horizontal Alignment Errors Impact on the Accuracy of the Rotating IMU Navigation System" (Liu Feng, Xu Ce, Shang Kejun, Wang Zijing; Chinese Journal of Inertial Technology; 2008 No. 6 of 2009), the article mainly described the influence of horizontal misalignment on the rotating strapdown inertial navigation system. It described the single-axis and one-way rotation of the IMU, but did not mention the content of on-site calibration. 2. "Application of Rotating IMU in Fiber Optic Strapdown Heading and Attitude System" (Wang Qi, Xu Xiaosu; Chinese Journal of Inertial Technology; Issue 3, 2007), this article mainly introduces single-axis and dual-axis rotation methods, and in theory Prove above. 3. "A new method for six-position rotating on-site calibration of fiber optic gyroscope IMU" (Liu Baiqi, Fang Jiancheng; Optoelectronic Engineering; No. 1, 2008), this article mainly introduces the six-position rotating fiber optic gyro IMU in the field of use. Secondary rotation, and then establish 42 nonlinear input and output equations according to the error model of the fiber optic gyro IMU, and eliminate the nonlinear terms in the equation through the rotation integral and symmetrical position error cancellation, and finally solve the gyro scale factor and gyro constant value 15 error coefficients for drift, gyro mounting error, and accelerometer constant bias.

发明内容 Contents of the invention

本发明的目的在于提供一种当载体处于系泊状态下，可以获得较高现场标定精度的一种基于数字高通滤波的旋转捷联系统现场标定方法。The purpose of the present invention is to provide an on-site calibration method for a rotating strapdown system based on a digital high-pass filter that can obtain higher on-site calibration accuracy when the carrier is in a mooring state.

本发明的目的是这样实现的：包括以下步骤：The object of the present invention is achieved like this: comprise the following steps:

(1)利用全球定位系统GPS确定载体的初始位置参数，将它们装订至导航计算机中；(1) Utilize the global positioning system GPS to determine the initial position parameters of the carrier, and bind them into the navigation computer;

(2)光纤陀螺捷联惯性导航系统进行预热后采集光纤陀螺仪和石英加速度计输出的数据；(2) After preheating, the fiber optic gyroscope strapdown inertial navigation system collects the data output by the fiber optic gyroscope and the quartz accelerometer;

(3)IMU采用8个转停次序为一个旋转周期的转位方案；(3) The IMU adopts an indexing scheme in which 8 rotation-stop sequences are one rotation cycle;

(4)利用谱条件数法求取IMU四位置转停过程中惯性器件偏差的可观测度；(4) Use the spectral condition number method to obtain the observability of the inertial device deviation during the four-position rotation and stop of the IMU;

(5)设计无限冲击响应数字高通滤波器，将导航系下解算出的载体水平速度进行高通滤波处理，滤除导航系下载体速度中的舒勒周期，保留载体由于摇摆和荡运动产生的速度偏差；(5) Design an infinite shock response digital high-pass filter, and perform high-pass filtering on the horizontal velocity of the carrier calculated by the solution under the navigation system, filter out the Schuler period in the velocity of the carrier under the navigation system, and retain the velocity of the carrier due to swaying and oscillating motion deviation;

(6)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以高通滤波后得到的速度直接作为观测量，利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定。(6) According to the error equation of the inertial navigation system's moving base, the drift estimation model is established when the vehicle is in mooring state, and the velocity obtained after high-pass filtering is directly used as the observation quantity, and the on-site calibration of the rotating strapdown inertial navigation system is realized by using the Kalman filter technology .

本发明还可以包括：The present invention may also include:

1、所述IMU采用8个转停次序为一个旋转周期的转位方案为：1. The IMU adopts 8 rotation-stop sequences as a rotation cycle rotation scheme as follows:

次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行；水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心；四位置转停方案仍然是转动角度为180°或90°间隔进行。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 rotates in a cycle according to this sequence; the IMU stop points p3, p8 and p4, p7 on the horizontal east axis Symmetrical to the center of the rotating shaft; the pause points p1, p5 and p2, p6 on the north axis 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°.

2、所述利用谱条件数法求取IMU四位置转停过程中惯性器件偏差的可观测度的方法为：2. The method of using the spectrum condition number method to obtain the observability of the inertial device deviation during the IMU four-position rotation and stop process is:

求解线性方程组Solve system of linear equations

AX＝b，b∈Cⁿ AX=b, b∈C ⁿ

设A∈C^n×n，||·||是一种算子范数，Suppose A∈C ^n×n , ||·|| is a kind of operator norm,

$cond cond ((A A)) = = \{\begin{matrix} | | | | A A | | | | | | | | {A A}^{- - 11} | | | |,, & det det A A &NotEqual; &NotEqual; 00 \\ \infty \infty,, & det det A A = = 00 \end{matrix}$

称cond(A)为矩阵A的关于算子范数||·||的条件数，常用的是关于p-范数||·||_p的条件数，记作cond_p(A)，cond₂(A)为谱条件数，Call cond(A) the condition number of the matrix A about the operator norm ||·||, and the condition number about the p-norm ||·|| _p is commonly used, denoted as cond _p (A), cond ₂ (A) is the spectral condition number,

针对离散时变系统：For discrete time-varying systems:

$\{\begin{matrix} {X x}_{k k + + 11} = = {F f}_{k k} {X x}_{k k} \\ {Z Z}_{k k} = = {HX HX}_{k k} \end{matrix}$

将系统状态方程带入观测方程得到一组方程：Bring the system state equation into the observation equation to get a set of equations:

$\{\begin{matrix} {Z Z}_{00} = = {HX HX}_{00} \\ {Z Z}_{11} = = {HF HF}_{00} {X x}_{00} \\ . . \\ . . \\ . . \\ {Z Z}_{k k} = = H h {Π Π}_{i i = = 00}^{k k} {F f}_{i i} {X x}_{00} \end{matrix}$

记remember

${O o}_{k k} = = {[\begin{matrix} H h & {HF HF}_{00} . . . . . . H h {Π Π}_{i i = = 00}^{k k} {F f}_{i i} \end{matrix}]}^{T T}$

则but

O_kX₀＝Z _Ok X ₀ = Z

对于定常系统F_k为常数，O_k就是可观性矩阵，时变系统在采样点上进行观测得到离散时变系统，F_k就是采样周期内的状态转移矩阵Φ(t_k+T，t_k)，For a steady system F _k is a constant, O _k is the observability matrix, and the time-varying system is observed at the sampling point to obtain a discrete time-varying system, and F _k is the state transition matrix Φ(t _k +T, t _k ) within the sampling period ,

O_k＝[HHΦ(t₁，t₀)…HΦ(t_k，t₀)]^T O _k =[HHΦ(t ₁ ,t ₀ )...HΦ(t _k ,t ₀ )] ^T

状态是n维的，一次观测量Z_k是r维的(r＜n)，观测阵H的秩为r，至少进行k次观测(kr≥n)，求出X₀，根据最小二乘法求解状态X₀，The state is n-dimensional, an observation quantity Z _k is r-dimensional (r<n), the rank of the observation array H is r, and at least k observations are made (kr≥n), and X ₀ is obtained, and the solution is obtained by the least square method state X ₀ ,

${X x}_{00} = = {(({O o}_{k k}^{T T} {O o}_{k k}))}^{- - 11} {O o}_{k k}^{T T} Z Z$

为观测阵，由于

是正规矩阵，通过计算谱条件数cond₂(M)，来分析解的稳定性，而

is the observation array, because

is a normal matrix, and the stability of the solution is analyzed by calculating the spectral condition number cond ₂ (M), while

${cond cond}_{22} ((M m)) = = \frac{\underset{λ λ &Element; &Element; λ λ ((M m))}{max max} | | λ λ | |}{\underset{λ λ &Element; &Element; λ λ ((M m))}{min min} | | λ λ | |}$

式中，λ为矩阵M的特征值，进一步分析矩阵M的特征值和特征向量，以便确定究竟哪些状态的可观测度较好，哪些状态的可观测度差，将M可酉对角化，记U^TMU＝Λ，其中Λ＝diag(λ₁，λ₂，...λ_n)，则状态X的可观测度S：In the formula, λ is the eigenvalue of the matrix M, further analyzing the eigenvalues and eigenvectors of the matrix M, in order to determine which states have better observability and which states have poor observability, and M can be unitary diagonalized, Denote U ^T MU = Λ, where Λ = diag(λ ₁ , λ ₂ ,...λ _n ), then the observability S of state X:

S＝abs(U[λ₁，λ₂，...，λ_n]^T)S=abs(U[λ ₁ ,λ ₂ ,...,λ _n ] ^T )

计算出系统可观测性矩阵M的特征值和特征向量，确定出各个状态的可观测度。Calculate the eigenvalues and eigenvectors of the system observability matrix M, and determine the observability of each state.

3、所述利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定的方法为：3. The method for realizing the on-site calibration of the rotary strapdown inertial navigation system by using the Kalman filter technology is:

1)建立卡尔曼滤波的状态方程：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:

$\overset{. .}{X x} ((t t)) = = A A ((t t)) X x ((t t)) + + B B ((t t)) W W ((t t))$

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；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 0 0 0]^T W＝[a _x a _y ω _x ω _y ωz 0 0 0 0 0 0 0 0] ^T

其中δV_e、δV_n分别表示东向、北向的速度误差；分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；δK_gx、δK_gy、δK_gz分别为IMU坐标系ox_s、oy_s、oz_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; δK _gx , δK _gy , δK _gz are the scale factor errors of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively; ω _x , ω _y and ω _z are the white noise errors of the IMU coordinate system ox _s , oy _s , and oz _s- axis gyro, respectively;

系统的状态转移矩阵为：The state transition matrix of the system is:

$A A = = [\begin{matrix} {F f}_{22 \times \times 22}^{11} & {f f}_{22 \times \times 33} & {\overset{~ ~}{T T}}_{22 \times \times 22} & {O o}_{22 \times \times 66} \\ {F f}_{33 \times \times 22}^{22} & {F f}_{33 \times \times 33}^{33} & {O o}_{33 \times \times 22} & {T T}_{33 \times \times 66} \\ {O o}_{88 \times \times 22} & {O o}_{88 \times \times 33} & {O o}_{88 \times \times 22} & {O o}_{88 \times \times 66} \end{matrix}]$

${F f}_{22 \times \times 22}^{11} = = [\begin{matrix} \frac{{V V}_{N N}}{{R R}_{n no}} tan the tan L L & 22 {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{n no}} tan the tan L L \\ - - ((22 {ω ω}_{ie ie} sin sin L L + + \frac{22 {V V}_{E E.}}{{R R}_{n no}} tan the tan L L)) & 00 \end{matrix}]$

${F f}_{3232}^{22} = = [\begin{matrix} 00 & - - \frac{11}{{R R}_{m m}} \\ \frac{11}{{R R}_{n no}} & 00 \\ \frac{tan the tan L L}{{R R}_{n no}} & 00 \end{matrix}]$

${F f}_{33 \times \times 33}^{33} = = [\begin{matrix} 00 & {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.} tan the tan L L}{{R R}_{n no}} & - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{n no}})) \\ - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.} tan the tan L L}{{R R}_{n no}})) & 00 & - - \frac{{V V}_{N N}}{{R R}_{m m}} \\ {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{n no}} & \frac{{V V}_{N N}}{{R R}_{}} & 00 \end{matrix}]$

${f f}_{22 \times \times 33} = = [\begin{matrix} 00 & - - {f f}_{U u} & {f f}_{N N} \\ {f f}_{U u} & 00 & {f f}_{E E.} \end{matrix}]$

${\overset{~ ~}{T T}}_{22 \times \times 22} = = [\begin{matrix} {T T}_{1111} & {T T}_{1212} \\ {T T}_{21 twenty one} & {T T}_{22 twenty two} \end{matrix}]$

${T T}_{33 \times \times 66} = = [\begin{matrix} - - {T T}_{1111} & - - {T T}_{1212} & {- - T T}_{1313} & - - {T T}_{1111} {ω ω}_{x x} & - - {T T}_{1212} {ω ω}_{y the y} & - - {T T}_{1313} {ω ω}_{z z} \\ - - {T T}_{21 twenty one} & - - {T T}_{22 twenty two} & - - {T T}_{23 twenty three} & - - {T T}_{21 twenty one} {ω ω}_{x x} & - - {T T}_{22 twenty two} {ω ω}_{y the y} & - - {T T}_{23 twenty three} {ω ω}_{z z} \\ - - {T T}_{3131} & - - {T T}_{3232} & - - {T T}_{3333} & - - {T T}_{3131} {ω ω}_{x x} & - - {T T}_{3232} {ω ω}_{y the y} & - - {T T}_{3333} {ω ω}_{z z} \end{matrix}]$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力；V _E , V _N represent the eastward and northward velocities respectively; ω _x , ω _y , ω _z represent the three input angular velocities of the gyroscope; ω _ie represent the earth's rotation angular velocity; R _m , R _n represent the earth's meridian, Radius of curvature; L represents the local latitude; f _E , f _N , and f _U represent the relative forces in the eastward, northward, and celestial directions of the navigation coordinate system, respectively;

2)建立卡尔曼滤波的量测方程：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)Z(t)=H(t)X(t)+V(t)

其中：Z(t)表示t时刻系统的量测向量；H(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:

$H h = = [\begin{matrix} 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \end{matrix}]$

量测量为高通滤波后得到的速度：Quantitative measurement as velocity obtained after high-pass filtering:

$Z Z = = {[[{\overset{~ ~}{V V}}_{E E.} {\overset{~ ~}{V V}}_{N N}]]}^{T T} . .$

本发明将惯性测量单元相对载体方位轴固定的四个位置转停，利用IMU的转停运动提高系统参数的可观测度，设计卡尔曼滤波器引入高精度外部信息源分别激励IMU的各个标定误差项，估计并补偿IMU输出误差，完成系统的现场标定。In the present invention, the inertial measurement unit is rotated and stopped at four fixed positions relative to the azimuth axis of the carrier, and the observability of system parameters is improved by using the rotation and stop movement of the IMU, and the Kalman filter is designed to introduce high-precision external information sources to respectively stimulate each calibration error of the IMU. item, estimate and compensate the IMU output error, and complete the on-site calibration of the system.

本发明与现有技术相比的优点在于：本发明打破了传统标定方法不适用于系统的现场标定，提出一种利用IMU转停提高系统参数可观测度并采用卡尔曼滤波技术对惯性器件偏差进行在线标定的方案，该方法可以将惯性器件常值偏差和陀螺仪标度因数误差进行现场估计并补偿，可以有效地提高系统对准及导航精度。Compared with the prior art, the present invention has the advantages that: the present invention breaks the traditional calibration method which is not applicable to the on-site calibration of the system, and proposes a method of improving the observability of system parameters by using IMU rotation and stopping, and using Kalman filter technology to correct the deviation of inertial devices. The scheme of online calibration, this method can estimate and compensate the constant value deviation of the inertial device and the scale factor error of the gyroscope on site, and can effectively improve the system alignment and navigation accuracy.

对本发明有益的效果说明如下：The beneficial effects of the present invention are described as follows:

在Visual C++仿真条件下，对该方法进行仿真实验：Under Visual C++ simulation conditions, the simulation experiment of this method is carried out:

设定陀螺仪常值漂移为0.01°/h，加速度计零位偏差为10^-4g；系统初始对准误差为0.1°、0.1°、0.5°；载体以正弦规律绕纵摇轴、横摇轴和航向轴作三轴摇摆运动，其数学模型为：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:

$\{\begin{matrix} θ θ = = {θ θ}_{m m} sin sin (({ω ω}_{θ θ} t t + + {φ φ}_{θ θ})) \\ γ γ = = {γ γ}_{m m} sin sin (({ω ω}_{γ γ} t t + + {φ φ}_{γ γ})) \\ ψ ψ = = {ψ ψ}_{m m} sin sin (({ω ω}_{ψ ψ} t t + + {φ φ}_{ψ ψ})) + + k k \end{matrix}$

其中：θ、γ、ψ分别表示纵摇角、横摇角和航向角的摇摆角度变量；θ_m、γ_m、ψ_m分别表示相应的摇摆角度幅值；ω_θ、ω_γ、ω_ψ分别表示相应的摇摆角频率；φ_θ、φ_γ、φ_ψ分别表示相应的初始相位；ω_i＝2π/T_i，i＝θ、γ、ψ，T_i表示相应的摇摆周期，k为初始航向角。仿真时取：θ_m＝6°，γ_m＝12°，ψ_m＝10°，T_θ＝8s，T_γ＝10s，T_ψ＝6s，k＝0°。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 acceleration caused by the sway, surge and heave of the carrier is:

式中，i＝x，y，z为地理坐标系的东向、北向、天向。

A_{D_{y}} = 0.03 m, A_{D_{z}} = 0.3 m; T_{D_{x}} = 7 s, T_{D_{y}} = 6 s, T_{D_{z}} = 8 s;

为[0，2π]上服从均匀分布的随机相位。In the formula, i=x, y, and z are the east, north, and sky directions of the geographic coordinate system.

A_{{D.}_{the y}} = 0.03 m, A_{{D.}_{z}} = 0.3 m; T_{{D.}_{x}} = 7 the s, T_{{D.}_{the y}} = 6 the s, T_{{D.}_{z}} = 8 the s;

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

载体初始位置：北纬45.7796°，东经126.6705°；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;

赤道半径：R_e＝6378393.0m；Equatorial radius: R _e = 6378393.0m;

椭球度：e＝3.367e-3；Ellipsoid: e=3.367e-3;

由万有引力可得的地球表面重力加速度：g₀＝9.78049；The gravitational acceleration on the earth's surface obtained from the universal gravitation: g ₀ =9.78049;

地球自转角速度(弧度/秒)：7.2921158e-5；Earth rotation angular velocity (rad/s): 7.2921158e-5;

陀螺仪常值漂移：0.01度/小时；Gyroscope constant value drift: 0.01 degrees/hour;

陀螺仪随机游走：0.001度/ Gyroscope random walk: 0.001 degrees/

陀螺仪标度因数误差：1000ppm；Gyroscope scale factor error: 1000ppm;

加速度计零偏：10^-4g₀；Accelerometer zero bias: 10 ^-4 g ₀ ;

加速度计噪声：10^-6g₀；Accelerometer noise: 10 ^-6 g ₀ ;

常数：π＝3.1415926；Constant: π=3.1415926;

IMU四位置转停方案的数学模型参数：Mathematical model parameters of IMU four-position turn-stop scheme:

四个位置的停顿时间：T_t＝5min；Dwell time at four positions: T _t = 5min;

转动180°和90°时消耗的时间：T_z＝12s；Time consumed when turning 180° and 90°: T _z =12s;

转动180°和90°的过程中，每一个转位中的加减速时间各为4s。In the process of turning 180° and 90°, the acceleration and deceleration time in each index is 4s.

附图说明 Description of drawings

图1为本发明的一种基于数字高通滤波的旋转捷联系统现场标定方法流程图；Fig. 1 is a kind of flow chart of on-the-spot calibration method of rotating strapdown system based on digital high-pass filter of the present invention;

图2为本发明的IMU四位置转停过程中，IMU坐标系与载体坐标系的相对位置关系；Fig. 2 is the relative positional relationship between the IMU coordinate system and the carrier coordinate system during the IMU four-position rotation stop process of the present invention;

图3为本发明的估计的陀螺仪常值漂移；Fig. 3 is the estimated gyroscope constant value drift of the present invention;

图4为本发明的估计的水平方向上的加速度计零偏；Fig. 4 is the accelerometer zero bias on the estimated horizontal direction of the present invention;

图5为本发明的估计的陀螺仪标度因数误差。Figure 5 is the estimated gyroscope scale factor error of the present invention.

具体实施方式 Detailed ways

下面结合附图举例对本发明做更详细地描述：The present invention is described in more detail below in conjunction with accompanying drawing example:

次序1，IMU从A点出发顺时针转动180°到达位置C，停止时间T_t；次序2，IMU从C点出发顺时针转动90°到达位置D，停止时间T_t；次序3，IMU从D点出发逆时针转动180°到达位置B，停止时间T_t；次序4，IMU从B点出发逆时针转动90°到达位置A，停止时间T_t；次序5，IMU从A点出发逆时针转动180°到达位置C，停止时间T_t；次序6，IMU从C点出发逆时针转动90°到达位置B，停止时间T_t；次序7，IMU从B点出发顺时针转动180°到达位置D，停止时间T_t；次序8，IMU从D点出发顺时针转动90°到达位置A，停止时间T_t；IMU按照此转动顺序循环进行。为了有效地对水平方向上的惯性器件偏差在对称位置上进行正负平均，定义水平东向轴上的IMU停顿点p3、p8与p4、p7对称于转轴中心；北向轴上的停顿点p1、p5与p2、p6对称于转轴中心。改进的四位置转停方案仍然是转动角度为180°或90°间隔进行。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 improved four-position turn-to-stop scheme is still carried out at intervals of 180° or 90° of turning angle.

考虑求解线性方程组Consider solving a system of linear equations

AX＝b，b∈Cⁿ (1)AX=b, b∈C ⁿ (1)

$cond cond ((A A)) = = \{\begin{matrix} | | | | A A | | | | | | | | {A A}^{- - 11} | | | |,, & det det A A &NotEqual; &NotEqual; 00 \\ \infty \infty,, & det det A A = = 00 \end{matrix} - - - - - - ((22))$

称cond(A)为矩阵A(关于求逆或求解线性方程组)的关于算子范数||·||的条件数。常用的是关于p-范数||·||_p的条件数，可记作cond_p(A)。特别，称cond₂(A)为谱条件数。We call cond(A) the condition number of the operator norm ||·|| of the matrix A (about inverting or solving linear equations). Commonly used is the condition number of the p-norm ||·|| _p , which can be denoted as cond _p (A). In particular, cond ₂ (A) is called the spectral condition number.

针对离散时变系统：For discrete time-varying systems:

$\{\begin{matrix} {X x}_{k k + + 11} = = {F f}_{k k} {X x}_{k k} \\ {Z Z}_{k k} = = {HX HX}_{k k} \end{matrix} - - - - - - ((33))$

能否由一组观测值Z＝[Z₀，Z₁，....Z_k]^T求出初始状态X₀是系统可观的实质。将系统状态方程带入观测方程得到一组方程：Whether the initial state X ₀ can be obtained from a set of observed values Z=[Z ₀ , Z ₁ , ... Z _k ] ^T is the substantial essence of the system. Bring the system state equation into the observation equation to get a set of equations:

$\{\begin{matrix} {Z Z}_{00} = = {HX HX}_{00} \\ {Z Z}_{11} = = {HF HF}_{00} {X x}_{00} \\ . . \\ . . \\ . . \\ {Z Z}_{k k} = = H h {Π Π}_{i i = = 00}^{k k} {F f}_{i i} {X x}_{00} \end{matrix} - - - - - - ((44))$

记 $O_{k} = {[\begin{matrix} H & {HF}_{0} . . . H Π_{i = 0}^{k} F_{i} \end{matrix}]}^{T} - - - (5)$ remember $o_{k} = {[\begin{matrix} h & HF & _{0} . . . h Π_{i = 0}^{k} f_{i} \end{matrix}]}^{T} - - - (5)$

则有then there is

O_kX₀＝Z (6) _Ok X ₀ = Z (6)

对于定常系统F_k为常数，O_k就是可观性矩阵。时变系统在采样点上进行观测得到离散时变系统，F_k就是采样周期内的状态转移矩阵Φ(t_k+T，t_k)，因此有For a steady system F _k is a constant, O _k is the observability matrix. The time-varying system is observed at the sampling point to obtain a discrete time-varying system, F _k is the state transition matrix Φ(t _k +T, t _k ) in the sampling period, so there is

O_k＝[HHΦ(t₁，t₀)…HΦ(t_k，t₀)]^T (7)O _k =[HHΦ(t ₁ ,t ₀ )...HΦ(t _k ,t ₀ )] ^T (7)

设状态是n维的，一次观测量Z_k是r维的(r＜n)，观测阵H的秩为r，至少进行k次观测(kr≥n)，才可以求出X₀。根据最小二乘法求解状态X₀。Assuming that the state is n-dimensional, one observation quantity Z _k is r-dimensional (r<n), the rank of the observation array H is r, and X ₀ can only be obtained by making at least k observations (kr≥n). The state X ₀ is solved according to the method of least squares.

${X x}_{00} = = {(({O o}_{k k}^{T T} {O o}_{k k}))}^{- - 11} {O o}_{k k}^{T T} Z Z - - - - - - ((88))$

记

为观测阵。由于

是正规矩阵，则可以通过计算谱条件数cond₂(M)，来分析解的稳定性，而remember

for the observation array. because

is a normal matrix, the stability of the solution can be analyzed by calculating the spectral condition number cond ₂ (M), and

${cond cond}_{22} ((M m)) = = \frac{\underset{λ λ &Element; &Element; λ λ ((M m))}{max max} | | λ λ | |}{\underset{λ λ &Element; &Element; λ λ ((M m))}{min min} | | λ λ | |} - - - - - - ((99))$

式中，λ为矩阵M的特征值。进一步分析矩阵M的特征值和特征向量，以便确定究竟哪些状态的可观测度较好，哪些状态的可观测度差。将M可酉对角化，记U^TMU＝Λ，其中Λ＝diag(λ₁，λ₂，...λ_n)，则状态X的可观测度S：In the formula, λ is the eigenvalue of matrix M. Further analyze the eigenvalues and eigenvectors of the matrix M in order to determine which states have better observability and which states have poor observability. M can be unitary diagonalized, write U ^T MU = Λ, where Λ = diag(λ ₁ , λ ₂ ,...λ _n ), then the observability S of state X:

S＝abs(U[λ₁，λ₂，...，λ_n]^T) (10)S=abs(U[λ ₁ ,λ ₂ ,...,λ _n ] ^T ) (10)

计算出系统可观测性矩阵M的特征值和特征向量，就可以确定出各个状态的可观测度。By calculating the eigenvalues and eigenvectors of the system observability matrix M, the observability of each state can be determined.

(5)设计无限冲击响应数字高通滤波器(IIR)，将导航系下解算出的载体水平速度进行高通滤波处理，滤除导航系下载体速度中的舒勒周期，保留载体由于摇摆和荡运动产生的速度偏差；(5) Design an infinite impulse response digital high-pass filter (IIR), perform high-pass filtering on the horizontal velocity of the carrier calculated by the solution in the navigation system, filter out the Schuler period in the carrier velocity in the navigation system, and retain the carrier due to swaying and swinging motion resulting speed deviation;

(6)根据惯导系统动基座误差方程建立载体系泊状态时的估漂模型，以高通滤波后得到的速度直接作为观测量。利用卡尔曼滤波技术实现旋转捷联惯导系统的现场标定；(6) According to the error equation of the inertial navigation system's moving base, the drift estimation model in the mooring state of the carrier is established, and the velocity obtained after high-pass filtering is used as the observation directly. Using Kalman filter technology to realize the on-site calibration of the rotating strapdown inertial navigation system;

建立以经过高通滤波后的导航系下的水平速度为观测量的卡尔曼滤波模型；Establish a Kalman filter model with the horizontal velocity in the navigation system after high-pass filtering as the observed quantity;

$\overset{. .}{X x} ((t t)) = = A A ((t t)) X x ((t t)) + + B B ((t t)) W W ((t t)) - - - - - - ((1111))$

系统的状态向量为：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 0 0 0]^T (13)W＝[a _x a _y ω _x ω _y ω _z 0 0 0 0 0 0 0 0] ^T (13)

其中δV_e、δV_n分别表示东向、北向的速度误差；

分别为IMU坐标系ox_s、oy_s轴加速度计零偏；ε_x、ε_y、ε_z分别为IMU坐标系ox_s、oy_s、oz_s轴陀螺的常值漂移；a_x、a_y分别为IMU坐标系ox_s、oy_s轴加速度计的白噪声误差；δK_gx、δK_gy、δK_gz分别为IMU坐标系ox_s、oy_s、oz_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; δK _gx , δK _gy , δK _gz are the scale factor errors of the IMU coordinate system ox _s , oy _s , oz _s axis gyroscope respectively; ω _x , ω _y and ω _z are the white noise errors of the IMU coordinate system ox _s , oy _s , and oz _s- axis gyro, respectively;

系统的状态转移矩阵为：The state transition matrix of the system is:

$A A = = [\begin{matrix} {F f}_{22 \times \times 22}^{11} & {f f}_{22 \times \times 33} & {\overset{~ ~}{T T}}_{22 \times \times 22} & {O o}_{22 \times \times 66} \\ {F f}_{33 \times \times 22}^{22} & {F f}_{33 \times \times 33}^{33} & {O o}_{33 \times \times 22} & {T T}_{33 \times \times 66} \\ {O o}_{88 \times \times 22} & {O o}_{88 \times \times 33} & {O o}_{88 \times \times 22} & {O o}_{88 \times \times 66} \end{matrix}] - - - - - - ((1414))$

${F f}_{22 \times \times 22}^{11} = = [\begin{matrix} \frac{{V V}_{N N}}{{R R}_{n no}} tan the tan L L & 22 {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.}}{{R R}_{n no}} tan the tan L L \\ - - ((22 {ω ω}_{ie ie} sin sin L L + + \frac{22 {V V}_{E E.}}{{R R}_{n no}} tan the tan L L)) & 00 \end{matrix}] - - - - - - ((1515))$

${F f}_{33 \times \times 22}^{22} = = [\begin{matrix} 00 & - - \frac{11}{{R R}_{m m}} \\ \frac{11}{{R R}_{n no}} & 00 \\ \frac{tan the tan L L}{{R R}_{n no}} & 00 \end{matrix}] - - - - - - ((1616))$

${F f}_{33 \times \times 33}^{33} = = [\begin{matrix} 00 & {ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.} tan the tan L L}{{R R}_{n no}} & - - (({ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{n no}})) \\ - - (({ω ω}_{ie ie} sin sin L L + + \frac{{V V}_{E E.} tan the tan L L}{{R R}_{n no}})) & 00 & - - \frac{{V V}_{N N}}{{R R}_{m m}} \\ {ω ω}_{ie ie} cos cos L L + + \frac{{V V}_{E E.}}{{R R}_{n no}} & \frac{{V V}_{N N}}{{R R}_{m m}} & 00 \end{matrix}] - - - - - - ((1717))$

${f f}_{22 \times \times 33} = = [\begin{matrix} 00 & - - {f f}_{U u} & {f f}_{N N} \\ {f f}_{U u} & 00 & {f f}_{E E.} \end{matrix}] - - - - - - ((1818))$

${\overset{~ ~}{T T}}_{22 \times \times 22} = = [\begin{matrix} {T T}_{1111} & {T T}_{1212} \\ {T T}_{21 twenty one} & {T T}_{22 twenty two} \end{matrix}] - - - - - - ((1919))$

${T T}_{33 \times \times 66} = = [\begin{matrix} - - {T T}_{1111} & - - {T T}_{1212} & {- - T T}_{1313} & - - {T T}_{1111} {ω ω}_{x x} & - - {T T}_{1212} {ω ω}_{y the y} & - - {T T}_{1313} {ω ω}_{z z} \\ - - {T T}_{21 twenty one} & - - {T T}_{22 twenty two} & - - {T T}_{23 twenty three} & - - {T T}_{21 twenty one} {ω ω}_{x x} & - - {T T}_{22 twenty two} {ω ω}_{y the y} & - - {T T}_{23 twenty three} {ω ω}_{z z} \\ - - {T T}_{3131} & - - {T T}_{3232} & - - {T T}_{3333} & - - {T T}_{3131} {ω ω}_{x x} & - - {T T}_{3232} {ω ω}_{y the y} & - - {T T}_{3333} {ω ω}_{z z} \end{matrix}] - - - - - - ((2020))$

V_E、V_N分别表示东向、北向的速度；ω_x、ω_y、ω_z分别表示陀螺的三个输入角速度；ω_ie表示地球自转角速度；R_m、R_n分别表示地球子午、卯酉曲率半径；L表示当地纬度；f_E、f_N、f_U分别表示为导航坐标系下东向、北向、天向的比力。V _E , V _N represent the eastward and northward velocities respectively; ω _x , ω _y , ω _z represent the three input angular velocities of the gyroscope; ω _ie represent the earth's rotation angular velocity; R _m , R _n represent the earth's meridian, Radius of curvature; L represents the local latitude; f _E , f _N , and f _U represent the relative forces in the east, north, and sky directions of the navigation coordinate system, respectively.

Z(t)＝H(t)X(t)+V(t) (21)Z(t)＝H(t)X(t)+V(t) (21)

系统量测矩阵为：The system measurement matrix is:

$H h = = [\begin{matrix} 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \\ 00 & 11 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 & 00 \end{matrix}] - - - - - - ((22 twenty two))$

$Z Z = = {[[{\overset{~ ~}{V V}}_{E E.} {\overset{~ ~}{V V}}_{N N}]]}^{T T} - - - - - - ((23 twenty three))$

Claims

1. rotating strapdown system field calibration method based on Digital High Pass Filter is characterized in that may further comprise the steps:

(1) utilizes global position system GPS to confirm the initial position parameters of carrier, they are bound to navigational computer;

(2) rotating strapdown system carries out gathering after the preheating data that fibre optic gyroscope and quartz accelerometer are exported;

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

(4) utilize the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process;

(5) design infinite impulse response digital high-pass filter; The carrier horizontal velocity that navigation coordinate system calculates is down carried out the high-pass filtering processing; Filtering navigation coordinate system is the Schuler period in the bearer rate down, keeps carrier owing to wave and swing the velocity deviation that motion produces;

Model is floated in estimating when (6) setting up the carrier moored condition according to the moving pedestal error equation of inertial navigation system, directly as observed quantity, utilizes Kalman Filter Technology to realize the on-site proving of rotating strapdown system with the speed that obtains after the high-pass filtering;

It is that the transposition scheme of a swing circle is that said 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; An IMU pause point p3, p8 and p4, p7 on the horizontal east orientation axle are symmetrical in the rotating shaft center; Pause point p1, p5 and p2 on the north orientation axle, p6 are symmetrical in the rotating shaft center; It is that carry out at 180 ° or 90 ° of intervals that four-position rotation and stop scheme remains rotational angle;

The said method of utilizing the spectral condition method of counting to ask for the observability degree of inertia device deviation in the IMU four-position rotation and stop process is:

Find the solution system of linear equations

AX＝b，b∈C ⁿ

If A ∈ is C ^{N * n}, || || be a kind of operator norm,

cond (A) = \{\begin{matrix} | | A | | | | A^{- 1} | |, & \det A &NotEqual; 0 \\ \infty, & \det A = 0 \end{matrix}

Claim cond (A) be matrix A about operator norm || || conditional number, about the p-norm || || _pConditional number, note is made cond _p(A), cond ₂(A) be the spectral condition number,

To discrete time-varying system:

\{\begin{matrix} X_{k + 1} = F_{k} X_{k} \\ Z_{k} = H X_{k} \end{matrix}

Bring system state equation into observation equation and obtain a set of equations:

\{\begin{matrix} Z_{0} = {HX}_{0} \\ Z_{1} = {HF}_{0} X_{0} \\ \cdot \\ \cdot \\ \cdot \\ Z_{k} = H Π_{i = 0}^{k} F_{i} X_{0} \end{matrix}

Note

O_{k} = {[\begin{matrix} H & {HF}_{0} \cdot \cdot \cdot \end{matrix} H Π_{i = 0}^{k} F_{i}]}^{T}

Then

O _kX ₀＝Z

For stational system F _kBe constant, O _kBe exactly the ornamental matrix, time-varying system is observed on sampled point and is obtained discrete time-varying system, F _kBe exactly the state-transition matrix Φ (t in the sampling period _k+ T, t _k),

O _k＝[H?HΦ(t ₁，t ₀)…HΦ(t _k，t ₀)] ^T

State is the n dimension, an observed quantity Z _kBe r dimension, r＜n wherein, the order of observation battle array H is r, carries out k time at least and observes, wherein kr>=n obtains X ₀, find the solution state X according to least square method ₀,

X_{0} = {(O_{k}^{T} O_{k})}^{- 1} O_{k}^{T} Z

Be the observation battle array, because

Be normal matrix, count cond through calculating spectral condition ₂(M), analyze stability of solution, and

{cond}_{2} (M) = \frac{\max_{λ &Element; λ (M)} | λ |}{\min_{λ &Element; λ (M)} | λ |}

In the formula, λ is the eigenwert of matrix M, eigenwert and the proper vector of further analysis matrix M, so that confirm that the observability degree of which state is better actually, the observability degree of which state is poor, but with M diagonalization at the tenth of the twelve Earthly Branches, note U ^TMU=Λ, wherein Λ=diag (λ ₁, λ ₂... λ _n), the observability degree S of state X then:

S＝abs(U[λ ₁，λ ₂，...，λ _n] ^T)

Calculate eigenwert and the proper vector of the observability matrix M of system, determine the observability degree of each state.