CN103278163A

CN103278163A - Nonlinear-model-based SINS/DVL (strapdown inertial navigation system/doppler velocity log) integrated navigation method

Info

Publication number: CN103278163A
Application number: CN2013101954402A
Authority: CN
Inventors: 高伟; 王文佳; 于飞; 徐博; 池姗姗; 肖永平; 陈春; 郭宇; 杨建�; 田学林
Original assignee: Harbin Engineering University
Current assignee: Harbin Engineering University
Priority date: 2013-05-24
Filing date: 2013-05-24
Publication date: 2013-09-04

Abstract

The invention discloses a SINS/DVL combined navigation method based on a nonlinear model. According to the working principle of a strapdown inertial navigation system and a Doppler log, a SINS nonlinear velocity, attitude and position error based on a quaternion are established model, and determine the error model of DVL; use the error model of the two systems to establish the state equation of the system, and take the difference between the actual speed measured by SINS and DVL as the quantity measurement, and establish the measurement equation of the system; the actual Discretize the continuous system to obtain a discrete nonlinear model that is easy to calculate; initialize the system, use the discrete nonlinear model, and calculate the sampling points and corresponding weights through the Unscented transformation; Time update and measurement update for trace Kalman filter. The SINS/DVL integrated navigation system in the present invention can effectively utilize the information of each subsystem to learn from each other's strengths and complement each other, so that the total accuracy of positioning is greatly improved. At the same time, the nonlinear model of the SINS/DVL integrated navigation system is used to estimate the UKF, which can effectively reduce System positioning error, better realize the precise positioning of the navigation system.

Description

A SINS/DVL Integrated Navigation Method Based on Nonlinear Model

技术领域 technical field

本发明属于导航技术领域，涉及一种基于非线性模型的SINS/DVL组合导航方法。 The invention belongs to the technical field of navigation, and relates to a SINS/DVL combined navigation method based on a nonlinear model. the

背景技术 Background technique

人类对海洋开发、海洋资源开放和利用日益加深，水下航行器的研究和发展日趋活跃。导航定位是影响水下航行器发展的关键技术，也是提高水下作业能力的瓶颈问题。由于水下导航系统具有工作时间长、环境复杂、信息源少等特点，若采用单一导航方法，其导航精度及可靠性无法满足需求。如果将捷联惯导系统(Strapdown Inertial Navigation System，SINS)和多普勒测速仪(Doppler Velocity Log，DVL)等系统适当地组合，便可取长补短，大大提高导航精度，这便是组合导航方法。在传统的SINS线性误差模型由于忽略了高阶项，不能精确描述组合导航系统的非线性特征和SINS姿态误差的变化，滤波过程中对于位置和速度的估计精度会随时间而持续降低，在中低精度导航系统中尤为明显，因而建立了基于四元数的SINS/DVL非线性模型，并利用无迹卡尔曼(UKF)滤波的方法对组合系统的误差进行估计。非线性模型的SINS/DVL组合导航具有以下优势：由于SINS/DVL组合导航系统能有效地利用各子系统的信息，相互取长补短，使得定位的总精度大大提高；SINS/DVL系统不需要外部提供任何信息，具有很好的隐蔽性和抗干扰性；考虑SINS/DVL组合导航系统的非线性模型，可以降低系统定位误差。 Human beings are increasingly deepening the development of the ocean, the opening and utilization of ocean resources, and the research and development of underwater vehicles are becoming more and more active. Navigation and positioning is a key technology that affects the development of underwater vehicles, and it is also a bottleneck problem in improving the ability of underwater operations. Since the underwater navigation system has the characteristics of long working hours, complex environment, and few information sources, if a single navigation method is adopted, its navigation accuracy and reliability cannot meet the requirements. If the strapdown inertial navigation system (Strapdown Inertial Navigation System, SINS) and Doppler Velocity Log (DVL) and other systems are properly combined, they can learn from each other and greatly improve the navigation accuracy. This is the integrated navigation method. Because the traditional SINS linear error model ignores the high-order items, it cannot accurately describe the nonlinear characteristics of the integrated navigation system and the change of the SINS attitude error. During the filtering process, the estimation accuracy of the position and velocity will continue to decrease with time. It is especially obvious in the low-precision navigation system, so the SINS/DVL nonlinear model based on quaternion is established, and the error of the combined system is estimated by the method of Unscented Kalman (UKF) filter. The SINS/DVL integrated navigation of the nonlinear model has the following advantages: Since the SINS/DVL integrated navigation system can effectively use the information of each subsystem and learn from each other, the overall positioning accuracy is greatly improved; the SINS/DVL system does not need any external Information, has good concealment and anti-interference; considering the nonlinear model of the SINS/DVL integrated navigation system, the system positioning error can be reduced. the

发明内容 Contents of the invention

本发明为了解决现有的SINS线性误差模型由于忽略了高阶项，不能精确描述组合导航系统的非线性特征和SINS姿态误差的变化，致使滤波过程中对于位置和速度的估计精度会随时间而持续降低的问题，本发明提供了一种基于非线性模型的SINS/DVL组合导航方法。其技术方案如下： The present invention aims to solve the problem that the existing SINS linear error model cannot accurately describe the nonlinear characteristics of the integrated navigation system and the change of the SINS attitude error due to ignoring the high-order items, so that the estimation accuracy of the position and velocity in the filtering process will decrease with time. To solve the problem of continuous reduction, the present invention provides a SINS/DVL integrated navigation method based on a nonlinear model. Its technical scheme is as follows:

一种基于非线性模型的SINS/DVL组合导航方法，它包括如下步骤： A kind of SINS/DVL integrated navigation method based on nonlinear model, it comprises the steps:

步骤一：建立SINS的非线性误差模型，首先将需要用到的导航坐标系做如下定义： Step 1: Establish the nonlinear error model of SINS, first define the navigation coordinate system to be used as follows:

i——惯性坐标系 i——inertial coordinate system

e——地球坐标系 e——Earth coordinate system

b——载体坐标系 b——carrier coordinate system

p——计算得出的平台坐标系 p——the calculated platform coordinate system

n——与东北天地理坐标系重合的导航坐标系 n——Navigation coordinate system that coincides with the Northeast Heaven geographic coordinate system

通过理想惯导比力方程的速度微分方程以及捷联系统实际的速度微分方程，可以推导出四元数表示的速度误差方程： Through the speed differential equation of the ideal inertial navigation specific force equation and the actual speed differential equation of the strapdown system, the speed error equation represented by the quaternion can be derived:

$δ δ {\overset{\cdot &Center Dot;}{V V}}^{n no} = = ((I I - - C C (({Q Q}_{p p}^{n no})))) C C (({Q Q}_{b b}^{p p})) {f f}^{b b} - - ((22 {ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times δ δ {V V}^{n no} - - ((22 δ δ {ω ω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times {V V}^{n no}$

$- - ((22 δ δ {ω ω}_{ie ie}^{n no} + + {δω δω}_{en en}^{n no})) \times \times δ δ {V V}^{n no} + + C C (({Q Q}_{b b}^{p p})) {&dtri; &dtri;}^{b b} + + {δg δg}^{n no}$

其中：Vⁿ为载体在n系下的理想速度，

是n系下的地球自转角速度，

为n系相对于e系的角速度在n系下的投影。f^b为b系下的比力。 Among them: V ⁿ is the ideal velocity of the carrier in the n system,

is the earth's rotation angular velocity in n system,

is the projection of the angular velocity of the n system relative to the e system under the n system. f ^b is the specific force under the b system.

式中：

分别代表p系到n系以及b系到p系的转换矩阵：

Q_{p}^{n} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}

为p系到n系的转换四元数，

Q_{b}^{p} = {[\begin{matrix} q_{b 0} & q_{b 1} & q_{b 2} & q_{b 3} \end{matrix}]}^{T}

为b系到p系的转换四元数。“δ·表示载体的理想值与实际测量值间的误差。g为地球重力加速度，gⁿ为n系下的重力加速度，

为b系下的加速度计误差向量。 In the formula:

Represent the conversion matrices from p-series to n-series and b-series to p-series respectively:

Q_{p}^{no} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}

is the conversion quaternion from the p system to the n system,

Q_{b}^{p} = {[\begin{matrix} q_{b 0} & q_{b 1} & q_{b 2} & q_{b 3} \end{matrix}]}^{T}

It is the conversion quaternion from b system to p system. "δ· represents the error between the ideal value of the carrier and the actual measured value. g is the acceleration of gravity on the earth, g ⁿ is the acceleration of gravity under the n system,

is the accelerometer error vector under the b system.

四元数姿态误差方程： Quaternion attitude error equation:

${\overset{\cdot &Center Dot;}{Q Q}}_{p p}^{n no} = = B B ((I I - - C C (({Q Q}_{n no}^{p p})))) {ω ω}_{in in}^{n no} - - BC BC (({Q Q}_{b b}^{p p})) {ϵ ϵ}^{b b}$

其中：

表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，ε^b为陀螺仪误差向量在b系下的投影，B为关于四元数的4×3维矩阵。 in:

Indicates the projection of the rotation angular velocity of the n system relative to the i system under the n system, is the direction cosine matrix from the n system to the p system, ε ^b is the projection of the gyroscope error vector in the b system, and B is a 4×3 dimensional matrix about the quaternion.

位置误差方程： Position error equation:

其中：R_M和R_N分别表示地球的子午圈曲率半径和卯酉圈曲率半径，λ为载体的经度，为载体的纬度，V_x和V_y分别为运载体的东向和北向速度。 Among them: R _M and R _N respectively represent the meridian circle curvature radius and the Maoyou circle curvature radius of the earth, λ is the longitude of the carrier, is the latitude of the carrier, V _x and V _y are the eastward and northward velocities of the carrier, respectively.

步骤二、建立DVL误差模型，DVL测量的是载体相对海底或某一流层的速度V_d和偏流角Δ，其测量误差主要由刻度系数误差δC，速度偏移误差δV_d和偏流角误差δΔ产生，δC为随机常数，δV_d和δΔ可用一阶马尔可夫过程表示，则相应的误差模型为： Step 2: Establish a DVL error model. DVL measures the velocity V _d and the drift angle Δ of the carrier relative to the seabed or a certain flow layer. The measurement error is mainly caused by the scale coefficient error δC, the velocity offset error δV _d and the drift angle error δΔ. , δC is a random constant, δV _d and δΔ can be expressed by a first-order Markov process, then the corresponding error model is:

$δ δ {\overset{\cdot &Center Dot;}{V V}}_{d d} = = - - {β β}_{d d} δ δ {V V}_{d d} + + {ω ω}_{d d}$

$δ δ {\overset{\cdot &Center Dot;}{V V}}_{Δ Δ} = = - - {β β}_{Δ Δ} δ δ {V V}_{Δ Δ} + + {ω ω}_{Δ Δ}$

$δ δ \overset{\cdot \cdot}{C C} = = 00$

其中：β_d，β_Δ表示速度偏移误差和偏流角误差相关时间的倒数，ω_d和ω_Δ为激励白噪声。 Among them: β _d , β _Δ represent the reciprocal of the relative time of velocity offset error and bias angle error, ω _d and ω _Δ are excitation white noise.

步骤三、利用步骤一获得的SINS系统模型和步骤二获得的DVL误差模型确定系统的非线性状态方程和量测方程； Step 3, using the SINS system model obtained in step 1 and the DVL error model obtained in step 2 to determine the nonlinear state equation and measurement equation of the system;

系统状态方程为： The state equation of the system is:

$\overset{\cdot \cdot}{X x} ((t t)) = = f f [[X x ((t t)),, t t]] + + Γ Γ ((t t)) W W ((t t))$

其中：X(t)为系统状态量，W(t)为系统噪声向量，Γ(t)为系统噪声阵。 Among them: X(t) is the system state quantity, W(t) is the system noise vector, Γ(t) is the system noise matrix. the

取SINS系统的解算速度和DVL测量速度之差作为观测量，并考虑测量噪声，则系统量测方程为： Taking the difference between the solution speed of the SINS system and the DVL measurement speed as the observation quantity, and considering the measurement noise, the system measurement equation is:

Z(t)＝h[X(t)，t]+V(t) Z(t)＝h[X(t), t]+V(t)

其中：Z(t)为系统量测量，K_V表示考虑偏流角的航迹向，V(t)为量测噪声向量。 Among them: Z(t) is the measurement of the system quantity, K _V represents the track direction considering the drift angle, and V(t) is the measurement noise vector.

步骤四、为了避免系统模型线性化而产生的线性化误差，因而将UKF方法应用于基于四元数的组合导航系统。为了进行UKF方法，需要先对系统模型进行离散化，将步骤三中的系统状态方程和量测方程离散化的系统模型为： Step 4. In order to avoid the linearization error caused by the linearization of the system model, the UKF method is applied to the integrated navigation system based on quaternions. In order to carry out the UKF method, the system model needs to be discretized first, and the system model for discretizing the system state equation and measurement equation in step 3 is:

X_k+1＝f[X_k，k]+Γ_kW_k X _k+1 = f[X _k , k]+Γ _k W _k

Z_k＝h[X_k，k]+V_k Z _k ＝h[X _k , k]+V _k

其中：X_k和Z_k分别为系统在t_k时刻的状态向量以及量测向量。W_k和V_k分别为系统噪声阵和量测噪声阵，且均值为零。Q(k)和R(k)分别为系统噪声协方差阵和量测噪声协方差阵。 Among them: X _k and Z _k are the state vector and measurement vector of the system at time t _k , respectively. W _k and V _k are the system noise matrix and the measurement noise matrix respectively, and the mean value is zero. Q(k) and R(k) are system noise covariance matrix and measurement noise covariance matrix respectively.

步骤五、基于步骤四中的离散化数学模型，利用Unscented变换，开始进行UKF滤波算法。系统的状态X_k为n维随机变量，其均值为

方差为P_x，那么2n+1个n维Sigma点χ_i为： Step five, based on the discretized mathematical model in step four, use the Unscented transformation to start the UKF filtering algorithm. The state X _k of the system is an n-dimensional random variable whose mean is

The variance is P _x , then 2n+1 n-dimensional Sigma points χ _i are:

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{X x},, i i = = 00$

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{X x} + + {((\sqrt{((n no + + λ λ)) {P P}_{x x}}))}_{i i},, i i = = 1,2 1,2,, . . . . . .,, n no$

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{X x} - - {((\sqrt{((n no + + λ λ)) {P P}_{x x}}))}_{i i},, i i = = n no + + 11,, n no + + 22,, . . . . . .,, 22 n no$

其中：λ为尺度因子，

表示方根矩阵的第i列。 Among them: λ is the scale factor,

Indicates the ith column of the square root matrix.

各点所对应的权值为： The weights corresponding to each point are:

${W W}_{i i}^{m m} = = \frac{λ λ}{n no + + λ λ},, i i = = 00$

${W W}_{i i}^{c c} = = \frac{λ λ}{n no + + λ λ} + + ((11 - - {α α}^{22} + + β β)),, i i = = 00$

${W W}_{i i}^{m m} {= = W W}_{i i}^{c c} = = \frac{λ λ}{22 ((n no + + λ λ))},, i i = = 1,2 1,2,, . . . . . .,, 22 n no$

其中：W_i ^c为均值权系数，W_i ^m为方差的权系数。α用来确定Sigma点的分布，β用于加入随机变量X_k的验前信息。 Among them: W _i ^c is the mean weight coefficient, W _i ^m is the variance weight coefficient. α is used to determine the distribution of Sigma points, and β is used to add the prior information of the random variable X _k .

步骤六、根据步骤五中构造的Sigma点按照离散化非线性模型进行UKF的时间更新： Step 6. According to the Sigma point constructed in step 5, the time update of UKF is performed according to the discrete nonlinear model:

一步预测样本点的均值和方差： One-step prediction of the mean and variance of the sample points:

χ_i，k|k-1＝f(χ_k-1) χ _{i, k|k-1} = f(χ _k-1 )

${\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {W W}_{i i}^{m m} {χ χ}_{i i,, k k | | k k - - 11}$

${P P}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {W W}_{i i}^{c c} [[{χ χ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k | | k k - - 11}]] {[[{χ χ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k | | k k - - 11}]]}^{T T} + + {Q Q}_{k k - - 11}$

其中：χ_i，k|k-1为t_k时刻预测的第i个样本点，

为t_k时刻预测的样本点的均值，P_k|k-1为t_k时刻预测的样本点的方差。 Among them: χ _{i, k|k-1} is the i-th sample point predicted at time t _k ,

is the mean value of sample points predicted at time t _k , and P _k|k-1 is the variance of sample points predicted at time t _k .

一步预测观测值的均值和方差： One-step prediction of the mean and variance of observations:

γ_i，k|k-1＝h(χ_i，k|k-1) γ _{i, k|k-1} ＝h(χ _{i, k|k-1} )

${\overset{^^}{Z Z}}_{k k | | k k - - 11} = = {Σ Σ}_{i i = = 00}^{22 n no} {W W}_{i i}^{m m} {γ γ}_{i i,, k k | | k k - - 11}$

${P P}_{{Z Z}_{k k} {Z Z}_{k k}} = = {Σ Σ}_{i i = = 00}^{{22 n no}_{x x}} {W W}_{i i}^{c c} [[{γ γ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{Z Z}}_{k k | | k k - - 11}]] {[[{γ γ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{Z Z}}_{k k | | k k - - 11}]]}^{T T} + + {R R}_{k k}$

其中：下标‘k’表示t_k时刻，γ_i，k|k-1为第i个量测值，

为预测的量测值的均值，

为预测的量测值的方差。 Among them: the subscript 'k' indicates the time t _k , γ _{i, k|k-1} is the i-th measurement value,

is the mean of the predicted measurements,

is the variance of the predicted measurements.

预测的协方差阵： Predicted covariance matrix:

${P P}_{{x x}_{k k} {z z}_{k k}} = = {Σ Σ}_{i i = = 00}^{{22 n no}_{x x}} {W W}_{i i}^{c c} [[{χ χ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k | | k k - - 11}]] {[[{γ γ}_{i i,, k k | | k k - - 11} - - {\overset{^^}{Z Z}}_{k k | | k k - - 11}]]}^{T T}$

其中：

为预测的样本点与量测值间的协方差阵。 in:

is the covariance matrix between predicted sample points and measured values.

步骤七、进行UKF的量测更新： Step 7. Perform UKF measurement update:

${K K}_{k k} = = {P P}_{{x x}_{k k} {z z}_{k k}} {P P}_{{z z}_{k k} {z z}_{k k}}^{- - 11}$

${\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k} = = {\overset{^^}{\overset{&OverBar; &OverBar;}{χ χ}}}_{k k | | k k - - 11} + + {K K}_{k k} (({z z}_{k k} - - {\overset{^^}{z z}}_{k k | | k k - - 11}))$

${P P}_{k k} = = {P P}_{k k | | k k - - 11} - - {K K}_{k k} {P P}_{{z z}_{k k} {z z}_{k k}} {K K}_{k k}^{T T}$

其中：K_k为滤波增益矩阵，为样本点均值的估计值，P_k为滤波误差协方差阵。 Where: K _k is the filter gain matrix, is the estimated value of the sample point mean, and P _k is the filter error covariance matrix.

与现有技术相比，本发明的有益效果： Compared with prior art, beneficial effect of the present invention:

本发明提出了一种基于非线性模型的SINS/DVL组合导航方法，即能够充分利用了两个系统的量测信息对系统误差进行估计，提高了载体的定位精度，又能够有效地避免由于线性化处理而引入的线性化误差。发明的非线性模型组合导航方法具有精度高、可靠性强等显著优点。 The present invention proposes a SINS/DVL integrated navigation method based on a nonlinear model, which can make full use of the measurement information of the two systems to estimate the system error, improve the positioning accuracy of the carrier, and effectively avoid the The linearization error introduced by the processing. The invented nonlinear model integrated navigation method has significant advantages such as high precision and strong reliability. the

附图说明 Description of drawings

图1是本发明基于非线性模型的SINS/DVL组合导航方法滤波估计的经度误差曲线； Fig. 1 is the longitude error curve that the present invention is based on the SINS/DVL integrated navigation method filter estimation of nonlinear model;

图2是本发明基于非线性模型的SINS/DVL组合导航方法滤波估计的纬度误差曲线； Fig. 2 is the latitude error curve that the present invention is based on the SINS/DVL integrated navigation method filter estimation of nonlinear model;

图3是本发明基于非线性模型的SINS/DVL组合导航方法滤波估计的东向速度误差曲线。 Fig. 3 is the eastward velocity error curve estimated by filtering of the nonlinear model-based SINS/DVL integrated navigation method of the present invention. the

图4是本发明基于非线性模型的SINS/DVL组合导航方法滤波估计的北向速度误差曲线。 Fig. 4 is the northward velocity error curve estimated by filtering of the nonlinear model-based SINS/DVL integrated navigation method of the present invention. the

具体实施方式 Detailed ways

下面结合附图和具体实施方式对本发明的技术方案作进一步详细地说明。 The technical solutions of the present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments. the

一种基于非线性模型的SINS/DVL组合导航方法，它由以下步骤实现： A kind of SINS/DVL integrated navigation method based on nonlinear model, it is realized by the following steps:

i——惯性坐标系 i——inertial coordinate system

e——地球坐标系 e——Earth coordinate system

b——载体坐标系 b——carrier coordinate system

${δ δ \overset{\cdot \cdot}{V V}}^{n no} = = {\overset{\cdot \cdot}{\overset{~ ~}{V V}}}^{n no} - - {\overset{\cdot \cdot}{V V}}^{n no}$

$= = (({\overset{~ ~}{f f}}^{n no} + + {\overset{~ ~}{g g}}^{n no} - - (({22 \overset{~ ~}{ω ω}}_{ie ie}^{n no} + + {\overset{~ ~}{ω ω}}_{en en}^{n no})) \times \times {\overset{~ ~}{V V}}^{n no})) - - (({f f}^{n no} + + {g g}^{n no} - - (({22 ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times {V V}^{n no}))$

$= = {f f}^{p p} - - {f f}^{n no} - - ((22 {\overset{~ ~}{ω ω}}_{ie ie}^{n no} + + {\overset{~ ~}{ω ω}}_{en en}^{n no})) \times \times {δV δV}^{n no} - - (({δω δω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times (({V V}^{n no} + + δ δ {V V}^{n no})) + + {&dtri; &dtri;}^{p p} + + δ δ {g g}^{n no}$

$= = ((I I - - C C (({Q Q}_{p p}^{n no})))) {f f}^{p p} - - ((22 {ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times δ δ {V V}^{n no} - - ((22 δ δ {ω ω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times (({V V}^{n no} + + {δV δV}^{n no})) + + {&dtri; &dtri;}^{p p} + + {δg δ g}^{n no}$

$= = ((I I - - C C (({Q Q}_{p p}^{n no})))) C C (({Q Q}_{b b}^{p p})) {f f}^{b b} - - ((22 {ω ω}_{ie ie}^{n no} + + {ω ω}_{en en}^{n no})) \times \times {δV δV}^{n no} - - ((22 {δω δω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times {V V}^{n no}$

$- - ((22 δ δ {ω ω}_{ie ie}^{n no} + + δ δ {ω ω}_{en en}^{n no})) \times \times {δV δV}^{n no} + + C C (({Q Q}_{b b}^{p p})) {&dtri; &dtri;}^{b b} + + δ δ {g g}^{n no}$

其中：Vⁿ为载体在n系下的理想速度，

是n系下的地球自转角速度，为n系相对于 e系的角速度在n系下的投影。f^b为b系下的比力。 Among them: V ⁿ is the ideal velocity of the carrier in the n system,

is the earth's rotation angular velocity in n system, is the projection of the angular velocity of the n system relative to the e system under the n system. f ^b is the specific force under the b system.

式中：

分别代表p系到n系以及b系到p系的转换矩阵： In the formula:

$C C (({Q Q}_{p p}^{n no})) = = [\begin{matrix} {q q}_{00}^{22} + + {q q}_{11}^{22} - - {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{11} {q q}_{22} - - {q q}_{00} {q q}_{33})) & 22 (({q q}_{11} {q q}_{33} + + {q q}_{00} {q q}_{22})) \\ 22 (({q q}_{11} {q q}_{22} + + {q q}_{00} {q q}_{33})) & {q q}_{00}^{22} - - {q q}_{11}^{22} + + {q q}_{22}^{22} - - {q q}_{33}^{22} & 22 (({q q}_{22} {q q}_{33} - - {q q}_{00} {q q}_{11})) \\ 22 (({q q}_{11} {q q}_{33} - - {q q}_{00} {q q}_{22})) & 22 (({q q}_{22} {q q}_{33} + + {q q}_{00} {q q}_{11})) & {q q}_{00}^{22} - - {q q}_{11}^{22} {- - q q}_{22}^{22} + + {q q}_{33}^{22} \end{matrix}]$

$C C (({Q Q}_{b b}^{p p})) = = [\begin{matrix} {q q}_{}^{b b 00} + + {q q}_{}^{b b 11} - - {q q}_{}^{b b 22} - - {q q}_{b b 33}^{22} & 22 (({q q}_{b b 11} {q q}_{b b 22} - - {q q}_{b b 00} {q q}_{b b 33})) & 22 (({q q}_{b b 11} {q q}_{b b 33} + + {q q}_{b b 00} {q q}_{b b 22})) \\ 22 (({q q}_{b b 11} {q q}_{b b 22} + + {q q}_{b b 00} {q q}_{b b 33})) & {q q}_{}^{b b 00} - - {q q}_{}^{b b 11} + + {q q}_{}^{b b 22} - - {q q}_{b b 33}^{22} & 22 (({q q}_{b b 22} {q q}_{b b 33} - - {q q}_{b b 00} {q q}_{b b 11})) \\ 22 (({q q}_{b b 11} {q q}_{b b 33} - - {q q}_{b b 00} {q q}_{b b 22})) & 22 (({q q}_{b b 22} {q q}_{b b 33} + + {q q}_{b b 00} {q q}_{b b 11})) & {q q}_{}^{b b 00} - - {q q}_{}^{b b 11} - - {q q}_{}^{b b 22} + + {q q}_{b b 33}^{22} \end{matrix}]$

其中： $Q_{p}^{n} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}$ 为p系到n系的转换四元数， $Q_{b}^{p} = {[\begin{matrix} q_{b 0} & q_{b 1} & q_{b 2} & q_{b 3} \end{matrix}]}^{T}$ 为b系到p系的转换四元数。 in: $Q_{p}^{no} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}$ is the conversion quaternion from the p system to the n system, $Q_{b}^{p} = {[\begin{matrix} q_{b 0} & q_{b 1} & q_{b 2} & q_{b 3} \end{matrix}]}^{T}$ It is the conversion quaternion from b system to p system.

$δ δ {ω ω}_{ie ie}^{n no} = = {\overset{~ ~}{ω ω}}_{ie ie}^{n no} - - {ω ω}_{ie ie}^{n no},, δ δ {ω ω}_{en en}^{n no} = = {\overset{~ ~}{ω ω}}_{en en}^{n no} - - {ω ω}_{en en}^{n no},, {\overset{~ ~}{f f}}^{n no} = = {f f}^{p p} + + {&dtri; &dtri;}^{p p},, δ δ {g g}^{n no} = = {\overset{~ ~}{g g}}^{n no} - - {g g}^{n no}$

式中：“～”为载体的实际测量值，δ·表示载体的理想值与实际测量值间的误差。gⁿ为n系下的重力加速度，为b系下的加速度计误差向量。 In the formula: "~" is the actual measured value of the carrier, and δ· represents the error between the ideal value of the carrier and the actual measured value. g ⁿ is the gravitational acceleration in the n system, is the accelerometer error vector under the b system.

四元数姿态误差方程： Quaternion attitude error equation:

${\overset{\cdot \cdot}{Q Q}}_{p p}^{n no} = = {\overset{\cdot \cdot}{Q Q}}_{p p}^{n no} = = \frac{11}{22} [[{ω ω}_{np np}^{p p} \times \times]] {Q Q}_{p p}^{n no} = = B B \overset{\cdot &Center Dot;}{ψ ψ}$

$= = B B [[((I I - - {C C}_{n no}^{p p})) {ω ω}_{in in}^{n no} - - {ϵ ϵ}^{p p}]]$

$= = B B ((I I - - C C (({Q Q}_{n no}^{p p})))) {ω ω}_{in in}^{n no} - - BC BC (({Q Q}_{b b}^{p p})) {ϵ ϵ}^{b b}$

其中：

表示n系相对于i系的旋转角速度在n系下的投影，为n系到p系的方向余弦矩阵，ε^b为陀螺仪误差向量在b系下的投影。 in:

Indicates the projection of the rotation angular velocity of the n system relative to the i system under the n system, is the direction cosine matrix from the n system to the p system, and ε ^b is the projection of the gyroscope error vector in the b system.

其中： $B = \frac{1}{2} [\begin{matrix} - q_{1} & - q_{2} & - q_{3} \\ q_{0} & - q_{3} & q_{2} \\ q_{2} & q_{0} & - q_{1} \\ - q_{2} & q_{1} & q_{0} \end{matrix}]$ in: $B = \frac{1}{2} [\begin{matrix} - q_{1} & - q_{2} & - q_{3} \\ q_{0} & - q_{3} & q_{2} \\ q_{2} & q_{0} & - q_{1} \\ - q_{2} & q_{1} & q_{0} \end{matrix}]$

位置误差方程： Position error equation:

$δ δ \overset{\cdot &Center Dot;}{C C} = = 00$

系统状态方程为： The state equation of the system is:

$\overset{\cdot &Center Dot;}{X x} ((t t)) = = f f [[X x ((t t)),, t t]] + + Γ Γ ((t t)) W W ((t t))$

其中：

为系统状态量，

W (t) = [\begin{matrix} 0 & 0 & {&dtri;}_{x} & {&dtri;}_{y} & - B ϵ_{x} & - B ϵ_{y} & - {Bϵ}_{z} & ω_{d} & ω_{Δ} & 0 \end{matrix}]

为系统噪声向量，Γ(t)为系统噪声阵。 in:

is the system state quantity,

W (t) = [\begin{matrix} 0 & 0 & {&dtri;}_{x} & {&dtri;}_{the y} & - B ϵ_{x} & - B ϵ_{the y} & - {Bϵ}_{z} & ω_{d} & ω_{Δ} & 0 \end{matrix}]

is the system noise vector, Γ(t) is the system noise matrix.

Z(t)＝h[X(t)，t]+V(t) Z(t)＝h[X(t), t]+V(t)

其中： $Z (t) = [\begin{matrix} δ V_{x} - V_{y} \cdot (ψ_{z} + δΔ) - \sin K_{V} \cdot δ V_{d} - V_{x} \cdot δC + n_{x} \\ δ V_{y} + V_{x} \cdot (ψ_{z} + δΔ) - \cos K_{V} \cdot δ V_{d} - V_{y} \cdot δC + n_{y} \end{matrix}]$ 为系统量测量，K_V表示考虑偏流角的航迹向，V(t)＝[n_x n_y]^T为量测噪声向量。 in: $Z (t) = [\begin{matrix} δ V_{x} - V_{the y} &Center Dot; (ψ_{z} + δΔ) - \sin K_{V} &Center Dot; δ V_{d} - V_{x} &Center Dot; δ C + {no}_{x} \\ δ V_{the y} + V_{x} &Center Dot; (ψ_{z} + δΔ) - \cos K_{V} &Center Dot; δ V_{d} - V_{the y} \cdot δ C + {no}_{the y} \end{matrix}]$ is the system quantity measurement, K _V represents the track direction considering the drift angle, V(t)=[n _x _ny ] ^T is the measurement noise vector.

X_k+1＝f[X_k，k]+Γ_kW_k X _k+1 = f[X _k , k]+Γ _k W _k

Z_k＝h[X_k，k]+V_k Z _k ＝h[X _k , k]+V _k

其中：X_k和Z_k分别为系统在t_k时刻的状态向量以及量测向量。W_k～N(0，Q(k))为零均值高斯白噪声，V_k～N(0，R(k))也为零均值高斯白噪声。Q(k)和R(k)分别为系统噪声协方差阵和量测噪声协方差阵。 Among them: X _k and Z _k are the state vector and measurement vector of the system at time t _k , respectively. W _k ~ N(0, Q(k)) is zero-mean white Gaussian noise, and V _k ~ N(0, R(k)) is also zero-mean white Gaussian noise. Q(k) and R(k) are system noise covariance matrix and measurement noise covariance matrix respectively.

The variance is P _x , then 2n+1 n-dimensional Sigma points χ _i are:

${χ χ}_{i i} = = \overset{&OverBar; &OverBar;}{X x},, i i = = 00$

其中：λ为尺度因子，表示方根矩阵的第i列。 Among them: λ is the scale factor, Indicates the ith column of the square root matrix.

各点所对应的权值为： The weights corresponding to each point are:

${W W}_{i i}^{m m} = = \frac{λ λ}{n no + + λ λ},, i i = = 00$

步骤六、根据步骤五中构造的Sigma点按照离散化非线性模型进行UKF的时间更新： Step 6. According to the Sigma points constructed in step 5, the time update of UKF is performed according to the discrete nonlinear model:

χ_i，k|k-1＝f(χ_k-1) χ _{i, k|k-1} = f(χ _k-1 )

其中：χ_i，k|k-1为t_k时刻预测的第i个样本点，

γ_i，k|k-1＝h(χ_i，k|k-1) γ _{i, k|k-1} ＝h(χ _{i, k|k-1} )

其中：下标‘k’表示t_k时刻，γ_i，k|k-1为第i个量测值，

为预测的量测值的均值，

is the mean of the predicted measurements,

is the variance of the predicted measurements.

预测的协方差阵： Predicted covariance matrix:

其中：为预测的样本点与量测值间的协方差阵。 in: is the covariance matrix between predicted sample points and measured values.

步骤七、进行UKF的量测更新： Step 7. Perform UKF measurement update:

其中：K_k为滤波增益矩阵，

为样本点均值的估计值，P_k为滤波误差协方差阵。 Where: K _k is the filter gain matrix,

is the estimated value of the sample point mean, and P _k is the filter error covariance matrix.

本发明提供的基于非线性模型的SINS/DVL组合导航方法具有以下优点： The SINS/DVL combined navigation method based on the nonlinear model provided by the present invention has the following advantages:

通过组合导航的方式，充分利用了两个系统的量测信息对系统误差进行估计，提高了载体的定位精度：基于四元数的方法建立了系统的非线性误差模型，进而推导出系统的状态以及量测方程，并通过UKF的方法对非线性模型进行滤波估计。由于未进行线性化处理，避免了线性化误差的引入，有效减小定位误差。 Through the way of combined navigation, the measurement information of the two systems is fully used to estimate the system error, which improves the positioning accuracy of the carrier: the nonlinear error model of the system is established based on the quaternion method, and then the state of the system is deduced As well as the measurement equation, the nonlinear model is filtered and estimated by the method of UKF. Since no linearization processing is performed, the introduction of linearization errors is avoided, and positioning errors are effectively reduced. the

为了进一步说明所述方法的效果，在以下条件下进行了仿真，仿真结果如图1、图2、图3与图4所示，并对其进行了分析。 In order to further illustrate the effect of the method, a simulation is carried out under the following conditions, and the simulation results are shown in Fig. 1 , Fig. 2 , Fig. 3 and Fig. 4 , and are analyzed. the

仿真条件： Simulation conditions:

初始航向45°，做匀速直线运动，航速为4m/s； The initial heading is 45°, with a uniform linear motion and a speed of 4m/s;

海洋环境模拟为：横摇6°，摇摆周期10s；纵摇3°，摇摆周期8s；艏摇5°，摇摆周期6s；初始相位均为0； The marine environment simulation is: roll 6°, roll period 10s; pitch 3°, roll period 8s; yaw 5°, roll period 6s; initial phase is 0;

SINS惯性器件性能参数如下：陀螺常值漂移为0.01°/h，加速度计偏置为10^-4g；多普勒测速仪速度偏移误差为(1′)²，相关时间为5min； The performance parameters of the SINS inertial device are as follows: the gyro constant drift is 0.01°/h, the accelerometer bias is 10 ^-4 g; the Doppler velocimeter velocity offset error is (1′) ² , and the correlation time is 5 minutes;

偏流角误差为0.001，相关时间为15min。 The bias angle error is 0.001, and the correlation time is 15min. the

分析结果 Analysis results

由图1可看出，经度误差稳定在20m以内；由图2可看出，纬度误差在15m以内；由图3可看出，东向速度误差范围是0.02m/s～0.1m/s；由图4可看出，北向速度误差范围为-0.03m/s～0.03m/s。由此可见，组合导航滤波器效果显著，较好的提高了导航精度。 It can be seen from Figure 1 that the longitude error is stable within 20m; it can be seen from Figure 2 that the latitude error is within 15m; it can be seen from Figure 3 that the eastward velocity error ranges from 0.02m/s to 0.1m/s; It can be seen from Figure 4 that the northward speed error range is -0.03m/s～0.03m/s. It can be seen that the effect of the integrated navigation filter is remarkable, and the navigation accuracy is better improved. the

以上所述，仅为本发明较佳的具体实施方式，本发明的保护范围不限于此，任何熟悉本技术领域的技术人员在本发明披露的技术范围内，可显而易见地得到的技术方案的简单变化或等效替换均落入本发明的保护范围内。 The above is only a preferred specific embodiment of the present invention, and the scope of protection of the present invention is not limited thereto. Any person familiar with the technical field within the technical scope disclosed in the present invention can obviously obtain the simplicity of the technical solution. Changes or equivalent replacements all fall within the protection scope of the present invention. the

Claims

1. the SINS/DVL Combinated navigation method based on nonlinear model is characterized in that, comprises the steps:

Step 1: set up the nonlinearity erron model of SINS, the navigation coordinate of at first needs being used system does as giving a definition:

I---inertial coordinates system

E---terrestrial coordinate system

B---carrier coordinate system

P---the platform coordinate system that calculates

N---the navigation coordinate system that overlaps with the geographical coordinate system in sky, northeast

By the speed differential equation of desirable inertial navigation specific force equation and the speed differential equation of strapdown system reality, derive the velocity error equation that hypercomplex number is represented:

δ {\overset{\cdot}{V}}^{n} = (I - C (Q_{p}^{n})) C (Q_{b}^{p}) f^{b} - (2 ω_{ie}^{n} + ω_{en}^{n}) \times δ V^{n}

- ({2 δω}_{ie}^{n} + {δω}_{en}^{n}) \times V^{n} - (2 {δω}_{ie}^{n} + {δω}_{en}^{n}) \times {δV}^{n} + C (Q_{b}^{p}) {&dtri;}^{b} + {δg}^{n}

Wherein: V ⁿBe the ideal velocity of carrier under n system, Be the rotational-angular velocity of the earth under the n system,

Be angular velocity the projection under n system of n system with respect to e system, f ^bBe the specific force under the b system;

In the formula:

Q_{p}^{n} = {[\begin{matrix} q_{0} & q_{1} & q_{2} & q_{3} \end{matrix}]}^{T}

For p is tied to the conversion hypercomplex number that n is,

Q_{b}^{p} = {[\begin{matrix} q_{b 0} & q_{b 1} & q_{b 2} & q_{b 3} \end{matrix}]}^{T}

For b is tied to the conversion hypercomplex number that p is,

Represent p respectively and be tied to the transition matrix that n system and b are tied to p system,

δ ω_{ie}^{n} = {\tilde{ω}}_{ie}^{n} - ω_{ie}^{n}, δ ω_{en}^{n} = {\tilde{ω}}_{en}^{n} - ω_{en}^{n}, {\tilde{f}}^{n} = f^{p} + {&dtri;}^{p}, δ g^{n} = {\tilde{g}}^{n} - g^{n}

In the formula: "～" is the actual measured value of carrier, and δ represents the ideal value of carrier and the error between actual measured value, g ⁿBe the acceleration of gravity under the n system,

Be the accelerometer error vector under the b system;

The hypercomplex number attitude error equations:

{\overset{\cdot}{Q}}_{p}^{n} = B (I - C (Q_{n}^{p})) ω_{in}^{n} - BC (Q_{b}^{p}) ϵ^{b}

Wherein:

The projection of angular velocity of rotation under n system that expression n system with respect to i is,

For n is tied to the direction cosine matrix that p is, ε ^bBe the projection of gyro error vector under b system, B is 4 * 3 dimension matrixes about hypercomplex number,

The site error equation:

Wherein: R _MAnd R _NRadius of curvature of meridian and the radius of curvature in prime vertical of representing the earth respectively, λ are the longitude of carrier,

Be the latitude of carrier, V _xAnd V _yBe respectively east orientation and the north orientation speed of carrier;

Step 2, set up the DVL error model, that DVL measures is the speed V of the relative seabed of carrier or a certain fluid layer _dWith the drift angle Δ, its measuring error is mainly by calibration factor error delta C, velocity shifts error delta V _dProduce with drift angle error delta Δ, δ C is arbitrary constant, δ V _dCan represent that with the δ Δ then corresponding error model is with first-order Markov process;

δ {\overset{\cdot}{V}}_{d} = - β_{d} δ V_{d} + ω_{d}

δ {\overset{\cdot}{V}}_{Δ} = - β_{Δ} δ V_{Δ} + ω_{Δ}

δ \overset{\cdot}{C} = 0

Wherein: β _d, β _ΔRepresent the velocity shifts sum of errors drift angle error inverse of correlation time, ω _dAnd ω _ΔBe the excitation white noise;

The DVL error model that step 3, the SINS system model that utilizes the step 1 acquisition and step 2 obtain is determined nonlinear state equation and the measurement equation of system;

System state equation is:

\overset{\cdot}{X} (t) = f [X (t), t] + Γ (t) W (t)

Wherein: X (t) is system state amount, and W (t) is the system noise vector, and Γ (t) is the system noise acoustic matrix;

Get the difference of the computing speed of SINS system and DVL measuring speed as observed quantity, and consider to measure noise, then the system measurements equation is:

Z(t)＝h[X(t)，t]+V(t)

Wherein: Z (t) measures K for system quantities _VThe course made good of drift angle is considered in expression, and V (t) is the measurement noise vector;

Step 4, with the system model of the system state equation in the step 3 and measurement equation discretize be:

X _k+1＝f[X _k，k]+Γ _kW _k

Z _k＝h[X _k，k]+V _k

Wherein: X _kAnd Z _kThe system that is respectively is at t _kState vector constantly and measurement vector, W _kAnd V _kBe respectively system noise acoustic matrix and measurement noise battle array, and average is zero, Q (k) and R (k) are respectively system noise covariance matrix and measurement noise covariance matrix;

Step 5, based on the discretize mathematical model in the step 4, utilize the Unscented conversion, begin to carry out the UKF filtering algorithm, the state X of system _kBe the n n-dimensional random variable n, its average is

Variance is P _x, 2n+1 n ties up Sigma point χ so _iFor:

χ_{i} = \overset{&OverBar;}{X}, i = 0

χ_{i} = \overset{&OverBar;}{X} + {(\sqrt{(n + λ) P_{x}})}_{i}, i = 1,2, . . ., n

χ_{i} = \overset{&OverBar;}{X} - {(\sqrt{(n + λ) P_{x}})}_{i}, i = n + 1, n + 2, . . ., 2 n

Wherein: λ is scale factor,

The i row of expression sqrtm;

The corresponding weights of each point are:

{W_{i}}^{m} = \frac{λ}{n + λ}, i = 0

{W_{i}}^{c} = \frac{λ}{n + λ} + (1 - α^{2} + β), i = 0

{W_{i}}^{m} {= W}_{i}^{c} = \frac{λ}{2 (n + λ)}, i = 1,2, . . ., 2 n

Wherein: W _i ^cBe average weight coefficient, W _i ^mWeight coefficient for variance; α is used for determining the distribution that Sigma is ordered, and β is used for adding stochastic variable X _kEmpirical prior information;

Step 6, upgrade according to the time that the discretize nonlinear model carries out UKF according to the Sigma point of constructing in the step 5:

The average of one-step prediction sample point and variance:

χ _i，k|k-1＝f(χ _k-1)

{\hat{\overset{&OverBar;}{χ}}}_{k | k - 1} = Σ_{i = 0}^{2 n} {W_{i}}^{m} χ_{i, k | k - 1}

P_{k | k - 1} = Σ_{i = 0}^{2 n} {W_{i}}^{c} [χ_{i, k | k - 1} - {\hat{\overset{&OverBar;}{χ}}}_{k | k - 1}] {[χ_{i, k | k - 1} - {\hat{\overset{&OverBar;}{χ}}}_{k | k - 1}]}^{T} + Q_{k - 1}

Wherein: χ _{I, k|k-1}Be t _kI sample point of moment prediction,

Be t _kThe average of the sample point of moment prediction, P _K|k-1Be t _kThe variance of the sample point of moment prediction;

The average of one-step prediction observed reading and variance:

γ _i，k|k-1＝h(χ _i，k|k-1)

{\hat{Z}}_{k | k - 1} = Σ_{i = 0}^{2 n} {W_{i}}^{m} γ_{i, k | k - 1}

P_{Z_{k} Z_{k}} = Σ_{i = 0}^{{2 n}_{x}} {W_{i}}^{c} [γ_{i, k | k - 1} - {\hat{Z}}_{k | k - 1}] {[γ_{i, k | k - 1} - {\hat{Z}}_{k | k - 1}]}^{T} + R_{k}

Wherein: subscript ' k ' expression t _kConstantly, γ _{I, k|k-1}Be i measuring value,

Be the average of measuring value of prediction, The variance of measuring value for prediction;

The covariance matrix of prediction:

P_{x_{k} z_{k}} = Σ_{i = 0}^{{2 n}_{x}} {W_{i}}^{c} [χ_{i, k | k - 1} - {\hat{\overset{&OverBar;}{χ}}}_{k | k - 1}] {[γ_{i, k | k - 1} - {\hat{Z}}_{k | k - 1}]}^{T}

Wherein:

Be the sample point of prediction and the covariance matrix between measuring value;

Step 7, the measurement of carrying out UKF are upgraded:

K_{k} = P_{x_{k} z_{k}} P_{z_{k} z_{k}}^{- 1}

{\hat{\overset{&OverBar;}{χ}}}_{k} = {\hat{\overset{&OverBar;}{χ}}}_{k | k - 1} + K_{k} (z_{k} - {\hat{z}}_{k | k - 1})

P_{k} = P_{k | k - 1} - K_{k} P_{z_{k} z_{k}} K_{k}^{T}

Wherein: K _kBe the filter gain matrix, Be the estimated value of sample point average, P _kBe the filtering error covariance matrix.

2. the SINS/DVL Combinated navigation method based on nonlinear model according to claim 1 is characterized in that, the system quantities measurement is in the described step 3:

Z (t) = [\begin{matrix} V_{Ix} - V_{dx} \\ V_{Iy} - V_{dy} \end{matrix}] = [\begin{matrix} V_{Ix} + {δV}_{x} - (1 + δC) (V_{d} + {δV}_{d}) \sin (K_{V} + ψ_{z} + δΔ) + n_{x} \\ V_{Iy} + {δV}_{y} - (1 + SC) (V_{d} + {δV}_{d}) \cos (K_{V} + ψ_{z} + δΔ) + n_{y} \end{matrix}]

= [\begin{matrix} {δV}_{x} - V_{y} \cdot (ψ_{z} + δΔ) - \sin K_{V} \cdot {δV}_{d} - V_{x} \cdot δC + n_{x} \\ {δV}_{y} + V_{x} \cdot (ψ_{z} + δΔ) - \cos K_{V} \cdot {δV}_{d} - V_{y} \cdot δC + n_{y} \end{matrix}]

Wherein: V _IxAnd V _IyActual east orientation and north orientation speed for SINS.