CN108088443A - 一种定位定向设备速度补偿方法 - Google Patents

一种定位定向设备速度补偿方法 Download PDF

Info

Publication number
CN108088443A
CN108088443A CN201611037069.7A CN201611037069A CN108088443A CN 108088443 A CN108088443 A CN 108088443A CN 201611037069 A CN201611037069 A CN 201611037069A CN 108088443 A CN108088443 A CN 108088443A
Authority
CN
China
Prior art keywords
mrow
mtd
msub
msubsup
mtr
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.)
Granted
Application number
CN201611037069.7A
Other languages
English (en)
Other versions
CN108088443B (zh
Inventor
马涛
朱红
李永峰
石志兴
王根
郭元江
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Automation Control Equipment Institute BACEI
Original Assignee
Beijing Automation Control Equipment Institute BACEI
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beijing Automation Control Equipment Institute BACEI filed Critical Beijing Automation Control Equipment Institute BACEI
Priority to CN201611037069.7A priority Critical patent/CN108088443B/zh
Publication of CN108088443A publication Critical patent/CN108088443A/zh
Application granted granted Critical
Publication of CN108088443B publication Critical patent/CN108088443B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Other Investigation Or Analysis Of Materials By Electrical Means (AREA)
  • Navigation (AREA)

Abstract

本发明属于车辆定位导航技术领域，具体涉及一种定位定向设备速度补偿方法。本发明的速度补偿方法，包括以下步骤：建立卡尔曼滤波模型；***初始化；天向和水平位移分量计算；东向和北向位移分量计算；等效速度计算，并更新卡尔曼滤波模型信息；对***进行卡尔曼滤波计算；对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正。本发明需要解决现有定位定向设备在航向机动过程中会出现定位定向误差的累积，最终影响导航定位的精度的技术问题，在计算速度量测信息时考虑了侧向速度，对其进行了有效的补偿，有利于减小在航向机动过程中出现的定位定向误差累积，提高了定位定向设备性能，保证了导航的精度。

Description

一种定位定向设备速度补偿方法
技术领域
本发明属于车辆定位导航技术领域，具体涉及一种定位定向设备速度补偿方法。
背景技术
在陆用车辆定位导航时，采用惯性导航与里程计组成定位定向设备是一种十分通用的手段，能够获得较为满意的性能。相比于采用卫星导航组成的组合导航***，具有全自主，全天候，不受外界信息干扰的优点。在广泛应用的同时，对定位定向设备性能要求也随之提升。
现有定位定向方法中，普遍采用里程计速度信息与惯导信息进行滤波计算，兼容零速修正技术，以期获得较高的定位定向精度。但是实际上，在航向机动过程中经常会出现意外的定位定向误差，定位定向误差会逐渐积累，最终影响导航定位的精度。
发明内容
本发明需要解决的技术问题为：现有定位定向方法中，定位定向设备在航向机动过程中会出现定位定向误差的累积，最终影响导航定位的精度。
本发明的技术方案如下所述：
一种定位定向设备速度补偿方法，包括以下步骤：
步骤1、建立卡尔曼滤波模型
定义坐标系如下：
n系：导航坐标系oxnynzn，为东北天地理坐标系，xn轴指向东，yn轴指向北，zn轴指向天；
b系：载体坐标系oxbybzb，为右前上右坐标系，xb轴指向载体的右方，yb轴指向载体的前方，zb轴指向载体的上方；
取状态向量X为
X＝[δVe,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx,Ry,Rz]T
其中，
[δVe,δVn,δVu]为速度误差矢量，δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差；
[φe,φn,φu]为姿态误差矢量，φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角；
[δλ,δL,δh]为位置误差矢量，δλ为经度误差、δL为纬度误差、δh为高度误差；
[▽x,▽y,▽z]为加速度计零偏矢量，▽x为xb轴加速度计零偏量、▽y为yb轴加速度计零偏量、▽z为zb轴加速度计零偏量；
[εx,εy,εz]为陀螺漂移矢量，εx为xb轴陀螺漂移量、εy为yb轴陀螺漂移量、εz为zb轴陀螺漂移量；
[φax,δKD,φaz]为里程计误差矢量，φax为xb轴安装误差、δKD为里程计刻度系数误差、φaz为zb轴安装误差；
Δt：为惯导/里程计速度间的时间延迟；
[Rx,Ry,Rz]为杆臂矢量，Rx为xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂；
状态方程为：
其中，G为***噪声矩阵，W为***噪声，A为***状态矩阵，
为从载体坐标系oxbybzb到导航坐标系oxnynzn的坐标变换矩阵，通过导航结算得到；
ωie为地球自转角速率，RM和RN分别为地球子午圈和卯酉圈半径，L为纬度，Vu为天向速度，Vn为北向速度，Ve为东向速度，fe、fn和fu分别为东北天向的等效加速度计测量值
量测方程为：
Z＝HX+V
其中，Z为量测量，H为量测矩阵，V为量测噪声；
分别为等效速度在东向、北向和天向的分量；
为坐标变换矩阵的第i行第j列元素，i＝1,2,3；j＝1,2,3；
VD为里程计输出等效速度；
为陀螺测量值向量；
分别为东北天向速度的一阶导数；
步骤2、***初始化
对惯导***进行对准，并开始惯性导航解算，获得姿态角[θ,γ,ψ]T，速度和[λ,φ,h]T，同时通过里程计获得位移ΔS；
ΔS＝KD·Npluse为标量，KD为里程计刻度系数，Npluse为里程计输出脉冲；
所述对准方法采用静基座对准或动基座对准方法；
步骤3、天向和水平位移分量计算
根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量；
为3×3矩阵，则里程计输出位移的天向分量为：
水平分量为：
其中为的第3行第2列的元素；
步骤4、东向和北向位移分量计算
根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量；具体方法如下：
其中，为程计位移的东向分量，为程计位移的北向分量；
步骤5、等效速度计算，并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度，具体公式如下：
其中，为等效速度，ΔSn(t)为ΔSn在时刻t的值，Te为滤波周期，Tn为导航解算周期；
更新***矩阵A、观测矩阵H，计算量测量Z；
步骤6、对***进行卡尔曼滤波计算；
步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正；
其中，姿态误差为φ＝[X(3) X(4) X(5)]T，
则惯导姿态阵误差修正公式为I为单位矩阵；
惯导速度误差为δV＝[X(0) X(1) X(2)]T，
则速度误差修正方法为Vn＝Vn-δV；
惯导位置误差修正方法为
λ＝λ-X(6)
L＝L-X(7)
h＝h-X(8)
里程计刻度系数误差为Kd＝X(15)，
则里程计刻度系数误差修正方法为KODO＝KODO×(1+Kd)；
各修正量使用完毕后相应的状态量应置为零。
本发明的有益效果为：
本发明的方法，在计算速度量测信息时充分考虑了在航向机动过程中造成的车辆的侧向速度，对其进行了有效的补偿，有利于减小定位定向设备在航向机动过程中出现的定位定向误差累积，提高了定位定向设备性能，保证了导航的精度。
具体实施方式
定义坐标系如下：
n系：导航坐标系oxnynzn，为东北天地理坐标系，xn轴指向东，yn轴指向北，zn轴指向天；
b系：载体坐标系oxbybzb，为右前上右坐标系，xb轴指向载体的右方，yb轴指向载体的前方，zb轴指向载体的上方。
本发明的方法具体包括以下步骤：
步骤1、建立卡尔曼滤波模型
取状态向量X为
X＝[δVe,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx,Ry,Rz]T
其中，
[δVe,δVn,δVu]为速度误差矢量，δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差；
[φe,φn,φu]为姿态误差矢量，φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角；
[δλ,δL,δh]为位置误差矢量，δλ为经度误差、δL为纬度误差、δh为高度误差；
[▽x,▽y,▽z]为加速度计零偏矢量，▽x为xb轴加速度计零偏量、▽y为yb轴加速度计零偏量、▽z为zb轴加速度计零偏量；
[εx,εy,εz]为陀螺漂移矢量，εx为xb轴陀螺漂移量、εy为yb轴陀螺漂移量、εz为zb轴陀螺漂移量；
[φax,δKD,φaz]为里程计误差矢量，φax为xb轴安装误差、δKD为里程计刻度系数误差、φaz为zb轴安装误差；
Δt：为惯导/里程计速度间的时间延迟；
[Rx,Ry,Rz]为杆臂矢量，Rx为xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂。
状态方程为：
其中，G为***噪声矩阵，W为***噪声，A为***状态矩阵，
为从载体坐标系oxbybzb到导航坐标系oxnynzn的坐标变换矩阵，通过导航结算得到。
ωie为地球自转角速率，RM和RN分别为地球子午圈和卯酉圈半径，L为纬度，Vu为天向速度，Vn为北向速度，Ve为东向速度，fe、fn和fu分别为东北天向的等效加速度计测量值
量测方程为：
Z＝HX+V
其中，Z为量测量，H为量测矩阵，V为量测噪声；
分别为等效速度在东向、北向和天向的分量；
为坐标变换矩阵的第i行第j列元素，i＝1,2,3；j＝1,2,3。
VD为里程计输出等效速度；
为陀螺测量值向量；
分别为东北天向速度的一阶导数；
步骤2、***初始化
对惯导***进行对准，并开始惯性导航解算，获得姿态角[θ,γ,ψ]T，速度和[λ,φ,h]T，同时通过里程计获得位移ΔS。
ΔS＝KD·Npluse为标量，KD为里程计刻度系数，Npluse为里程计输出脉冲。
所述对准方法采用静基座对准或动基座对准方法。
步骤3、天向和水平位移分量计算
根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量。
为3×3矩阵，则里程计输出位移的天向分量为：
水平分量为：
其中为的第3行第2列的元素。
步骤4、东向和北向位移分量计算
根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量。具体方法如下：
其中，为程计位移的东向分量，为程计位移的北向分量。
步骤5、等效速度计算，并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度，具体公式如下：
其中，为等效速度，ΔSn(t)为ΔSn在时刻t的值，Te为滤波周期，Tn为导航解算周期。
更新***矩阵A、观测矩阵H，计算量测量Z。
步骤6、对***进行卡尔曼滤波计算。
步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正。
其中，姿态误差为φ＝[X(3) X(4) X(5)]T，
则惯导姿态阵误差修正公式为I为单位矩阵。
惯导速度误差为δV＝[X(0) X(1) X(2)]T，
则速度误差修正方法为Vn＝Vn-δV。
惯导位置误差修正方法为
λ＝λ-X(6)
L＝L-X(7)
h＝h-X(8)
里程计刻度系数误差为Kd＝X(15)，
则里程计刻度系数误差修正方法为KODO＝KODO×(1+Kd)。
各修正量使用完毕后相应的状态量应置为零。

Claims (1)

1.一种定位定向设备速度补偿方法，其特征在于：包括以下步骤：
步骤1、建立卡尔曼滤波模型
定义坐标系如下：
n系：导航坐标系oxnynzn，为东北天地理坐标系，xn轴指向东，yn轴指向北，zn轴指向天；
b系：载体坐标系oxbybzb，为右前上右坐标系，xb轴指向载体的右方，yb轴指向载体的前方，zb轴指向载体的上方；
取状态向量X为
X＝[δVe,δVn,δVu,φe,φn,φu,δλ,δL,δh,▽x,▽y,▽z,εx,εy,εz,φax,δKD,φaz,Δt,Rx,Ry,Rz]T
其中，
[δVe,δVn,δVu]为速度误差矢量，δVe为东向速度误差、δVn为北向速度误差、δVu为天向速度误差；
[φe,φn,φu]为姿态误差矢量，φe为东向姿态误差角、φn为北向姿态误差角、φu为天向姿态误差角；
[δλ,δL,δh]为位置误差矢量，δλ为经度误差、δL为纬度误差、δh为高度误差；
[▽x,▽y,▽z]为加速度计零偏矢量，▽x为xb轴加速度计零偏量、▽y为yb轴加速度计零偏量、▽z为zb轴加速度计零偏量；
[εx,εy,εz]为陀螺漂移矢量，εx为xb轴陀螺漂移量、εy为yb轴陀螺漂移量、εz为zb轴陀螺漂移量；
[φax,δKD,φaz]为里程计误差矢量，φax为xb轴安装误差、δKD为里程计刻度系数误差、φaz为zb轴安装误差；
Δt：为惯导/里程计速度间的时间延迟；
[Rx,Ry,Rz]为杆臂矢量，Rx为xb轴方向杆臂、Ry为yb轴方向杆臂、Rz为zb轴方向杆臂；
状态方程为：
<mrow> <mover> <mi>X</mi> <mo>&amp;CenterDot;</mo> </mover> <mo>=</mo> <mi>A</mi> <mi>X</mi> <mo>+</mo> <mi>G</mi> <mi>W</mi> </mrow>
其中，G为***噪声矩阵，W为***噪声，A为***状态矩阵，
<mrow> <mi>A</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msub> <mi>A</mi> <mn>1</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>2</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>3</mn> </msub> </mtd> <mtd> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>A</mi> <mn>4</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>5</mn> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>6</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>A</mi> <mn>7</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mi>A</mi> <mn>8</mn> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>3</mn> </mrow> </msub> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>7</mn> </mrow> </msub> </mtd> </mtr> <mtr> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>13</mn> <mo>&amp;times;</mo> <mn>22</mn> </mrow> </msub> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>1</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>)</mo> <mo>/</mo> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>+</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>E</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mo>(</mo> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> <mo>)</mo> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mo>(</mo> <mrow> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> </mrow> <mo>)</mo> <mo>+</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <msub> <mi>A</mi> <mn>2</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>u</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>f</mi> <mi>n</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>u</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>f</mi> <mi>n</mi> </msub> </mrow> </mtd> <mtd> <msub> <mi>f</mi> <mi>e</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>3</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mn>2</mn> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>u</mi> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> </mrow> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>n</mi> </msub> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>n</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>)</mo> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>V&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>V</mi> <mi>u</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>2</mn> <msub> <mi>V</mi> <mi>e</mi> </msub> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mn>2</mn> </msubsup> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mn>2</mn> </msubsup> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>4</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>5</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>6</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>sin</mi> <mi> </mi> <mi>L</mi> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>e</mi> </mrow> </msub> <mi>cos</mi> <mi> </mi> <mi>L</mi> <mo>+</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <msup> <mi>sec</mi> <mn>2</mn> </msup> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>7</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mn>1</mn> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow>
<mrow> <msub> <mi>A</mi> <mn>8</mn> </msub> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>tan</mi> <mi> </mi> <mi>L</mi> <mi> </mi> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> </mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msub> <mi>V</mi> <mi>e</mi> </msub> <mi>sec</mi> <mi> </mi> <mi>L</mi> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>N</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <msub> <mi>V</mi> <mi>n</mi> </msub> <mo>/</mo> <msup> <mrow> <mo>(</mo> <msub> <mi>R</mi> <mi>M</mi> </msub> <mo>+</mo> <mi>h</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> </mtable> </mfenced> </mrow>
为从载体坐标系oxbybzb到导航坐标系oxnynzn的坐标变换矩阵，通过导航结算得到；
ωie为地球自转角速率，RM和RN分别为地球子午圈和卯酉圈半径，L为纬度，Vu为天向速度，Vn为北向速度，Ve为东向速度，fe、fn和fu分别为东北天向的等效加速度计测量值
量测方程为：
Z＝HX+V
其中，Z为量测量，H为量测矩阵，V为量测噪声；
<mrow> <mi>Z</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>e</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>n</mi> </msub> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msub> <mi>&amp;delta;V</mi> <mi>u</mi> </msub> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <msubsup> <mi>V</mi> <mi>u</mi> <mi>n</mi> </msubsup> <mo>-</mo> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>u</mi> </mrow> <mi>n</mi> </msubsup> </mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
<mrow> <mi>H</mi> <mo>=</mo> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>1</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>e</mi> </msub> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mrow> <mo>(</mo> <msup> <mi>V</mi> <mi>n</mi> </msup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> </mtd> <mtd> <msub> <mn>0</mn> <mrow> <mn>3</mn> <mo>&amp;times;</mo> <mn>9</mn> </mrow> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>2</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>n</mi> </msub> </mtd> <mtd> <mrow> <mo>-</mo> <mrow> <mo>(</mo> <msubsup> <mi>&amp;omega;</mi> <mrow> <mi>i</mi> <mi>b</mi> </mrow> <mi>b</mi> </msubsup> <mo>&amp;times;</mo> <mo>)</mo> </mrow> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow></mrow> </mtd> <mtd> <mrow> <mo>-</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>3</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <mrow> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>1</mn> <mo>)</mo> </mrow> <msub> <mi>V</mi> <mi>D</mi> </msub> </mrow> </mtd> <mtd> <msub> <mover> <mi>V</mi> <mo>&amp;CenterDot;</mo> </mover> <mi>u</mi> </msub> </mtd> <mtd> <mrow></mrow> </mtd> </mtr> </mtable> </mfenced> </mrow>
分别为等效速度在东向、北向和天向的分量；
为坐标变换矩阵的第i行第j列元素，i＝1,2,3；j＝1,2,3；
VD为里程计输出等效速度；
为陀螺测量值向量；
分别为东北天向速度的一阶导数；
步骤2、***初始化
对惯导***进行对准，并开始惯性导航解算，获得姿态角[θ,γ,ψ]T，速度和[λ,φ,h]T，同时通过里程计获得位移ΔS；
ΔS＝KD·Npluse为标量，KD为里程计刻度系数，Npluse为里程计输出脉冲；
所述对准方法采用静基座对准或动基座对准方法；
步骤3、天向和水平位移分量计算
根据步骤2导航解算得到的坐标变换矩阵计算里程计位移的天向分量和水平分量；
为3×3矩阵，则里程计输出位移的天向分量为：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>u</mi> <mi>n</mi> </msubsup> <mo>=</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mrow> <mo>(</mo> <mn>3</mn> <mo>,</mo> <mn>2</mn> <mo>)</mo> </mrow> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>S</mi> <mo>,</mo> </mrow>
水平分量为：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> <mo>=</mo> <msqrt> <mrow> <mn>1</mn> <mo>-</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>C</mi> <mi>b</mi> <mi>n</mi> </msubsup> <mo>(</mo> <mrow> <mn>3</mn> <mo>,</mo> <mn>2</mn> </mrow> <mo>)</mo> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> <mo>&amp;CenterDot;</mo> <mi>&amp;Delta;</mi> <mi>S</mi> <mo>,</mo> </mrow>
其中为的第3行第2列的元素；
步骤4、东向和北向位移分量计算
根据步骤2导航解算得到的速度计算里程计位移的东向分量和北向分量；具体方法如下：
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>&amp;CenterDot;</mo> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> </mrow>
<mrow> <msubsup> <mi>&amp;Delta;S</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>=</mo> <mfrac> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <msqrt> <mrow> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>e</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>+</mo> <msup> <mrow> <mo>(</mo> <msubsup> <mi>V</mi> <mi>n</mi> <mi>n</mi> </msubsup> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mrow> </msqrt> </mfrac> <mo>&amp;CenterDot;</mo> <msubsup> <mi>&amp;Delta;S</mi> <mrow> <mi>l</mi> <mi>e</mi> <mi>v</mi> <mi>e</mi> <mi>l</mi> </mrow> <mi>n</mi> </msubsup> </mrow>
其中，为程计位移的东向分量，为程计位移的北向分量；
步骤5、等效速度计算，并更新卡尔曼滤波模型信息
计算滤波周期内的等效速度，具体公式如下：
<mrow> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> </mrow> <mi>n</mi> </msubsup> <mo>=</mo> <msup> <mfenced open = "[" close = "]"> <mtable> <mtr> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>e</mi> </mrow> <mi>n</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>n</mi> </mrow> <mi>n</mi> </msubsup> </mtd> <mtd> <msubsup> <mi>V</mi> <mrow> <mi>o</mi> <mi>d</mi> <mi>o</mi> <mi>u</mi> </mrow> <mi>n</mi> </msubsup> </mtd> </mtr> </mtable> </mfenced> <mi>T</mi> </msup> <mo>=</mo> <mfrac> <mrow> <munderover> <mo>&amp;Sigma;</mo> <mrow> <mi>t</mi> <mo>=</mo> <msub> <mi>T</mi> <mi>n</mi> </msub> </mrow> <msub> <mi>T</mi> <mi>e</mi> </msub> </munderover> <msup> <mi>&amp;Delta;S</mi> <mi>n</mi> </msup> <mrow> <mo>(</mo> <mi>t</mi> <mo>)</mo> </mrow> </mrow> <msub> <mi>T</mi> <mi>e</mi> </msub> </mfrac> </mrow>
其中，为等效速度，ΔSn(t)为ΔSn在时刻t的值，Te为滤波周期，Tn为导航解算周期；
更新***矩阵A、观测矩阵H，计算量测量Z；
步骤6、对***进行卡尔曼滤波计算；
步骤7、对惯导姿态阵、速度、位置误差进行修正，对里程计刻度系数和安装误差进行修正；
其中，姿态误差为φ＝[X(3) X(4) X(5)]T，
则惯导姿态阵误差修正公式为I为单位矩阵；
惯导速度误差为δV＝[X(0) X(1) X(2)]T，
则速度误差修正方法为Vn＝Vn-δV；
惯导位置误差修正方法为
λ＝λ-X(6)
L＝L-X(7)
h＝h-X(8)
里程计刻度系数误差为Kd＝X(15)，
则里程计刻度系数误差修正方法为KODO＝KODO×(1+Kd)；
各修正量使用完毕后相应的状态量应置为零。
CN201611037069.7A 2016-11-23 2016-11-23 一种定位定向设备速度补偿方法 Active CN108088443B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201611037069.7A CN108088443B (zh) 2016-11-23 2016-11-23 一种定位定向设备速度补偿方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201611037069.7A CN108088443B (zh) 2016-11-23 2016-11-23 一种定位定向设备速度补偿方法

Publications (2)

Publication Number Publication Date
CN108088443A true CN108088443A (zh) 2018-05-29
CN108088443B CN108088443B (zh) 2021-06-08

Family

ID=62169960

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201611037069.7A Active CN108088443B (zh) 2016-11-23 2016-11-23 一种定位定向设备速度补偿方法

Country Status (1)

Country Link
CN (1) CN108088443B (zh)

Cited By (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109059913A (zh) * 2018-08-27 2018-12-21 立得空间信息技术股份有限公司 一种用于车载导航***的零延迟组合导航初始化方法
CN109143304A (zh) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 用于确定无人驾驶车辆位姿的方法和装置
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性***的高精度测绘方法
CN110514221A (zh) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 一种里程仪初始参数快速计算方法
CN110657788A (zh) * 2018-06-29 2020-01-07 北京自动化控制设备研究所 一种起重机轨道平顺性动态检测方法
CN110873563A (zh) * 2018-08-30 2020-03-10 杭州海康机器人技术有限公司 一种云台姿态估计方法及装置
CN111912405A (zh) * 2019-05-10 2020-11-10 中国人民解放***箭军工程大学 一种基于车载惯组与多普勒雷达的组合导航方法及***
CN112284415A (zh) * 2020-10-19 2021-01-29 株洲菲斯罗克光电技术有限公司 里程计标度误差标定方法、***及计算机存储介质
CN115773751A (zh) * 2023-02-13 2023-03-10 中国航空工业集团公司西安飞行自动控制研究所 一种修正等效天向加计零位造成对准误差的方法

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672650A (zh) * 2009-09-29 2010-03-17 北京航空航天大学 一种电磁干扰环境下的定向定位导航***
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN104977004A (zh) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 一种激光惯组与里程计组合导航方法及***

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101672650A (zh) * 2009-09-29 2010-03-17 北京航空航天大学 一种电磁干扰环境下的定向定位导航***
CN103217157A (zh) * 2012-01-18 2013-07-24 北京自动化控制设备研究所 一种惯导/里程计自主组合导航方法
CN104977004A (zh) * 2015-07-13 2015-10-14 湖北航天技术研究院总体设计所 一种激光惯组与里程计组合导航方法及***

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
PENGXIANG YANG: ""Local Feedback Compensation Method for INS/GPS/OD Land Navigation System"", 《PROCEEDINGS OF THE 2008 IEEE INTERNATIONAL CONFERENCE ON INFORMATION AND AUTOMATION》 *
陈鸿跃等: ""一种里程计辅助车载捷联惯导行进间对准方法"", 《导弹与航天运载技术》 *

Cited By (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110657788A (zh) * 2018-06-29 2020-01-07 北京自动化控制设备研究所 一种起重机轨道平顺性动态检测方法
CN109059913A (zh) * 2018-08-27 2018-12-21 立得空间信息技术股份有限公司 一种用于车载导航***的零延迟组合导航初始化方法
CN110873563A (zh) * 2018-08-30 2020-03-10 杭州海康机器人技术有限公司 一种云台姿态估计方法及装置
CN109143304A (zh) * 2018-09-30 2019-01-04 百度在线网络技术（北京）有限公司 用于确定无人驾驶车辆位姿的方法和装置
CN109143304B (zh) * 2018-09-30 2020-12-29 百度在线网络技术（北京）有限公司 用于确定无人驾驶车辆位姿的方法和装置
CN109827572A (zh) * 2019-03-12 2019-05-31 北京星网宇达科技股份有限公司 一种检测车位置预测的方法及装置
CN109974697A (zh) * 2019-03-21 2019-07-05 中国船舶重工集团公司第七0七研究所 一种基于惯性***的高精度测绘方法
CN109974697B (zh) * 2019-03-21 2022-07-26 中国船舶重工集团公司第七0七研究所 一种基于惯性***的高精度测绘方法
CN111912405A (zh) * 2019-05-10 2020-11-10 中国人民解放***箭军工程大学 一种基于车载惯组与多普勒雷达的组合导航方法及***
CN110514221A (zh) * 2019-08-13 2019-11-29 中国航空工业集团公司西安飞行自动控制研究所 一种里程仪初始参数快速计算方法
CN110514221B (zh) * 2019-08-13 2023-03-14 中国航空工业集团公司西安飞行自动控制研究所 一种里程仪初始参数快速计算方法
CN112284415A (zh) * 2020-10-19 2021-01-29 株洲菲斯罗克光电技术有限公司 里程计标度误差标定方法、***及计算机存储介质
CN115773751A (zh) * 2023-02-13 2023-03-10 中国航空工业集团公司西安飞行自动控制研究所 一种修正等效天向加计零位造成对准误差的方法

Also Published As

Publication number Publication date
CN108088443B (zh) 2021-06-08

Similar Documents

Publication Publication Date Title
CN108088443A (zh) 一种定位定向设备速度补偿方法
CN101893445B (zh) 摇摆状态下低精度捷联惯导***快速初始对准方法
CN108180925A (zh) 一种里程计辅助车载动态对准方法
CN103245359B (zh) 一种惯性导航***中惯性传感器固定误差实时标定方法
CN103900565B (zh) 一种基于差分gps的惯导***姿态获取方法
CN105823481A (zh) 一种基于单天线的gnss-ins车辆定姿方法
CN108051866A (zh) 基于捷联惯性/gps组合辅助水平角运动隔离的重力测量方法
CN105698822B (zh) 基于反向姿态跟踪的自主式惯性导航行进间初始对准方法
CN106507913B (zh) 用于管道测绘的组合定位方法
CN104977004B (zh) 一种激光惯组与里程计组合导航方法及***
CN104697526A (zh) 用于农业机械的捷联惯导***以及控制方法
CN107270893A (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN109974697A (zh) 一种基于惯性***的高精度测绘方法
CN107588769A (zh) 一种车载捷联惯导、里程计及高程计组合导航方法
CN102679978B (zh) 一种旋转式捷联惯性导航***静基座初始对准方法
CN102519485B (zh) 一种引入陀螺信息的二位置捷联惯性导航***初始对准方法
CN101949703A (zh) 一种捷联惯性/卫星组合导航滤波方法
CN103217174B (zh) 一种基于低精度微机电***的捷联惯导***初始对准方法
CN109612460B (zh) 一种基于静止修正的垂线偏差测量方法
CN111678514B (zh) 一种基于载体运动条件约束和单轴旋转调制的车载自主导航方法
CN104165638A (zh) 一种双轴旋转惯导***多位置自主标定方法
CN102853837A (zh) 一种mimu和gnss信息融合的方法
CN105737842A (zh) 基于旋转调制和虚拟里程仪的车载自主导航方法
CN109470276B (zh) 基于零速修正的里程计标定方法与装置
CN108225312B (zh) 一种gnss/ins松组合中杆臂估计以及补偿方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant