CN105157724B - 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 - Google Patents
一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 Download PDFInfo
- Publication number
- CN105157724B CN105157724B CN201510457943.1A CN201510457943A CN105157724B CN 105157724 B CN105157724 B CN 105157724B CN 201510457943 A CN201510457943 A CN 201510457943A CN 105157724 B CN105157724 B CN 105157724B
- Authority
- CN
- China
- Prior art keywords
- msub
- mtd
- mrow
- mtr
- mtable
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Fee Related
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
- Navigation (AREA)
Abstract
一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法有四大步骤：一、利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；二、主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；三、在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；四、滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。本发明通过滤波方法可以准确估计时间延迟误差，同时能有效抑制时间延迟对传递对准及惯性器件误差估计精度影响。
Description
技术领域
本发明涉及一种惯导系统传递对准基准信息时间延迟的估计与补偿方法，尤其涉及一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法。属于惯性技术领域。
背景技术
在传递对准过程中，子惯导将主惯导信息作为基准信息进行初始对准。子惯导得到的基准信息会因为主子惯导时钟不同步、数据的传输处理等因素而存在时间延迟。当时间延迟存在时，子惯导不进行时间延迟的估计与补偿，直接利用接收到的信息会影响对准的精度。因此实现对时间延迟的估计与补偿，会提升传递对准的精度。
使用卡尔曼滤波方法，可以有效的对时间延迟进行估计与补偿，但现有的方法没有在提升传递对准精度的同时，实现抑制时间延迟对惯性器件误差估计精度的影响。
发明内容
本发明基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟模型的观测方程，提出一种传递对准时间延迟的滤波估计与补偿方法。目的在于准确估计时间延迟误差，同时有效抑制时间延迟对传递对准精度及惯性器件误差估计精度的影响。
技术方案：本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法具体步骤如下：
步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；
步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；
步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；取游动自由方位系为子惯导的导航坐标系，所采用子惯导误差模型为：
其中δvx、δvy、δvz为速度误差，ρx、ρy、ρz为载体运动角速率矢量分量，Ωx、Ωy、Ωz为地球自转角速率矢量分量,ω表示ρ+Ω；▽x、▽y、▽z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；ψx、ψy、ψz为姿态误差，εx、εy、εz为陀螺漂移；ηx、ηy、ηz为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τηx、τηy、τηz分别为三轴向相对安装角误差的相关时间；κ为主惯导相对子惯导的延迟时间误差，τκ为延迟时间误差的相关时间。
子惯导器件误差模型：
陀螺的误差模型为
Bx、By、Bz为陀螺的零位误差项；Sgx、Sgy、Sgz为陀螺的标度误差项。Cij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素。
加速度计的误差模型为
其中Ax、Ay、Az为加速度计的零位误差项；Sax、Say、Saz为加速度计的标度误差项。
所选系统状态变量为：
X(t)＝[δvx δvy δvz ψx ψy ψz ηx ηy ηz κ Bx By Bz Sgx Sgy Sgz Ax Ay Az Sax SaySaz]T对应的系统状态方程为：
其中
wax,way,waz为加速度计零偏随机白噪声，wεx,wεy,wεz为陀螺仪漂移随机白噪声。
式中
系统观测量：
Z＝[δvx δvy δvz M23 -M13 M12]T
其中子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差
子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积
式中其他类推。
观测方程为
Z＝HX+V
其中
时间延迟量测值计算方法
考虑到闭环卡尔曼滤波可以减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进。设子惯导t时刻时间延迟估计值为相对安装角误差估计值为
1)速度量测值
2)姿态量测值
步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。
优点及功效：本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法的优点是：本发明通过滤波方法可以准确估计时间延迟误差，同时能有效抑制时间延迟对传递对准及惯性器件误差估计精度的影响。和没有时间延迟误差时相比，应用本发明滤波方法后传递对准精度以及器件主要零位、标度误差项估计精度变化可忽略，可以有效抑制基准信息时延对传递对准性能的影响。
附图说明
图1为本发明流程框图。
具体实施方式
见图1，本发明一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，该方法具体步骤如下：
步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；
步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；
步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟模型的观测方程，进行标准离散卡尔曼滤迭代结算，具体包括如下五个步骤：
(1)子惯导误差方程的建立；(2)传递对准系统状态方程的建立；(3)建立考虑时间延迟的速度加姿态匹配方法的量测方程；(4)时间延迟量测值计算方法；(5)卡尔曼滤迭代解算。
(1)子惯导误差方程的建立。取游动自由方位系为子惯导的导航坐标系，可得子惯导的误差方程为
1)速度误差方程
其中δvx、δvy、δvz为速度误差，ρx、ρy、ρz为载体运动角速率矢量分量，Ωx、Ωy、Ωz为地球自转角速率矢量分量,ω表示ρ+Ω；▽x、▽y、▽z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；
2)姿态误差方程
ψx、ψy、ψz为姿态误差，εx、εy、εz为陀螺漂移；
3)相对安装角误差方程
ηx、ηy、ηz为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τηx、τηy、τηz分别为三轴向相对安装角误差的相关时间。
4)延迟时间误差方程
κ为主惯导相对子惯导的延迟时间误差，τκ为延迟时间误差的相关时间。
5)陀螺、加速度计误差方程
一般情形下精确标定后的弹载惯组的安装误差变化较为缓慢，但零位误差、标度误差不能忽略。陀螺的误差模型为：
Bx、By、Bz为陀螺的零位误差项；Sgx、Sgy、Sgz为陀螺的标度误差项。Cij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素。
加速度计的误差模型为：
其中Ax、Ay、Az为加速度计的零位误差项；Sax、Say、Saz为加速度计的标度误差项。
(2)传递对准系统状态方程的建立。综合子惯导的误差方程，得到传递对准系统状态方程，状态变量为
X(t)＝[δvx δvy δvz ψx ψy ψz ηx ηy ηz κ Bx By Bz Sgx Sgy Sgz Ax Ay Az Sax SaySaz]T
对应的系统状态方程为：
其中
wax,way,waz为加速度计零偏随机白噪声，wεx,wεy,wεz为陀螺仪漂移随机白噪声。
式中
(3)建立考虑时间延迟的速度加姿态匹配方法的量测方程。
子惯导系统在时刻t得到主惯导系统在时刻(t-κ)的数据，速度和姿态量测方程分别为：
1)速度量测方程
子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差
传递对准速度量测模型为
Z1(t)为速度观测矢量，ξ1(t)为速度量测噪声，I为单位阵。
2)姿态量测方程
子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积
式中其他类推。
传递对准姿态量测模型为
上标T表示矩阵转置，Z2(t)为姿态观测矢量，ξ2(t)为姿态量测噪声。
(4)时间延迟量测值计算方法。
考虑到闭环卡尔曼滤波可以减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进。设子惯导t时刻时间延迟估计值为相对安装角误差估计值为
3)速度量测值
4)姿态量测值
(5)依据所建系统状态方程和观测方程，结合主、子惯导信息进行卡尔曼滤波迭代解算。
步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。
Claims (1)
1.一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法，其特征在于：该方法具体步骤如下：
步骤一：利用已经对准好的主惯导系统信息，完成子惯导系统的粗对准及导航信息的初始化；
步骤二：主、子惯导系统分别进行导航解算，主惯导的速度、姿态信息传输到子惯导的导航计算机；
步骤三：在子惯导的导航计算机中，基于常用的速度/姿态匹配模式推导滤波器观测模型，建立含时间延迟的观测方程，进行标准离散卡尔曼滤迭代解算；取游动自由方位系为子惯导的导航坐标系，所采用子惯导误差模型为：
<mfenced open = '{' close = ''>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
</msub>
<mo>=</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&delta;v</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&rho;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>y</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>&delta;v</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
<mo>+</mo>
<msub>
<mo>&dtri;</mo>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&delta;v</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&rho;</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>x</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>&delta;v</mi>
<mi>z</mi>
</msub>
<mo>+</mo>
<msub>
<mi>f</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
<mo>+</mo>
<msub>
<mo>&dtri;</mo>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&delta;</mi>
<msub>
<mover>
<mi>v</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
</msub>
<mo>=</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<msub>
<mi>&Omega;</mi>
<mi>y</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>&delta;v</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<msub>
<mi>&Omega;</mi>
<mi>x</mi>
</msub>
<mo>)</mo>
</mrow>
<msub>
<mi>&delta;v</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<msub>
<mo>&dtri;</mo>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
</msub>
<mo>=</mo>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&epsiv;</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&epsiv;</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
</msub>
<mo>=</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>&epsiv;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&eta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>x</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>x</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&eta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>y</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>y</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>&eta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>z</mi>
</msub>
<mo>=</mo>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>z</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&kappa;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mi>&kappa;</mi>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
其中δvx、δvy、δvz为速度误差，ρx、ρy、ρz为载体运动角速率矢量分量，Ωx、Ωy、Ωz为地球自转角速率矢量分量,ω表示ρ+Ω；▽x、▽y、▽z为加速度计偏置，f为导航坐标系下载体感受的比力矢量；ψx、ψy、ψz为姿态误差，εx、εy、εz为陀螺漂移；ηx、ηy、ηz为主惯导相对子惯导的安装角误差在主惯导坐标系下的投影分量，τηx、τηy、τηz分别为三轴向相对安装角误差的相关时间；κ为主惯导相对子惯导的延迟时间误差，τκ为延迟时间误差的相关时间；
子惯导器件误差模型：
陀螺的误差模型为
<mrow>
<msup>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>n</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&epsiv;</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>b</mi>
</msup>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>11</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>12</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>13</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>21</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>22</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>23</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>31</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>32</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>33</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<mo>(</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>g</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>g</mi>
<mi>y</mi>
</mrow>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>g</mi>
<mi>z</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>x</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>y</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
</mrow>
<mi>z</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>)</mo>
</mrow>
</mrow>
Bx、By、Bz为陀螺的零位误差项；Sgx、Sgy、Sgz为陀螺的标度误差项，Cij(i,j＝1,2,3)为系统子惯导捷联矩阵中的第i行j列元素；
加速度计的误差模型为
<mrow>
<msup>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>n</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<msup>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mo>&dtri;</mo>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>b</mi>
</msup>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>11</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>12</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>13</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>21</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>22</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>23</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>31</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>32</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>33</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<mo>(</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>A</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>A</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>A</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>+</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>S</mi>
<mrow>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>b</mi>
<mi>x</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>b</mi>
<mi>y</mi>
</msubsup>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>f</mi>
<mi>b</mi>
<mi>z</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>)</mo>
</mrow>
</mrow>
其中Ax、Ay、Az为加速度计的零位误差项；Sax、Say、Saz为加速度计的标度误差项；
所选系统状态变量为：
X(t)＝[δvx δvy δvz ψx ψy ψz ηx ηy ηz κ Bx By Bz Sgx Sgy Sgz Ax Ay Az Sax Say Saz]T
对应的系统状态方程为：
<mrow>
<mover>
<mi>X</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>F</mi>
<mi>X</mi>
<mo>+</mo>
<mi>G</mi>
<mi>W</mi>
</mrow>
其中
<mrow>
<mi>F</mi>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>F</mi>
<mn>11</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>12</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>13</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>14</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>21</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>22</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>F</mi>
<mn>23</mn>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>16</mn>
<mo>&times;</mo>
<mn>22</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>G</mi>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>1</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>12</mn>
<mo>&times;</mo>
<mn>22</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
<mrow>
<mi>W</mi>
<mo>=</mo>
<msup>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>a</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>a</mi>
<mi>y</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>a</mi>
<mi>z</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>&epsiv;</mi>
<mi>x</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>&epsiv;</mi>
<mi>y</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>w</mi>
<mrow>
<mi>&epsiv;</mi>
<mi>z</mi>
</mrow>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>x</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>x</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mrow>
<mi>&eta;</mi>
<mi>x</mi>
</mrow>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mfrac>
<mn>1</mn>
<msub>
<mi>&tau;</mi>
<mi>&kappa;</mi>
</msub>
</mfrac>
</mrow>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>1</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mi>T</mi>
</msup>
</mrow>
wax,way,waz为加速度计零偏随机白噪声，wεx,wεy,wεz为陀螺仪漂移随机白噪声；
式中
<mfenced open = '' close = ''>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mn>11</mn>
</msub>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&rho;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>y</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<msub>
<mi>&rho;</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&rho;</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mrow>
<mo>(</mo>
<msub>
<mi>&rho;</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<mn>2</mn>
<msub>
<mi>&Omega;</mi>
<mi>x</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mn>12</mn>
</msub>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>f</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = '' close = ''>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mn>13</mn>
</msub>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>11</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>12</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>13</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>21</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>22</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>23</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>C</mi>
<mn>31</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>32</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>C</mi>
<mn>33</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mn>14</mn>
</msub>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>f</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>f</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>f</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mrow>
<msub>
<mi>F</mi>
<mn>23</mn>
</msub>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
<mi>x</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
<mi>y</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>i</mi>
<mi>b</mi>
<mi>z</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
系统观测量：
Z＝[δvx δvy δvz M23 -M13 M12]T
其中子惯导t时刻速度为此时主惯导速度采样有时间延迟κ，速度为主、子惯导速度之差
<mrow>
<mi>&delta;</mi>
<mi>V</mi>
<mo>=</mo>
<msubsup>
<mi>V</mi>
<mi>s</mi>
<mi>t</mi>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>V</mi>
<mi>m</mi>
<mrow>
<mi>t</mi>
<mo>-</mo>
<mi>&kappa;</mi>
</mrow>
</msubsup>
<mo>&ap;</mo>
<mi>&delta;</mi>
<mi>v</mi>
<mo>+</mo>
<mi>&kappa;</mi>
<mo>&CenterDot;</mo>
<mi>f</mi>
</mrow>
子惯导t时刻捷联矩阵为导航坐标系相对于惯导本体坐标系的角速率在导航坐标系下的投影为此时主惯导姿态采样有时间延迟κ，捷联矩阵为取二者之乘积
<mrow>
<mi>M</mi>
<mo>=</mo>
<msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mover>
<mi>C</mi>
<mo>&OverBar;</mo>
</mover>
<mi>b</mi>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mi>s</mi>
<mi>t</mi>
</msubsup>
<mo>&CenterDot;</mo>
<msubsup>
<mrow>
<mo>(</mo>
<msubsup>
<mi>C</mi>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mi>n</mi>
</msubsup>
<mo>)</mo>
</mrow>
<mi>m</mi>
<mrow>
<mo>&prime;</mo>
<mi>t</mi>
<mo>-</mo>
<mi>&kappa;</mi>
</mrow>
</msubsup>
<mo>&ap;</mo>
<mo>&lsqb;</mo>
<mi>I</mi>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mrow>
<mrow>
<mo>(</mo>
<mrow>
<mi>&psi;</mi>
<mo>+</mo>
<msubsup>
<mi>C</mi>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mi>n</mi>
</msubsup>
<mi>&eta;</mi>
<mo>+</mo>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>b</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
<mo>&CenterDot;</mo>
<mi>&kappa;</mi>
</mrow>
<mo>)</mo>
</mrow>
<mo>&times;</mo>
</mrow>
<mo>)</mo>
</mrow>
<mo>&rsqb;</mo>
</mrow>
式中
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>&times;</mo>
<mo>)</mo>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&psi;</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&psi;</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&psi;</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msub>
<mi>&psi;</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
</mrow>
其他类推；
观测方程为
Z＝HX+V
其中
<mrow>
<mi>H</mi>
<mo>=</mo>
<mfenced open = '[' close = ']'>
<mtable>
<mtr>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<mi>f</mi>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>I</mi>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>3</mn>
</mrow>
</msub>
</mtd>
<mtd>
<msubsup>
<mi>C</mi>
<msub>
<mi>b</mi>
<mn>1</mn>
</msub>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<msubsup>
<mi>&omega;</mi>
<mrow>
<mi>b</mi>
<mi>n</mi>
</mrow>
<mi>n</mi>
</msubsup>
</mtd>
<mtd>
<msub>
<mn>0</mn>
<mrow>
<mn>3</mn>
<mo>&times;</mo>
<mn>12</mn>
</mrow>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
时间延迟量测值计算方法
考虑到闭环卡尔曼滤波能减少模型线性化误差的影响、准确估计系统状态变量，将上述时间延迟量测值计算过程进行改进，设子惯导t时刻时间延迟估计值为相对安装角误差估计值为
1)速度量测值
2)姿态量测值
步骤四：滤波结束后，得到时间延迟的估计值及经过补偿过后的其他状态量的估计值。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510457943.1A CN105157724B (zh) | 2015-07-30 | 2015-07-30 | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201510457943.1A CN105157724B (zh) | 2015-07-30 | 2015-07-30 | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN105157724A CN105157724A (zh) | 2015-12-16 |
| CN105157724B true CN105157724B (zh) | 2017-12-26 |
Family
ID=54798663
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201510457943.1A Expired - Fee Related CN105157724B (zh) | 2015-07-30 | 2015-07-30 | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN105157724B (zh) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105973268B (zh) * | 2016-05-06 | 2019-09-27 | 哈尔滨工程大学 | 一种基于共基座安装的传递对准精度定量评估方法 |
| FR3057349B1 (fr) * | 2016-10-11 | 2019-07-12 | Safran Electronics & Defense | Perfectionnements aux procedes d'alignement de centrale inertielle |
| CN107525524B (zh) * | 2017-09-11 | 2020-09-11 | 哈尔滨工程大学 | 一种基于三轴同步转台的惯性导航系统时延确定方法 |
| CN113311463A (zh) * | 2020-02-26 | 2021-08-27 | 北京三快在线科技有限公司 | Gps延迟时间在线补偿方法、装置、电子设备和存储介质 |
| CN112254736A (zh) * | 2020-09-15 | 2021-01-22 | 株洲菲斯罗克光电技术有限公司 | 一种惯导与里程计组合导航的时延误差的补偿方法及系统 |
| CN113124903B (zh) * | 2021-04-23 | 2024-07-30 | 中国电子科技集团公司第二十六研究所 | 传递对准下基于姿态匹配的最小二乘陀螺零偏快速估计 |
| CN116678406B (zh) * | 2023-07-31 | 2023-10-03 | 河北美泰电子科技有限公司 | 组合导航姿态信息确定方法、装置、终端设备及存储介质 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103278178A (zh) * | 2013-04-26 | 2013-09-04 | 哈尔滨工程大学 | 在传递对准中考虑传输延迟的挠曲变形测量方法 |
| CN103344251A (zh) * | 2013-06-08 | 2013-10-09 | 哈尔滨工程大学 | 一种基于速度加比力匹配的传递对准时间延迟估计方法 |
| CN103424127A (zh) * | 2013-09-03 | 2013-12-04 | 哈尔滨工程大学 | 一种速度加比力匹配传递对准方法 |
| CN104567930A (zh) * | 2014-12-30 | 2015-04-29 | 南京理工大学 | 一种能够估计和补偿机翼挠曲变形的传递对准方法 |
| CN104748761A (zh) * | 2013-12-26 | 2015-07-01 | 南京理工大学 | 基于最优姿态匹配的动基座传递对准时延补偿方法 |
-
2015
- 2015-07-30 CN CN201510457943.1A patent/CN105157724B/zh not_active Expired - Fee Related
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103278178A (zh) * | 2013-04-26 | 2013-09-04 | 哈尔滨工程大学 | 在传递对准中考虑传输延迟的挠曲变形测量方法 |
| CN103344251A (zh) * | 2013-06-08 | 2013-10-09 | 哈尔滨工程大学 | 一种基于速度加比力匹配的传递对准时间延迟估计方法 |
| CN103424127A (zh) * | 2013-09-03 | 2013-12-04 | 哈尔滨工程大学 | 一种速度加比力匹配传递对准方法 |
| CN104748761A (zh) * | 2013-12-26 | 2015-07-01 | 南京理工大学 | 基于最优姿态匹配的动基座传递对准时延补偿方法 |
| CN104567930A (zh) * | 2014-12-30 | 2015-04-29 | 南京理工大学 | 一种能够估计和补偿机翼挠曲变形的传递对准方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN105157724A (zh) | 2015-12-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105157724B (zh) | 一种基于速度加姿态匹配的传递对准时间延迟估计与补偿方法 | |
| CN102486377B (zh) | 一种光纤陀螺捷联惯导系统初始航向的姿态获取方法 | |
| CN101514900B (zh) | 一种单轴旋转的捷联惯导系统初始对准方法 | |
| CN106342284B (zh) | 一种飞行载体姿态确定方法 | |
| CN100516775C (zh) | 一种捷联惯性导航系统初始姿态确定方法 | |
| CN103900565B (zh) | 一种基于差分gps的惯导系统姿态获取方法 | |
| CN104374388B (zh) | 一种基于偏振光传感器的航姿测定方法 | |
| CN104698485B (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
| CN101074881B (zh) | 一种月球探测器软着陆阶段惯性导航方法 | |
| CN101893445A (zh) | 摇摆状态下低精度捷联惯导系统快速初始对准方法 | |
| CN102519485B (zh) | 一种引入陀螺信息的二位置捷联惯性导航系统初始对准方法 | |
| CN104748761B (zh) | 基于最优姿态匹配的动基座传递对准时延补偿方法 | |
| CN101963512A (zh) | 船用旋转式光纤陀螺捷联惯导系统初始对准方法 | |
| CN103941274B (zh) | 一种导航方法及导航终端 | |
| CN102706366A (zh) | 一种基于地球自转角速率约束的sins初始对准方法 | |
| CN103148868B (zh) | 匀速直航下基于多普勒计程仪地理系测速误差估计的组合对准方法 | |
| CN103674064B (zh) | 捷联惯性导航系统的初始标定方法 | |
| CN103175545A (zh) | 惯导系统速度加部分角速度匹配抗干扰快速传递对准方法 | |
| CN101900573B (zh) | 一种实现陆用惯性导航系统运动对准的方法 | |
| CN105698822A (zh) | 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法 | |
| CN103256943A (zh) | 一种在单轴旋转捷联惯导系统中刻度因数误差的补偿方法 | |
| CN102853837A (zh) | 一种mimu和gnss信息融合的方法 | |
| CN106482746A (zh) | 一种用于混合式惯导系统的加速度计内杆臂标定与补偿方法 | |
| CN103245357A (zh) | 一种船用捷联惯导系统二次快速对准方法 | |
| CN105606093B (zh) | 基于重力实时补偿的惯性导航方法及装置 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| C06 | Publication | ||
| PB01 | Publication | ||
| C10 | Entry into substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20171226 Termination date: 20190730 |
|
| CF01 | Termination of patent right due to non-payment of annual fee |