CN101915578A - 基于光纤捷联惯性系统测量船上任意两位置间距离的方法 - Google Patents

Abstract

本发明的目的在于提供基于光纤捷联惯性系统测量船上任意两位置间距离的方法。将两套子惯导系统分别放置在船的两个待测位置上，主惯导系统对准并处于导航状态，采集子惯导系统光纤陀螺仪和石英挠性加速度计输出的数据，分别建立以主惯导系统与两个子惯导系统的速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程，分别估计出两个子惯导系统和主惯导系统之间的距离，并矢量做差，得到两个待测位置的距离。本发明具有对准时间短、对准精度高、对器件的要求宽松等优点。

Description

基于光纤捷联惯性系统测量船上任意两位置间距离的方法

技术领域

本发明涉及的是一种用于导航领域的船舶上两位置间距离的测量方法。

背景技术

捷联惯性导航系统是将惯性传感器固联在载体上，直接测量载体的加速度和角速度等运动信息，通过导航计算机对惯性传感器采集的信息进行处理，得到载体的姿态，速度及位置等导航信息的完全自主导航设备。船上一般有两套或多套惯导系统，船本身有一套精度较高的惯导系统，称为主惯导系统。船上的附属装备自身有一套惯导系统，称为子惯导系统。捷联惯性导航系统在进入导航状态前，需要确定导航的初始条件，包括初始速度、位置和姿态等，这个过程就是初始对准。根据载体在初始对准时的运动状态可以将初始对准分为两类：静基座对准和动基座对准。传递对准技术就是在动基座条件下，利用载体上已对准的高精度主惯导的信息，采用惯性信息匹配方法确定子惯导的导航初始信息及坐标系的指向。

传递对准具有对准时间短、对准精度高、对器件的要求宽松等优点。传递对准的方法可分为两类：一类是计算参数匹配，包括速度匹配和位置匹配；另一类是测量参数匹配，包括加速度匹配、角速度匹配和姿态匹配。其中速度匹配传递对准是目前为止较为成熟的匹配方法之一，它利用主惯导和子惯导之间的速度差作为观测量。

发明内容

本发明的目的在于提供对准时间短、对准精度高、对器件要求宽松的基于光纤捷联惯性系统测量船上任意两位置间距离的方法。

本发明的目的是这样实现的：

本发明基于光纤捷联惯性系统测量船上任意两位置间距离的方法，其特征是：

(1)将两套光纤陀螺捷联惯系统s1和s2作为子惯导系统分别放置在船的两个待测位置上；

(2)将船上的高精度主惯导系统n对准并处于导航状态；

(3)将子惯导系统s1和s2预热后，分别采集光纤陀螺仪和石英挠性加速度计输出的数据；

(4)建立以主惯导系统n和s1子惯导系统s1的速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

(5)利用卡尔曼滤波技术进行状态估计，实时的估计出子惯导系统s1和主惯导系统n之间的距离r1_p；

(6)建立以主惯导系统n和s2子惯导系统的速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

(7)利用卡尔曼滤波技术进行状态估计，实时的估计出子惯导系统s2和主惯导系统n之间的距离r2_p；

(8)将得到的两套子惯导系统s1和s2和主惯导系统n之间的距离r1_p、r2_p矢量做差，得到两个子惯导系统s1和s2和之间的距离、即两个待测位置的距离。

本发明的优势在于：由于船上的环境复杂，一般很难测算船体上两个位置间的距离。本发明利用速度匹配传递对准，建立以速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程，利用卡尔曼滤波方法实时的估计出子惯导和主惯导之间的距离。传递对准具有对准时间短、对准精度高、对器件的要求宽松等优点。

附图说明

图1为本发明的光纤捷联惯性系统在船上的位置示意图；

图2为本发明的流程图；

图3为仿真得到的子惯导s1与主惯导n之间的距离的估计曲线图；

图4为仿真得到的子惯导s2与主惯导n之间的距离的估计曲线图；

图5为仿真得到的子惯导s1与子惯导s2之间的距离的估计曲线图；

图6为仿真得到的子惯导s1与子惯导s2之间的距离的误差曲线图。

具体实施方式

下面结合附图举例对本发明做更详细地描述：

结合图1～2，本发明基于光纤捷联惯性系统测量船上任意两位置间距离的方法分以下步骤：

(1)为了测量船上某两个位置之间的距离，首先在船上待测的两个位置上各放置一套光纤捷联惯导系统，用s1、s2分别表示两套子惯导系统。

(2)将两套光纤陀螺捷联惯导系统预热后，分别采集光纤陀螺仪和石英挠性加速度计输出的数据。

(3)船上的高精度主惯导系统已经对准并处于导航状态，利用船上主惯导系统输出的惯性信息，同时对s1子惯导系统和s2子惯导系统进行速度匹配传递对准。坐标系n代表主惯导的导航坐标系。利用惯导基本方程，对于主惯导有：

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

其中

为运载体相对于地球的加速度向量；fⁿ为加速度计测量的比力向量；

为地球自转角速度；

为载体运动引起的导航系相对于地球坐标系运动的角速度；Vⁿ为载体相对于地球的运动速度向量；gⁿ为重力矢量。

(4)对于s1子惯导系统，建立主惯导系统和s1子惯导系统的速度匹配误差方程，由于子捷联惯导系统s1和主惯导系统安装的位置不同，主子惯导系统会敏感到不同的加速度，用r1_p表示主惯导和子惯导s1之间的距离，则杆臂效应产生的加速度计误差为：

$\mathrm{\δ}{f}_1=\frac{d{\mathrm{\ω}}_{\mathrm{ib}}}{\mathrm{dt}}{|}_b\×{r1}_p+{\mathrm{\ω}}_{\mathrm{ib}}\×({\mathrm{\ω}}_{\mathrm{ib}}\×{r1}_p)---\left(2\right)$

其中ω_ib为陀螺仪的输出

主惯导和子惯导s1之间的速度误差方程：

$\mathrm{\δ}{\stackrel{\·}{V}}_1^n=f^n\×{\mathrm{\φ}}_1-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δ}{V^n}_1+C_b^n{\▿}^b+C_b^n\mathrm{\δ}{f}_1^b---\left(3\right)$

其中

为加速度计零偏。

主子惯导间的失准角为φ₁＝[φ_x φ_y φ_z]^T，则失准角微分方程：

${\stackrel{\·}{\mathrm{\φ}}}_1={\mathrm{\φ}}_1\×{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{in}}^n+{\mathrm{\ϵ}}^n+{\mathrm{\μ}}^n---\left(4\right)$

其中εⁿ为主惯导的陀螺漂移，μⁿ为噪声项。

一般将陀螺仪和加速度计的误差表述为随机常值和白噪声误差，则有：

${\stackrel{\·}{\mathrm{\ϵ}}}^b=0---\left(5\right)$

${\stackrel{\·}{\▿}}^b=0---\left(6\right)$

(5)建立以主惯导系统和s1子惯导系统速度误差和姿态误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程：

根据式(3)、(4)、(5)及(6)可以得到速度匹配传递对准的状态方程

$\left\{\begin{array}{c}{\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{v}}}_1^{n^{\′}}={\stackrel{\‾}{f}}^{n^{\′}}\×{\mathrm{\φ}}_1-(2{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ie}}^{n^{\′}}+{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{en}}^{n^{\′}})\×{\mathrm{\δ}\stackrel{\‾}{v}}_1^{n^{\′}}+{\▿}^{n^{\′}}+\mathrm{\η}\\ {\stackrel{\·}{\mathrm{\φ}}}_1=-{\stackrel{\‾}{\mathrm{\ω}}}_{{\mathrm{in}}^{\′}}^{n^{\′}}\×{\mathrm{\φ}}_1+{\mathrm{\ϵ}}^{n^{\′}}+{\mathrm{\μ}}^{n^{\′}}\\ {\stackrel{\·}{\▿}}^b=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\end{array}\right.---\left(7\right)$

速度匹配传递对准以主子惯导间的速度误差作为观测量，因此的速度匹配卡尔曼滤波方程如下：

状态方程： $\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(8\right)$

量测方程：Z(t)＝H(t)X(t)+v(t)    (9)

其中X(t)为t时刻系统的状态向量；A(t)和B(t)分别为系统的状态矩阵和噪声矩阵；W(t)为系统噪声向量；Z(t)表示t时刻系统的量测向量；H(t)表示系统的量测矩阵；v(t)表示系统的量测噪声；

系统的状态向量为：

${X=\left[\begin{array}{ccccccccccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {r1}_{\mathrm{px}}& {r1}_{\mathrm{py}}& {r1}_{\mathrm{pz}}\end{array}\right]}^T$

系统的白噪声向量为：

W＝[w_ax w_ay w_εx w_εy w_εz 0 0 0 0 0 0 0 0]^T

其中δV_x、δV_y分别表示东向、北向的速度误差；φ_x、φ_y、φ_z分别为东向、北向、天向失准角；

分别为X、Y、Z轴加速度计零偏；ε_x、ε_y、ε_z分别为X、Y、Z轴陀螺的常值漂移；w_ax、w_ay分别为X、Y轴加速度计的白噪声误差；w_εx、w_εy、w_εz分别为X、Y、Z轴陀螺的白噪声误差；

系统的状态转移矩阵和观测矩阵为：

$A\left(t\right)=\left[\begin{array}{cc}{A}_{5\×5}& A_{5\×8}\\ 0_{8\×5}& 0_{8\×8}\end{array}\right],$ $B\left(t\right)=\left[\begin{array}{cc}C& D\\ 0_{8\×5}& 0_{8\×5}\end{array}\right]$

其中：

$A_{5\×8}=\left[\begin{array}{cccccccc}{C}_{11}& C_{12}& 0& 0& 0& {r1}_{p1x}& {r1}_{p1y}& {r1}_{p1z}\\ C_{21}& C_{22}& 0& 0& 0& {r1}_{p2x}& {r1}_{p2y}& {r1}_{p2z}\\ 0& 0& -C_{11}& -C_{12}& -C_{13}& 0& 0& 0\\ 0& 0& -C_{21}& -C_{22}& -C_{23}& 0& 0& 0\\ 0& 0& -C_{31}& -C_{32}& {-C}_{33}& 0& 0& 0\end{array}\right]$

$C=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& C_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{21}& C_{22}& C_{23}\\ 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\end{array}\right]$

$D=\left[\begin{array}{cccc}{0}_{2\×1}& 0_{2\×1}& 0_{2\×1}& 0_{2\×1}\\ -C_{11}& -C_{12}& -C_{13}& 0_{1\×4}\\ -C_{21}& -C_{22}& -C_{23}& 0_{1\×4}\\ -C_{31}& -C_{32}& -C_{33}& 0_{1\×4}\end{array}\right]$

$\left[\begin{array}{c}{r1}_{p1}\\ {r1}_{p2}\\ {r1}_{p3}\end{array}\right]=\left[\begin{array}{ccc}{r1}_{p1x}& {r1}_{p1y}& {r1}_{p1z}\\ {r1}_{p2x}& {r1}_{p2y}& {r1}_{p2z}\\ {r1}_{p3x}& {r1}_{p3y}& {r1}_{p3z}\end{array}\right]$

$=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\left[\begin{array}{ccc}-({\mathrm{\ω}}_{\mathrm{iby}}^b\·{\mathrm{\ω}}_{\mathrm{iby}}^b+{\mathrm{\ω}}_{\mathrm{ibz}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b)& {\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{iby}}^b-{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ibz}}^b& {\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{iby}}^b\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{iby}}^b+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ibz}}^b& -({\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{ibx}}^b+{\mathrm{\ω}}_{\mathrm{ibz}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b)& {\mathrm{\ω}}_{\mathrm{iby}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b-{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ibx}}^b\\ {\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b-{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{iby}}^b& {\mathrm{\ω}}_{\mathrm{iby}}^b\·{\mathrm{\ω}}_{\mathrm{ibz}}^b+{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{ibx}}^b& -({\mathrm{\ω}}_{\mathrm{ibx}}^b\·{\mathrm{\ω}}_{\mathrm{ibx}}^b+{\mathrm{\ω}}_{\mathrm{iby}}^b\·{\mathrm{\ω}}_{\mathrm{iby}}^b)\end{array}\right]$

$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

ω_ie表示地球自转角速度；

表示当地纬度；

速度匹配传递对准取主惯导和子惯导间的速度差作为观测量，因此量测矩阵为：

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

利用卡尔曼滤波技术估计出主惯导和子惯导s1之间的距离r1_p，r1_p是以船摇摆中心为原点的向量。

(6)按照步骤(4)中的方法，建立主惯导系统和s2子惯导系统的速度匹配误差方程，用r2_p表示主惯导和子惯导s2之间的距离，主惯导和子惯导s2之间的速度误差方程：

$\mathrm{\δ}{\stackrel{\·}{V}}_2^n=f^n\×{\mathrm{\φ}}_2-(2{\mathrm{\ω}}_{\mathrm{ie}}^n+{\mathrm{\ω}}_{\mathrm{en}}^n)\×\mathrm{\δ}{V}_2^n+C_b^n{\▿}^b+C_b^n\mathrm{\δ}{f}_2^b---\left(10\right)$

失准角微分方程：

${\stackrel{\·}{\mathrm{\φ}}}_2={\mathrm{\φ}}_2\×{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{in}}^n+{\mathrm{\ϵ}}^n+{\mathrm{\μ}}^n---\left(11\right)$

(7)建立以主惯导系统和s2子惯导系统速度误差和姿态误差为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程：

根据式(10)及(11)可以得到速度匹配传递对准的状态方程

$\left\{\begin{array}{c}{\mathrm{\δ}\stackrel{\·}{\stackrel{\‾}{v}}}_2^{n^{\′}}={\stackrel{\‾}{f}}^{n^{\′}}\×{\mathrm{\φ}}_2-(2{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{ie}}^{n^{\′}}+{\stackrel{\‾}{\mathrm{\ω}}}_{\mathrm{en}}^{n^{\′}})\×{\mathrm{\δ}\stackrel{\‾}{v}}_2^{n^{\′}}+{\▿}^{n^{\′}}+\mathrm{\η}\\ {\stackrel{\·}{\mathrm{\φ}}}_2=-{\stackrel{\‾}{\mathrm{\ω}}}_{{\mathrm{in}}^{\′}}^{n^{\′}}\×{\mathrm{\φ}}_2+{\mathrm{\ϵ}}^{n^{\′}}+{\mathrm{\μ}}^{n^{\′}}\\ {\stackrel{\·}{\▿}}^b=0\\ {\stackrel{\·}{\mathrm{\ϵ}}}^b=0\end{array}\right.---\left(12\right)$

速度匹配传递对准以主子惯导间的速度误差作为观测量，因此的速度匹配卡尔曼滤波方程如下：

状态方程： $\stackrel{\·}{X}\left(t\right)=A\left(t\right)X\left(t\right)+B\left(t\right)W\left(t\right)---\left(13\right)$

量测方程：Z(t)＝H(t)X(t)+v(t)    (14)

系统的状态向量为：

$X={\left[\begin{array}{ccccccccccccc}\mathrm{\δ}{V}_x& \mathrm{\δ}{V}_y& {\mathrm{\φ}}_x& {\mathrm{\φ}}_y& {\mathrm{\φ}}_z& {\▿}_x& {\▿}_y& {\mathrm{\ϵ}}_x& {\mathrm{\ϵ}}_y& {\mathrm{\ϵ}}_z& {r2}_{\mathrm{px}}& {r2}_{\mathrm{py}}& {r2}_{\mathrm{pz}}\end{array}\right]}^T$

其中 $A\left(t\right)=\left[\begin{array}{cc}{A}_{5\×5}& A_{5\×8}\\ 0_{8\×5}& 0_{8\×8}\end{array}\right],$ $B\left(t\right)=\left[\begin{array}{cc}C& D\\ 0_{8\×5}& 0_{8\×5}\end{array}\right]$

$A_{5\×8}=\left[\begin{array}{cccccccc}{C}_{11}& C_{12}& 0& 0& 0& {r2}_{p1x}& {r2}_{p1y}& {r2}_{p1z}\\ C_{21}& C_{22}& 0& 0& 0& {r2}_{p2x}& {r2}_{p2y}& {r2}_{p2z}\\ 0& 0& -C_{11}& -C_{12}& -C_{13}& 0& 0& 0\\ 0& 0& -C_{21}& -C_{22}& -C_{23}& 0& 0& 0\\ 0& 0& -C_{31}& -C_{32}& {-C}_{33}& 0& 0& 0\end{array}\right]$

$C=\left[\begin{array}{ccccc}{C}_{11}& C_{12}& C_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{21}& C_{22}& C_{23}\\ 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}& 0_{3\×1}\end{array}\right]$

$D=\left[\begin{array}{cccc}{0}_{2\×1}& 0_{2\×1}& 0_{2\×1}& 0_{2\×1}\\ -C_{11}& -C_{12}& -C_{13}& 0_{1\×4}\\ -C_{21}& -C_{22}& -C_{23}& 0_{1\×4}\\ -C_{31}& -C_{32}& -C_{33}& 0_{1\×4}\end{array}\right]$

$C_b^n=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]$

(8)利用卡尔曼滤波技术估计出主惯导和子惯导s2之间的距离r2_p，r2_p是以船摇摆中心为原点的向量，用步骤(5)中得到的r1_p与r2_p进行矢量相减，既得到子惯导s1和子惯导s2之间的距离R，从而测得了船上安装两套子惯导位置之间的距离。

在以下的仿真条件下，对该方法进行仿真实验：

载体以正弦规律绕航向角、纵摇角和横摇角摇摆，其数学模型为：

$\left\{\begin{array}{c}\mathrm{\ψ}={\mathrm{\ψ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\ψ}}t+{\mathrm{\ψ}}_0)+K\\ \mathrm{\θ}={\mathrm{\θ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\θ}}t+{\mathrm{\θ}}_0)\\ \mathrm{\γ}={\mathrm{\γ}}_m\sin ({\mathrm{\ω}}_{\mathrm{\γ}}t+{\mathrm{\γ}}_0)\end{array}\right.$

其中：ψ，θ，γ分别表示绕航向角、纵摇角和横摇角的摇摆角度变量；ψ_m，θ_m，γ_m分别表示相应的摇摆角度幅值；ω_ψ，ω_θ，ω_γ分别表示相应的摇摆角频率；ψ₀，θ₀，γ₀分别表示相应的初相位；而ω_i＝2π/T_i，i＝ψ，θ，γ，T_i表示相应的摇摆周期；K为初始航向。仿真时取ψ_m＝5°，θ_m＝4°，γ_m＝5°，T_ψ＝8s，T_θ＝8s，T_γ＝6s，K＝0，初相位ψ₀，θ₀，γ₀取值为0°。

船的初始位置：北纬45.7796°，东经126.6705°；

初始姿态误差角：横摇误差角0.1°，纵摇误差角0.1°，方位误差角0.5°；

赤道半径：R_e＝6378393.0m；

椭圆度：e＝3.367e-3；

由万有引力可得的地球表面重力加速度：g₀＝9.78049；

地球自转角速度(弧度/秒)：7.2921158e-5；

陀螺仪常值漂移：0.01度/小时；

陀螺仪白噪声误差：0.005度/小时；

加速度计零位偏移：10^-4g₀；

加速度计白噪声误差：10^-5g₀；

主惯导和子惯导s1之间的距离：r1_px＝10m，r1_py＝10m，r1_pz＝5m；

主惯导和子惯导s2之间的距离：r2_px＝35m，r2_py＝35m，r2_pz＝10m；

采样时间和滤波周期：T＝0.1秒；

常数：π＝3.1415926；

利用本发明所述方法得到子惯导s1、s2与主惯导n之间的距离的估计曲线分别如图3、图4所示；子惯导s1与子惯导s2之间的距离的估计曲线如图5所示，子惯导s1与子惯导s2之间的距离的误差曲线图如图6所示。

Claims (1)

1.基于光纤捷联惯性系统测量船上任意两位置间距离的方法，其特征是：

(1)将两套光纤陀螺捷联惯系统s1和s2作为子惯导系统分别放置在船的两个待测位置上；

(2)将船上的高精度主惯导系统n对准并处于导航状态；

(3)将子惯导系统s1和s2预热后，分别采集光纤陀螺仪和石英挠性加速度计输出的数据；

(4)建立以主惯导系统n和s1子惯导系统s1的速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

(5)利用卡尔曼滤波技术进行状态估计，实时的估计出子惯导系统s1和主惯导系统n之间的距离r1_p；

(6)建立以主惯导系统n和s2子惯导系统的速度误差、姿态误差及杆臂长度作为状态变量的卡尔曼滤波状态方程及速度误差为量测量的量测方程；

(7)利用卡尔曼滤波技术进行状态估计，实时的估计出子惯导系统s2和主惯导系统n之间的距离r2_p；

(8)将得到的两套子惯导系统s1和s2和主惯导系统n之间的距离r1_p、r2_p矢量做差，得到两个子惯导系统s1和s2和之间的距离、即两个待测位置的距离。

Images