CN115235513A - 一种基于卫导伪距和伪距率的惯导校正方法 - Google Patents

一种基于卫导伪距和伪距率的惯导校正方法 Download PDF

Info

Publication number
CN115235513A
CN115235513A CN202211134731.6A CN202211134731A CN115235513A CN 115235513 A CN115235513 A CN 115235513A CN 202211134731 A CN202211134731 A CN 202211134731A CN 115235513 A CN115235513 A CN 115235513A
Authority
CN
China
Prior art keywords
error
equation
matrix
satellite
inertial navigation
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
CN202211134731.6A
Other languages
English (en)
Other versions
CN115235513B (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.)
707th Research Institute of CSIC
Original Assignee
707th Research Institute of CSIC
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 707th Research Institute of CSIC filed Critical 707th Research Institute of CSIC
Priority to CN202211134731.6A priority Critical patent/CN115235513B/zh
Publication of CN115235513A publication Critical patent/CN115235513A/zh
Application granted granted Critical
Publication of CN115235513B publication Critical patent/CN115235513B/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
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Manufacturing & Machinery (AREA)
  • Navigation (AREA)

Abstract

本发明涉及一种基于卫导伪距和伪距率的惯导校正方法，步骤为：1、以惯导误差方程和卫导误差方程为基础建立Kalman滤波状态方程；2、以伪距差模型、伪距率差模型作为观测量，建立Kalman滤波观测方程；3、对步骤1建立的Kalman滤波状态方程和步骤2建立的Kalman滤波观测方程进行Kalman滤波解算；4、依据步骤3得到的滤波解算公式，采用序贯法依次计算多颗卫星伪距和伪距率信息：5、依据步骤4得到的伪距和伪距率信息，定时校正惯导误差和误差源。本方法保障了惯导长周期导航精度。

Description

一种基于卫导伪距和伪距率的惯导校正方法
技术领域
本发明属于惯导/卫导信息的紧耦合技术，具体涉及一种基于卫导伪距和伪距率的惯导校正方法。
背景技术
卫星导航与惯性导航在导航定位误差特性方面具有良好的互补性，因此惯导/卫导紧耦合技术可实现深远海连续导航任务，是导航技术的主要发展方向之一，在国民经济和国防等领域具有极为重要的应用价值。
卫导信息中的伪距信息真实的反映了卫星与接收机之间的距离，伪距测量值包含钟差、大气延时等误差，在同一时刻利用四颗及以上的不同可见卫星的伪距测量值，接收机就可以实现三维绝对定位与定时。伪距变化率（以下简称伪距率）反映着卫星与接收机之间的相对运动速度，在获得多个卫星伪距率测量值的条件下，接收机可能从中解算出用户运动速度。因此基于伪距和伪距率的紧耦合技术比单使用伪距信息具有更好的观测性，估算和校正惯导误差精度更高。
发明内容
本发明的目的是在于克服现有技术的不足之处，提供一种基于卫导伪距和伪距率的惯导校正方法。
本发明的上述目的通过如下技术方案来实现：
一种基于卫导伪距和伪距率的惯导校正方法，其特征在于，包括如下步骤：
步骤1、以惯导误差方程和卫导误差方程为基础建立Kalman滤波状态方程；
步骤2、以卫导伪距差模型、伪距率差模型作为观测量，建立Kalman滤波观测方程；
步骤3、对步骤1建立的Kalman滤波状态方程和步骤2建立的Kalman滤波观测方程进行Kalman滤波解算；
步骤4、依据步骤3得到的滤波解算方式，采用序贯法依次计算多颗卫星伪距和伪距率信息：
步骤5、依据步骤4得到的伪距和伪距率信息，定时校正惯导误差和误差源。
进一步的：步骤1包括：
（1）建立惯导误差方程
惯导系统主要误差包括经度误差
Figure 164410DEST_PATH_IMAGE001
、纬度误差
Figure 550392DEST_PATH_IMAGE002
、东速误差
Figure 533391DEST_PATH_IMAGE003
、北速误差
Figure 397442DEST_PATH_IMAGE004
、纵摇误差
Figure 179191DEST_PATH_IMAGE005
、横摇误差
Figure 470495DEST_PATH_IMAGE006
、航向误差
Figure 206370DEST_PATH_IMAGE007
，误差源主要为陀螺常值漂移、陀螺随机漂移、加速度计零偏误差，其中，陀螺常值漂移包括东向陀螺常值漂移
Figure 874112DEST_PATH_IMAGE008
、北向陀螺常值漂移
Figure 11832DEST_PATH_IMAGE009
和方位陀螺常值漂移
Figure 474037DEST_PATH_IMAGE010
；陀螺随机漂移包括东向陀螺随机漂移
Figure 697208DEST_PATH_IMAGE011
、北向陀螺随机漂移
Figure 903062DEST_PATH_IMAGE012
和方位陀螺随机漂移
Figure 393824DEST_PATH_IMAGE013
；加速度计零偏误差包括东向加速度计零偏误差
Figure 26930DEST_PATH_IMAGE014
、北向加速度计零偏误差
Figure 737397DEST_PATH_IMAGE015
；各误差误差微分方程为：
Figure 481362DEST_PATH_IMAGE016
(1)
Figure 593675DEST_PATH_IMAGE017
(2)
Figure 132104DEST_PATH_IMAGE018
(3)
Figure 595446DEST_PATH_IMAGE019
(4)
Figure 381917DEST_PATH_IMAGE020
(5)
Figure 348736DEST_PATH_IMAGE021
(6)
Figure 589225DEST_PATH_IMAGE022
(7)
Figure 8705DEST_PATH_IMAGE023
(8)
Figure 360052DEST_PATH_IMAGE024
(9)
Figure 181377DEST_PATH_IMAGE025
(10)
Figure 592767DEST_PATH_IMAGE026
(11)
Figure 499543DEST_PATH_IMAGE027
(12)
Figure 887537DEST_PATH_IMAGE028
(13)
Figure 828948DEST_PATH_IMAGE029
(14)
Figure 411239DEST_PATH_IMAGE030
(15)
式中：
Figure 539732DEST_PATH_IMAGE031
——重力加速度，与当地纬度有关，球面上的重力
Figure 232882DEST_PATH_IMAGE032
Figure 763220DEST_PATH_IMAGE033
；
R M 、R N ——子午圈和卯酉圈内的地球曲率半径；
Figure 516412DEST_PATH_IMAGE034
——地球自转角速度，
Figure 397781DEST_PATH_IMAGE035
；
Figure 393156DEST_PATH_IMAGE036
——表示东向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 43580DEST_PATH_IMAGE037
——表示北向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 702095DEST_PATH_IMAGE038
——表示方位陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 70759DEST_PATH_IMAGE039
——东向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure 105711DEST_PATH_IMAGE040
——北向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure 876221DEST_PATH_IMAGE041
——方位陀螺随机漂移一阶马尔科夫模型随机噪声；
将惯导误差和误差源作为惯导误差变量，记为：
Figure 705637DEST_PATH_IMAGE042
惯导误差方程写成矩阵形式为：
Figure 561598DEST_PATH_IMAGE043
(16)
式中：
Figure 898776DEST_PATH_IMAGE044
——表示t时刻惯导误差方程观测矩阵，具体为
Figure 992634DEST_PATH_IMAGE045
为15行3列矩阵，
Figure 258530DEST_PATH_IMAGE046
Figure 336207DEST_PATH_IMAGE047
——表示t时刻惯导测量噪声，具体为
Figure 978541DEST_PATH_IMAGE048
；
Figure 458064DEST_PATH_IMAGE049
——表示t时刻惯导误差方程观测矩阵，
Figure 894862DEST_PATH_IMAGE050
为15行15列矩阵，以
Figure 975949DEST_PATH_IMAGE051
表示矩阵
Figure 156394DEST_PATH_IMAGE052
第i行j列元素，则矩阵
Figure 490424DEST_PATH_IMAGE053
非零元素为：
Figure 98122DEST_PATH_IMAGE054
，
Figure 150392DEST_PATH_IMAGE055
，
Figure 868949DEST_PATH_IMAGE056
，
Figure 57485DEST_PATH_IMAGE057
，
Figure 836085DEST_PATH_IMAGE058
，
Figure 874186DEST_PATH_IMAGE059
，
Figure 396435DEST_PATH_IMAGE060
，
Figure 705056DEST_PATH_IMAGE061
，
Figure 654558DEST_PATH_IMAGE062
，
Figure 150261DEST_PATH_IMAGE063
，
Figure 741779DEST_PATH_IMAGE064
，
Figure 639328DEST_PATH_IMAGE065
，
Figure 258266DEST_PATH_IMAGE066
，
Figure 772424DEST_PATH_IMAGE067
，
Figure 636475DEST_PATH_IMAGE068
，
Figure 654109DEST_PATH_IMAGE069
，
Figure 210993DEST_PATH_IMAGE070
，
Figure 946868DEST_PATH_IMAGE071
，
Figure 614609DEST_PATH_IMAGE072
，
Figure 486750DEST_PATH_IMAGE073
，
Figure 713070DEST_PATH_IMAGE074
，
Figure 936241DEST_PATH_IMAGE075
；
（2）建立卫导误差方程
卫导误差包含两个与时间有关的误差：一个是与时钟误差等效的距离误差
Figure 142094DEST_PATH_IMAGE076
，另一个是与时钟频率误差等效的距离率误差
Figure 868742DEST_PATH_IMAGE077
，其微分方程为：
Figure 767428DEST_PATH_IMAGE078
(17)
Figure 212316DEST_PATH_IMAGE079
(18)
式中：
Figure 221860DEST_PATH_IMAGE080
——距离误差
Figure 334172DEST_PATH_IMAGE081
的测量噪声；
Figure 642575DEST_PATH_IMAGE082
——距离率误差
Figure 840338DEST_PATH_IMAGE083
的测量噪声；
Figure 387994DEST_PATH_IMAGE084
——距离率误差
Figure 354813DEST_PATH_IMAGE085
的一阶马尔科夫模型相关频率参数；
将卫导误差方程写成矩阵的形式为
Figure 329722DEST_PATH_IMAGE086
(19)
式中：
Figure 749202DEST_PATH_IMAGE087
——表示t时刻卫导测量噪声，具体为
Figure 100549DEST_PATH_IMAGE088
；
Figure 187454DEST_PATH_IMAGE089
——表示t时刻卫导误差方程观测矩阵，具体为
Figure 831800DEST_PATH_IMAGE090
；
Figure 738576DEST_PATH_IMAGE091
——表示t时刻卫导误差方程观测矩阵，具体为
Figure 628034DEST_PATH_IMAGE092
；
（3）建立Kalman滤波状态方程
将惯导误差方程与卫导误差方程合并，得到kalman滤波状态方程，如下所示：
Figure 569446DEST_PATH_IMAGE093
(20)
即
Figure 886157DEST_PATH_IMAGE094
(21)
式中
Figure 545809DEST_PATH_IMAGE095
为t时刻状态转移矩阵，
Figure 973379DEST_PATH_IMAGE096
为t时刻系统过程噪声输入矩阵，
Figure 34876DEST_PATH_IMAGE097
为t时刻系统过程噪声向量，系统过程噪声向量
Figure 21024DEST_PATH_IMAGE098
的方差强度阵记为
Figure 902393DEST_PATH_IMAGE099
。
进一步的：步骤2包括：
（1）建立伪距差模型
设惯导位置坐标为
Figure 133654DEST_PATH_IMAGE100
，当前时刻收到
Figure 784078DEST_PATH_IMAGE101
颗卫星信息；
第
Figure 708172DEST_PATH_IMAGE102
颗卫星的位置坐标为
Figure 811257DEST_PATH_IMAGE103
，
Figure 111788DEST_PATH_IMAGE104
，依次计算
Figure 849675DEST_PATH_IMAGE101
颗星伪距
Figure 210249DEST_PATH_IMAGE105
，计算公式如下：
Figure 800630DEST_PATH_IMAGE106
(22)
设载体的真实位置坐标为
Figure 373694DEST_PATH_IMAGE107
，将伪距
Figure 998711DEST_PATH_IMAGE108
一阶泰勒展开，有：
Figure 264607DEST_PATH_IMAGE109
(23)
令
Figure 342284DEST_PATH_IMAGE110
(24)
则有
Figure 984618DEST_PATH_IMAGE111
(25)
Figure 956817DEST_PATH_IMAGE112
(26)
Figure 393614DEST_PATH_IMAGE113
(27)
因此第
Figure 693009DEST_PATH_IMAGE114
颗卫星伪距
Figure 607875DEST_PATH_IMAGE115
模型为：
Figure 207484DEST_PATH_IMAGE116
(28)
卫星接收机输出的第
Figure 815183DEST_PATH_IMAGE102
颗卫星测量伪距
Figure 867452DEST_PATH_IMAGE117
可表示为，
Figure 84545DEST_PATH_IMAGE118
(29)
第
Figure 538660DEST_PATH_IMAGE102
颗卫星计算伪距
Figure 51681DEST_PATH_IMAGE119
与测量伪距
Figure 591246DEST_PATH_IMAGE120
之差记为伪距差
Figure 113495DEST_PATH_IMAGE121
，模型如下，
Figure 422116DEST_PATH_IMAGE122
(30)
式中，
Figure 371618DEST_PATH_IMAGE123
为伪距测量噪声；
上式写为矩阵形式为：
Figure 132900DEST_PATH_IMAGE124
(31)
公式（31）中矩阵仅列出第一颗星、第二颗星和第N颗星信息，第3颗星至第N-1颗星信息用省略号“
Figure 957375DEST_PATH_IMAGE125
”代替；
直角坐标和大地坐标之间的转换关系可由下式表示，
Figure 120503DEST_PATH_IMAGE126
(32)
式中，R N 为卯酉圈内的地球曲率半径；由公式（32）可得坐标误差，
Figure 240905DEST_PATH_IMAGE127
(33)
将系数矩阵记为
Figure 489484DEST_PATH_IMAGE128
，即
Figure 619114DEST_PATH_IMAGE129
则
Figure 371170DEST_PATH_IMAGE130
(34)
将（34）式代入（30）式得到伪距差模型为：
Figure 928053DEST_PATH_IMAGE131
(35)
式中元素
Figure 162463DEST_PATH_IMAGE132
计算公式为：
Figure 564625DEST_PATH_IMAGE133
(36)
(2)建立伪距率差模型
由（30）式可得伪距率差的模型为：
Figure 436766DEST_PATH_IMAGE134
(37)
其中
Figure 164551DEST_PATH_IMAGE135
、
Figure 122143DEST_PATH_IMAGE136
、
Figure 593575DEST_PATH_IMAGE137
为在地球直角坐标系中表示的速度误差，通过坐标变换矩阵将其变换到地理坐标系中，变换公式为，
Figure 585802DEST_PATH_IMAGE138
(38)
水面舰船忽略垂向速度误差，则（38）式简写为下式
Figure 484488DEST_PATH_IMAGE139
(39)
令
Figure 433770DEST_PATH_IMAGE140
则得到
Figure 443315DEST_PATH_IMAGE141
(40)
将（40）式代入（37）式得到伪距率差模型为：
Figure 290048DEST_PATH_IMAGE142
(41)
式中元素
Figure 625214DEST_PATH_IMAGE143
计算公式为：
Figure 557398DEST_PATH_IMAGE144
(42)
（3）建立Kalman滤波观测方程
基于伪距和伪距率的惯导与卫导紧耦合滤波器选择惯导和卫导的伪距差、伪距率差作为观测量，Kalman滤波观测方程记为：
Figure 370633DEST_PATH_IMAGE145
(43)
式中
Figure 71873DEST_PATH_IMAGE146
为t时刻观测矩阵，
Figure 312362DEST_PATH_IMAGE147
为t时刻观测噪声向量，记系统过程噪声向量
Figure 495956DEST_PATH_IMAGE148
的方差强度阵为
Figure 847303DEST_PATH_IMAGE149
t时刻第
Figure 668628DEST_PATH_IMAGE150
颗卫星观测模型为：
Figure 814439DEST_PATH_IMAGE151
观测量计算公式为，
Figure 721215DEST_PATH_IMAGE152
(44)
相应的，t时刻第
Figure 876253DEST_PATH_IMAGE153
颗卫星观测矩阵
Figure 552085DEST_PATH_IMAGE154
的定义如下，
Figure 134376DEST_PATH_IMAGE155
。
进一步的：步骤3中：
状态方程和观测方程组成随机连续系统，模型如下：
Figure 26983DEST_PATH_IMAGE156
(45)
Figure 720133DEST_PATH_IMAGE157
(46)
进行Kalman滤波解算包括如下步骤：
（1）随机连续系统离散化
为了适应工程应用需要将随机连续系统（45）、（46）离散化，根据线性系统理论得到随机线性离散系统的状态方程，
Figure 516051DEST_PATH_IMAGE158
(47)
式中，
Figure 269243DEST_PATH_IMAGE159
为k-1时刻至k时刻离散化的状态转移矩阵，以
Figure 885032DEST_PATH_IMAGE160
为时间长度将k-1时刻至k时刻之间分为n段，每个
Figure 381872DEST_PATH_IMAGE161
时间内的状态转移矩阵为
Figure 32297DEST_PATH_IMAGE162
，n个
Figure 956390DEST_PATH_IMAGE163
时间内的状态转移矩阵
Figure 823590DEST_PATH_IMAGE164
相乘构成k-1时刻至k时刻状态转移矩阵
Figure 858542DEST_PATH_IMAGE165
，
Figure 629052DEST_PATH_IMAGE166
计算公式如下，
Figure 458468DEST_PATH_IMAGE167
(48)
Figure 48849DEST_PATH_IMAGE168
(49)
k时刻离散系统噪声矩阵
Figure 887492DEST_PATH_IMAGE169
计算公式如下，
Figure 512508DEST_PATH_IMAGE170
(50)
Figure 778405DEST_PATH_IMAGE171
(51)
Figure 121661DEST_PATH_IMAGE172
(52)
Figure 748950DEST_PATH_IMAGE173
为状态矩阵
Figure 494052DEST_PATH_IMAGE174
更新时间间隔，与导航解算时间间隔相关，导航解算间隔
Figure 930850DEST_PATH_IMAGE175
；
（2）基于上述随机线性离散系统的状态方程进行Kalman滤波解算，解算公式包括：
状态一步预测
Figure 495823DEST_PATH_IMAGE176
、一步预测误差方差阵
Figure 676269DEST_PATH_IMAGE177
、状态估计
Figure 275877DEST_PATH_IMAGE178
、滤波增益矩阵
Figure 617997DEST_PATH_IMAGE179
、估计误差的方差阵
Figure 670267DEST_PATH_IMAGE180
公式分别如下：
Figure 152938DEST_PATH_IMAGE181
(53)
Figure 607053DEST_PATH_IMAGE182
(54)
Figure 385654DEST_PATH_IMAGE183
(55)
Figure 659640DEST_PATH_IMAGE184
(56)
Figure 181888DEST_PATH_IMAGE185
(57)
Figure 490510DEST_PATH_IMAGE186
为k-1时刻状态向量的滤波估计值，初值为零；
Figure 440011DEST_PATH_IMAGE187
为k-1时刻系统滤波估计方差矩阵，初值
Figure 466873DEST_PATH_IMAGE188
与惯导系统误差相关；本例中
Figure 25768DEST_PATH_IMAGE188
为
Figure 454476DEST_PATH_IMAGE189
矩阵，非零元素为：
Figure 100002_DEST_PATH_IMAGE190
，
Figure 43720DEST_PATH_IMAGE191
，
Figure 557878DEST_PATH_IMAGE192
，
Figure 156349DEST_PATH_IMAGE193
，
Figure 439563DEST_PATH_IMAGE194
，
Figure 730867DEST_PATH_IMAGE195
，
Figure 230856DEST_PATH_IMAGE196
，
Figure 633019DEST_PATH_IMAGE197
，
Figure 36318DEST_PATH_IMAGE198
，
Figure 232945DEST_PATH_IMAGE199
，
Figure 721695DEST_PATH_IMAGE200
，
Figure 193127DEST_PATH_IMAGE201
，
Figure 185354DEST_PATH_IMAGE202
，
Figure 818461DEST_PATH_IMAGE203
，
Figure 33322DEST_PATH_IMAGE204
。
进一步的：步骤4中：
依次处理k时刻第j颗卫星的观测信息，如下式：
Figure 42867DEST_PATH_IMAGE205
(58)
第j颗卫星滤波计算公式为：
Figure 889600DEST_PATH_IMAGE206
(59)
Figure 959187DEST_PATH_IMAGE207
(60)
Figure 891371DEST_PATH_IMAGE208
(61)
K时刻卫星信息全部计算后，将得到的
Figure 704606DEST_PATH_IMAGE209
与
Figure 405846DEST_PATH_IMAGE210
分别赋值给这个k时刻校正后的状态及其均方误差阵，完成k时刻滤波估计，见式（62）和（63）；
Figure 646335DEST_PATH_IMAGE211
(62)
Figure 564350DEST_PATH_IMAGE212
(63)。
进一步的：步骤5中：
每4小时修正一次惯导系统，校正量包括惯导系统的经度误差
Figure 915697DEST_PATH_IMAGE213
、纬度误差
Figure 737022DEST_PATH_IMAGE214
、东向速度误差
Figure 148412DEST_PATH_IMAGE215
、北向速度误差
Figure 55188DEST_PATH_IMAGE216
、航向角误差
Figure 210226DEST_PATH_IMAGE217
、北向陀螺常值漂移
Figure 886058DEST_PATH_IMAGE218
和方位陀螺常值漂移
Figure 468349DEST_PATH_IMAGE219
；校正量为校正时刻的滤波估计值，见式（64）：
Figure 360956DEST_PATH_IMAGE220
(64) 。
本发明具有的优点和积极效果：
1、本发明设计了一种基于伪距和伪距率的惯导校正方法，通过误差推导建立滤波模型和观测模型，以Kalman滤波为基础，通过序贯法依次处理多颗卫星伪距和伪距率信息，进行卫星信息有效性检验，实时估算惯导误差和误差源，定时校正惯导系统误差和误差源，从而保障惯导长周期导航精度。
2、本发明设计了数值计算的方法处理多颗卫星伪距和伪距率信息，解决了多颗卫星信息同步处理解算效率问题，避免了大矩阵求逆可能带来的奇异值问题，具备工程可行性。
3、该技术不改变惯导系统的硬件结构，便于推广至船用惯导系统，无需较大改动，具有较好的工程应用前景。
具体实施方式
以下通过实施例对本发明的结构作进一步说明。需要说明的是本实施例是叙述性的，而不是限定性的。
一种基于卫导伪距和伪距率的惯导校正方法，其发明点为，包括以下步骤：
步骤1、建立Kalman滤波状态方程
以惯导误差方程和卫导接收机误差方程为基础建立Kalman滤波状态方程，具体包括：
（1）建立惯导误差方程
惯导系统主要误差包括经度误差
Figure 54106DEST_PATH_IMAGE221
、纬度误差
Figure 850023DEST_PATH_IMAGE222
、东速误差
Figure 337637DEST_PATH_IMAGE223
、北速误差
Figure 219005DEST_PATH_IMAGE224
、纵摇误差
Figure 715845DEST_PATH_IMAGE225
、横摇误差
Figure 366270DEST_PATH_IMAGE226
、航向误差
Figure 290363DEST_PATH_IMAGE227
，误差源主要为陀螺常值漂移、陀螺随机漂移、加速度计零偏误差，其中，陀螺常值漂移包括东向陀螺常值漂移
Figure 891984DEST_PATH_IMAGE228
、北向陀螺常值漂移
Figure 192515DEST_PATH_IMAGE229
和方位陀螺常值漂移
Figure 963025DEST_PATH_IMAGE230
；陀螺随机漂移包括东向陀螺随机漂移
Figure 792441DEST_PATH_IMAGE231
、北向陀螺随机漂移
Figure 382822DEST_PATH_IMAGE232
和方位陀螺随机漂移
Figure 221465DEST_PATH_IMAGE233
；加速度计零偏误差包括东向加速度计零偏误差
Figure 846481DEST_PATH_IMAGE234
、北向加速度计零偏误差
Figure 377957DEST_PATH_IMAGE235
；各误差误差微分方程为：
Figure 682731DEST_PATH_IMAGE236
(1)
Figure 59485DEST_PATH_IMAGE237
(2)
Figure 804588DEST_PATH_IMAGE238
(3)
Figure 241385DEST_PATH_IMAGE239
(4)
Figure 806359DEST_PATH_IMAGE240
(5)
Figure 986804DEST_PATH_IMAGE241
(6)
Figure 320834DEST_PATH_IMAGE242
(7)
Figure 928532DEST_PATH_IMAGE243
(8)
Figure 980802DEST_PATH_IMAGE244
(9)
Figure 463474DEST_PATH_IMAGE245
(10)
Figure 652010DEST_PATH_IMAGE246
(11)
Figure 430610DEST_PATH_IMAGE247
(12)
Figure 970176DEST_PATH_IMAGE248
(13)
Figure 492424DEST_PATH_IMAGE249
(14)
Figure 801045DEST_PATH_IMAGE250
(15)
式中：
Figure 750547DEST_PATH_IMAGE251
——重力加速度，与当地纬度有关，球面上的重力公式为
Figure 777409DEST_PATH_IMAGE252
，
Figure 336304DEST_PATH_IMAGE253
；
R M 、R N ——子午圈和卯酉圈内的地球曲率半径；
Figure 499432DEST_PATH_IMAGE254
——地球自转角速度，
Figure 885414DEST_PATH_IMAGE255
；
Figure 133992DEST_PATH_IMAGE256
——表示东向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 998043DEST_PATH_IMAGE257
——表示北向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 281257DEST_PATH_IMAGE258
——表示方位陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 572561DEST_PATH_IMAGE259
——东向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure 308436DEST_PATH_IMAGE260
——北向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure 474713DEST_PATH_IMAGE261
——方位陀螺随机漂移一阶马尔科夫模型随机噪声；
将惯导误差和误差源作为惯导误差变量，记为：
Figure 612433DEST_PATH_IMAGE262
惯导误差方程写成矩阵形式为：
Figure 340218DEST_PATH_IMAGE263
(16)
式中：
Figure 297809DEST_PATH_IMAGE264
——表示t时刻惯导误差方程观测矩阵，具体为
Figure 503663DEST_PATH_IMAGE265
为15行3列矩阵，
Figure 495890DEST_PATH_IMAGE266
Figure 394575DEST_PATH_IMAGE267
——表示t时刻惯导测量噪声，具体为
Figure 370622DEST_PATH_IMAGE268
；
Figure 353402DEST_PATH_IMAGE269
——表示t时刻惯导误差方程观测矩阵，
Figure 731294DEST_PATH_IMAGE270
为15行15列矩阵，以
Figure 269723DEST_PATH_IMAGE271
表示矩阵
Figure 467486DEST_PATH_IMAGE272
第i行j列元素，则矩阵
Figure 15142DEST_PATH_IMAGE273
非零元素为：
Figure 247540DEST_PATH_IMAGE274
，
Figure 222449DEST_PATH_IMAGE275
，
Figure 907508DEST_PATH_IMAGE276
，
Figure 757390DEST_PATH_IMAGE277
，
Figure 578716DEST_PATH_IMAGE278
，
Figure 990106DEST_PATH_IMAGE279
，
Figure 896882DEST_PATH_IMAGE280
，
Figure 786340DEST_PATH_IMAGE281
，
Figure 727752DEST_PATH_IMAGE282
，
Figure 310043DEST_PATH_IMAGE283
，
Figure 704115DEST_PATH_IMAGE284
，
Figure 895800DEST_PATH_IMAGE285
，
Figure 426138DEST_PATH_IMAGE286
，
Figure 179330DEST_PATH_IMAGE287
，
Figure 60699DEST_PATH_IMAGE288
，
Figure 557539DEST_PATH_IMAGE289
，
Figure 473543DEST_PATH_IMAGE290
，
Figure 866478DEST_PATH_IMAGE291
，
Figure 235142DEST_PATH_IMAGE292
，
Figure 768630DEST_PATH_IMAGE293
，
Figure 539139DEST_PATH_IMAGE294
，
Figure 634134DEST_PATH_IMAGE295
；
（2）建立卫导误差方程
卫导误差包含两个与时间有关的误差：一个是与时钟误差等效的距离误差
Figure 224516DEST_PATH_IMAGE296
，另一个是与时钟频率误差等效的距离率误差
Figure 328738DEST_PATH_IMAGE297
，其微分方程为：
Figure 688175DEST_PATH_IMAGE298
(17)
Figure 954071DEST_PATH_IMAGE299
(18)
式中：
Figure 31749DEST_PATH_IMAGE300
——距离误差
Figure 924617DEST_PATH_IMAGE301
的测量噪声；
Figure 404140DEST_PATH_IMAGE302
——距离率误差
Figure 106516DEST_PATH_IMAGE303
的测量噪声；
Figure 671490DEST_PATH_IMAGE304
——距离率误差
Figure 851936DEST_PATH_IMAGE305
的一阶马尔科夫模型相关频率参数；
将卫导误差方程写成矩阵的形式为
Figure 185965DEST_PATH_IMAGE306
(19)
式中：
Figure 793664DEST_PATH_IMAGE307
——表示t时刻卫导测量噪声，具体为
Figure 845933DEST_PATH_IMAGE308
；
Figure 564491DEST_PATH_IMAGE309
——表示t时刻卫导误差方程观测矩阵，具体为
Figure 517141DEST_PATH_IMAGE310
；
Figure 295741DEST_PATH_IMAGE311
——表示t时刻卫导误差方程观测矩阵，具体为
Figure 835307DEST_PATH_IMAGE312
；
（3）建立Kalman滤波状态方程
将惯导误差方程与卫导误差方程合并，得到kalman滤波状态方程，如下所示：
Figure 357555DEST_PATH_IMAGE313
(20)
即
Figure 666177DEST_PATH_IMAGE314
(21)
式中
Figure 615678DEST_PATH_IMAGE315
为t时刻状态转移矩阵，
Figure 376961DEST_PATH_IMAGE316
为t时刻系统过程噪声输入矩阵，
Figure 702900DEST_PATH_IMAGE317
为t时刻系统过程噪声向量，系统过程噪声向量
Figure 364563DEST_PATH_IMAGE318
的方差强度阵记为
Figure 484966DEST_PATH_IMAGE319
。
步骤2、建立Kalman滤波观测方程
以伪距差模型、伪距率差模型作为观测量，建立观测量与惯导系统经、纬度误差的关系式，具体如下：
（1）建立伪距差模型
设惯导位置坐标为
Figure 999124DEST_PATH_IMAGE320
，当前时刻收到
Figure 863175DEST_PATH_IMAGE321
颗卫星信息；
第
Figure 880809DEST_PATH_IMAGE322
颗卫星的位置坐标为
Figure 437693DEST_PATH_IMAGE323
，
Figure 173567DEST_PATH_IMAGE324
，依次计算
Figure 841309DEST_PATH_IMAGE321
颗星伪距
Figure 211985DEST_PATH_IMAGE325
，计算公式如下：
Figure 939770DEST_PATH_IMAGE326
(22)
设载体的真实位置坐标为
Figure 162941DEST_PATH_IMAGE327
，将伪距
Figure 368794DEST_PATH_IMAGE328
一阶泰勒展开，有：
Figure 361021DEST_PATH_IMAGE329
(23)
令
Figure 259707DEST_PATH_IMAGE330
(24)
则有
Figure 704595DEST_PATH_IMAGE331
(25)
Figure 714139DEST_PATH_IMAGE332
(26)
Figure 330846DEST_PATH_IMAGE333
(27)
因此第
Figure 134854DEST_PATH_IMAGE322
颗卫星伪距
Figure 332617DEST_PATH_IMAGE334
模型为：
Figure 880273DEST_PATH_IMAGE335
(28)
卫星接收机输出的第
Figure 847092DEST_PATH_IMAGE322
颗卫星测量伪距
Figure 822001DEST_PATH_IMAGE336
可表示为，
Figure 507061DEST_PATH_IMAGE337
(29)
第
Figure 858407DEST_PATH_IMAGE322
颗卫星计算伪距
Figure 178268DEST_PATH_IMAGE338
与测量伪距
Figure 589658DEST_PATH_IMAGE339
之差记为伪距差
Figure 762013DEST_PATH_IMAGE340
，模型如下，
Figure 651472DEST_PATH_IMAGE341
(30)
式中，
Figure 592883DEST_PATH_IMAGE342
为伪距测量噪声；
上式写为矩阵形式为：
Figure 909595DEST_PATH_IMAGE343
(31)
公式（31）中矩阵仅列出第一颗星、第二颗星和第N颗星信息，第3颗星至第N-1颗星信息用省略号“
Figure 303667DEST_PATH_IMAGE125
”代替；
直角坐标和大地坐标之间的转换关系可由下式表示，
Figure 996817DEST_PATH_IMAGE344
(32)
式中，R N 为卯酉圈内的地球曲率半径；由公式（32）可得坐标误差，
Figure 291269DEST_PATH_IMAGE345
(33)
将系数矩阵记为
Figure 778883DEST_PATH_IMAGE128
，即
Figure 925830DEST_PATH_IMAGE346
则
Figure 157091DEST_PATH_IMAGE130
(34)
将（34）式代入（30）式得到伪距差模型为：
Figure 807516DEST_PATH_IMAGE347
(35)
式中元素
Figure 731609DEST_PATH_IMAGE132
计算公式为：
Figure 100274DEST_PATH_IMAGE133
(36)
(2)建立伪距率差模型
由（30）式可得伪距率差的模型为：
Figure 869647DEST_PATH_IMAGE134
(37)
其中
Figure 138692DEST_PATH_IMAGE135
、
Figure 233687DEST_PATH_IMAGE348
、
Figure 89647DEST_PATH_IMAGE137
为在地球直角坐标系中表示的速度误差，通过坐标变换矩阵将其变换到地理坐标系中，变换公式为，
Figure 662711DEST_PATH_IMAGE349
(38)
水面舰船忽略垂向速度误差，则（38）式简写为下式
Figure 287727DEST_PATH_IMAGE350
(39)
令
Figure 553624DEST_PATH_IMAGE140
则得到
Figure 631301DEST_PATH_IMAGE141
(40)
将（40）式代入（37）式得到伪距率差模型为：
Figure 273635DEST_PATH_IMAGE351
(41)
式中元素
Figure 245834DEST_PATH_IMAGE143
计算公式为：
Figure 682631DEST_PATH_IMAGE352
(42)
（3）建立Kalman滤波观测方程
基于伪距和伪距率的惯导与卫导紧耦合滤波器选择惯导和卫导的伪距差、伪距率差作为观测量，Kalman滤波观测方程记为：
Figure 247605DEST_PATH_IMAGE145
(43)
式中
Figure 162471DEST_PATH_IMAGE146
为t时刻观测矩阵，
Figure 762080DEST_PATH_IMAGE147
为t时刻观测噪声向量，记系统过程噪声向量
Figure 369779DEST_PATH_IMAGE148
的方差强度阵为
Figure 422048DEST_PATH_IMAGE149
t时刻第
Figure 140605DEST_PATH_IMAGE150
颗卫星观测模型为：
Figure 93256DEST_PATH_IMAGE353
观测量计算公式为，
Figure 871856DEST_PATH_IMAGE354
(44)
相应的，t时刻第
Figure 411422DEST_PATH_IMAGE153
颗卫星观测矩阵
Figure 933670DEST_PATH_IMAGE154
的定义如下，
Figure 242291DEST_PATH_IMAGE155
。
步骤3、进行Kalman滤波解算
状态方程和观测方程组成随机连续系统，模型如下：
Figure 191793DEST_PATH_IMAGE156
(45)
Figure 953075DEST_PATH_IMAGE157
(46)
进行Kalman滤波解算包括如下步骤：
（1）随机连续系统离散化
为了适应工程应用需要将随机连续系统（45）、（46）离散化，根据线性系统理论得到随机线性离散系统的状态方程，
Figure 279015DEST_PATH_IMAGE355
(47)
式中，
Figure 940678DEST_PATH_IMAGE159
为k-1时刻至k时刻离散化的状态转移矩阵，以
Figure 61081DEST_PATH_IMAGE160
为时间长度将k-1时刻至k时刻之间分为n段，每个
Figure 575239DEST_PATH_IMAGE356
时间内的状态转移矩阵为
Figure 439289DEST_PATH_IMAGE162
，n个
Figure 456924DEST_PATH_IMAGE163
时间内的状态转移矩阵
Figure 13807DEST_PATH_IMAGE357
相乘构成k-1时刻至k时刻状态转移矩阵
Figure 484103DEST_PATH_IMAGE358
，
Figure 886265DEST_PATH_IMAGE166
计算公式如下，
Figure 522521DEST_PATH_IMAGE167
(48)
Figure 250305DEST_PATH_IMAGE359
(49)
k时刻离散系统噪声矩阵
Figure 207897DEST_PATH_IMAGE360
计算公式如下，
Figure 679330DEST_PATH_IMAGE361
(50)
Figure 671556DEST_PATH_IMAGE362
(51)
Figure 304663DEST_PATH_IMAGE363
(52)
Figure 15130DEST_PATH_IMAGE173
为状态矩阵
Figure 24674DEST_PATH_IMAGE174
更新时间间隔，与导航解算时间间隔相关，导航解算间隔
Figure 375802DEST_PATH_IMAGE175
；
（2）基于上述随机线性离散系统的状态方程进行Kalman滤波解算，解算公式包括：
状态一步预测
Figure 445389DEST_PATH_IMAGE176
、一步预测误差方差阵
Figure 377573DEST_PATH_IMAGE177
、状态估计
Figure 190809DEST_PATH_IMAGE178
、滤波增益矩阵
Figure 157628DEST_PATH_IMAGE179
、估计误差的方差阵
Figure 132537DEST_PATH_IMAGE180
公式分别如下：
Figure 817596DEST_PATH_IMAGE181
(53)
Figure 168943DEST_PATH_IMAGE182
54)
Figure 488804DEST_PATH_IMAGE183
(55)
Figure 634614DEST_PATH_IMAGE184
(56)
Figure 541390DEST_PATH_IMAGE185
(57)
Figure 696428DEST_PATH_IMAGE364
为k-1时刻状态向量的滤波估计值，初值为零；
Figure 372260DEST_PATH_IMAGE187
为k-1时刻系统滤波估计方差矩阵，初值
Figure 954551DEST_PATH_IMAGE188
与惯导系统误差相关；本例中
Figure 348623DEST_PATH_IMAGE188
为
Figure 307352DEST_PATH_IMAGE189
矩阵，非零元素为：
Figure 336226DEST_PATH_IMAGE190
，
Figure 89418DEST_PATH_IMAGE365
，
Figure 705207DEST_PATH_IMAGE192
，
Figure 467627DEST_PATH_IMAGE366
，
Figure 852472DEST_PATH_IMAGE194
，
Figure 42145DEST_PATH_IMAGE195
，
Figure 145230DEST_PATH_IMAGE196
，
Figure 445761DEST_PATH_IMAGE367
，
Figure 183648DEST_PATH_IMAGE198
，
Figure 544222DEST_PATH_IMAGE368
，
Figure 869024DEST_PATH_IMAGE369
，
Figure 707667DEST_PATH_IMAGE201
，
Figure 332684DEST_PATH_IMAGE202
，
Figure 864159DEST_PATH_IMAGE370
，
Figure 941836DEST_PATH_IMAGE371
。
步骤4、序贯法依次计算多颗卫星伪距和伪距率信息
根据上述解算方程，序贯法依次计算多颗卫星伪距和伪距率信息，将求逆的矩阵维数降低至两维，有效提高解算效率，避免大矩阵求逆可能带来的奇异值问题。依次处理k时刻第j颗卫星的观测信息，见式（58）：
Figure 318591DEST_PATH_IMAGE372
(58)
滤波公式为：
Figure 48648DEST_PATH_IMAGE373
(59)
Figure DEST_PATH_IMAGE374
(60)
(61)
K时刻N颗星信息全部计算后，将得到的与分别赋值给这个k时刻校正后的状态及其均方误差阵，完成该时刻滤波估计，见式（62）和（63）。
(62)
(63)。
步骤5、定时校正惯导误差和误差源
每4小时修正一次惯导系统，校正量包括惯导系统的经度误差
Figure 954287DEST_PATH_IMAGE213
、纬度误差
Figure 784840DEST_PATH_IMAGE214
、东向速度误差
Figure 965286DEST_PATH_IMAGE215
、北向速度误差
Figure 564894DEST_PATH_IMAGE216
、航向角误差
Figure 172593DEST_PATH_IMAGE217
、北向陀螺常值漂移
Figure 959283DEST_PATH_IMAGE218
和方位陀螺常值漂移
Figure 441955DEST_PATH_IMAGE219
；校正量为校正时刻的滤波估计值，见式（64）：
Figure 896070DEST_PATH_IMAGE375
(64)。
综上，本发明构成了基于卫导伪距和伪距率的紧耦合卡尔曼滤波器，采用递推算法实时估算各状态变量，实现了定期修正惯导系统误差，提高了惯导导航精度，延长惯导重调周期，保证了惯导长周期导航任务需求，对深远海航行具有重要意义。
尽管为说明目的公开了本发明的实施例，但是本领域的技术人员可以理解：在不脱离本发明及所附权利要求的精神范围内，各种替换、变化和修改都是可以的，因此，本发明的范围不局限于实施例所公开的内容。

Claims (6)

1.一种基于卫导伪距和伪距率的惯导校正方法，其特征在于，包括如下步骤：
步骤1、以惯导误差方程和卫导误差方程为基础建立Kalman滤波状态方程；
步骤2、以卫导伪距差模型、伪距率差模型作为观测量，建立Kalman滤波观测方程；
步骤3、对步骤1建立的Kalman滤波状态方程和步骤2建立的Kalman滤波观测方程进行Kalman滤波解算；
步骤4、依据步骤3得到的滤波解算方式，采用序贯法依次计算多颗卫星伪距和伪距率信息：
步骤5、依据步骤4得到的伪距和伪距率信息，定时校正惯导误差和误差源。
2.根据权利要求1所述的基于卫导伪距和伪距率的惯导校正方法，其特征在于：步骤1包括：
（1）建立惯导误差方程
惯导系统主要误差包括经度误差
Figure DEST_PATH_IMAGE001
、纬度误差
Figure 694324DEST_PATH_IMAGE002
、东速误差
Figure DEST_PATH_IMAGE003
、北速误差
Figure 827365DEST_PATH_IMAGE004
、纵摇误差
Figure DEST_PATH_IMAGE005
、横摇误差
Figure 677509DEST_PATH_IMAGE006
、航向误差
Figure DEST_PATH_IMAGE007
，误差源主要为陀螺常值漂移、陀螺随机漂移、加速度计零偏误差，其中，陀螺常值漂移包括东向陀螺常值漂移
Figure 343720DEST_PATH_IMAGE008
、北向陀螺常值漂移
Figure DEST_PATH_IMAGE009
和方位陀螺常值漂移
Figure 886697DEST_PATH_IMAGE010
；陀螺随机漂移包括东向陀螺随机漂移
Figure DEST_PATH_IMAGE011
、北向陀螺随机漂移
Figure 241455DEST_PATH_IMAGE012
和方位陀螺随机漂移
Figure DEST_PATH_IMAGE013
；加速度计零偏误差包括东向加速度计零偏误差
Figure 160870DEST_PATH_IMAGE014
、北向加速度计零偏误差
Figure DEST_PATH_IMAGE015
；各误差误差微分方程为：
Figure 684517DEST_PATH_IMAGE016
(1)
Figure DEST_PATH_IMAGE017
(2)
Figure 398395DEST_PATH_IMAGE018
(3)
Figure DEST_PATH_IMAGE019
(4)
Figure 240449DEST_PATH_IMAGE020
(5)
Figure DEST_PATH_IMAGE021
(6)
Figure 697975DEST_PATH_IMAGE022
(7)
Figure DEST_PATH_IMAGE023
(8)
Figure 309085DEST_PATH_IMAGE024
(9)
Figure DEST_PATH_IMAGE025
(10)
Figure 686540DEST_PATH_IMAGE026
(11)
Figure DEST_PATH_IMAGE027
(12)
Figure 281470DEST_PATH_IMAGE028
(13)
Figure DEST_PATH_IMAGE029
(14)
Figure 542687DEST_PATH_IMAGE030
(15)
式中：
Figure DEST_PATH_IMAGE031
——重力加速度，与当地纬度有关，球面上的重力公式为
Figure 775347DEST_PATH_IMAGE032
，
Figure DEST_PATH_IMAGE033
；
Figure 96607DEST_PATH_IMAGE034
——子午圈和卯酉圈内的地球曲率半径；
Figure DEST_PATH_IMAGE035
——地球自转角速度，
Figure 913254DEST_PATH_IMAGE036
；
Figure DEST_PATH_IMAGE037
——表示东向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 978162DEST_PATH_IMAGE038
——表示北向陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure DEST_PATH_IMAGE039
——表示方位陀螺随机漂移一阶马尔科夫模型相关频率参数；
Figure 327978DEST_PATH_IMAGE040
——东向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure DEST_PATH_IMAGE041
——北向陀螺随机漂移一阶马尔科夫模型随机噪声；
Figure 554560DEST_PATH_IMAGE042
——方位陀螺随机漂移一阶马尔科夫模型随机噪声；
将惯导误差和误差源作为惯导误差变量，记为：
Figure DEST_PATH_IMAGE043
惯导误差方程写成矩阵形式为：
Figure 858502DEST_PATH_IMAGE044
(16)
式中：
Figure DEST_PATH_IMAGE045
——表示
Figure 461522DEST_PATH_IMAGE046
时刻惯导误差方程观测矩阵，具体为
Figure DEST_PATH_IMAGE047
为15行3列矩阵，
Figure 403196DEST_PATH_IMAGE048
Figure DEST_PATH_IMAGE049
——表示
Figure 66258DEST_PATH_IMAGE046
时刻惯导测量噪声，具体为
Figure 60759DEST_PATH_IMAGE050
；
Figure DEST_PATH_IMAGE051
——表示
Figure 467469DEST_PATH_IMAGE046
时刻惯导误差方程观测矩阵，
Figure 496605DEST_PATH_IMAGE052
为15行15列矩阵，以
Figure DEST_PATH_IMAGE053
表示矩阵
Figure 834964DEST_PATH_IMAGE054
第
Figure DEST_PATH_IMAGE055
行
Figure 847919DEST_PATH_IMAGE056
列元素，则矩阵
Figure DEST_PATH_IMAGE057
非零元素为：
Figure 792741DEST_PATH_IMAGE058
，
Figure DEST_PATH_IMAGE059
，
Figure 207542DEST_PATH_IMAGE060
，
Figure DEST_PATH_IMAGE061
，
Figure 448293DEST_PATH_IMAGE062
，
Figure DEST_PATH_IMAGE063
，
Figure 214124DEST_PATH_IMAGE064
，
Figure DEST_PATH_IMAGE065
，
Figure 962637DEST_PATH_IMAGE066
，
Figure DEST_PATH_IMAGE067
，
Figure 966365DEST_PATH_IMAGE068
，
Figure DEST_PATH_IMAGE069
，
Figure 906246DEST_PATH_IMAGE070
，
Figure DEST_PATH_IMAGE071
，
Figure 893793DEST_PATH_IMAGE072
，
Figure DEST_PATH_IMAGE073
，
Figure 914839DEST_PATH_IMAGE074
，
Figure DEST_PATH_IMAGE075
，
Figure 304232DEST_PATH_IMAGE076
，
Figure DEST_PATH_IMAGE077
，
Figure 152364DEST_PATH_IMAGE078
，
Figure DEST_PATH_IMAGE079
；
（2）建立卫导误差方程
卫导误差包含两个与时间有关的误差：一个是与时钟误差等效的距离误差
Figure 627208DEST_PATH_IMAGE080
，另一个是与时钟频率误差等效的距离率误差
Figure DEST_PATH_IMAGE081
，其微分方程为：
Figure 451945DEST_PATH_IMAGE082
(17)
Figure DEST_PATH_IMAGE083
(18)
式中：
Figure 695844DEST_PATH_IMAGE084
——距离误差
Figure DEST_PATH_IMAGE085
的测量噪声；
Figure 729526DEST_PATH_IMAGE086
——距离率误差
Figure DEST_PATH_IMAGE087
的测量噪声；
Figure 426087DEST_PATH_IMAGE088
——距离率误差
Figure DEST_PATH_IMAGE089
的一阶马尔科夫模型相关频率参数；
将卫导误差方程写成矩阵的形式为
Figure 54514DEST_PATH_IMAGE090
(19)
式中：
Figure DEST_PATH_IMAGE091
——表示
Figure 418500DEST_PATH_IMAGE046
时刻卫导测量噪声，具体为
Figure 77276DEST_PATH_IMAGE092
；
Figure DEST_PATH_IMAGE093
——表示
Figure 261133DEST_PATH_IMAGE046
时刻卫导误差方程观测矩阵，具体为
Figure 162093DEST_PATH_IMAGE094
；
Figure DEST_PATH_IMAGE095
——表示
Figure 380585DEST_PATH_IMAGE046
时刻卫导误差方程观测矩阵，具体为
Figure 708798DEST_PATH_IMAGE096
；
（3）建立Kalman滤波状态方程
将惯导误差方程与卫导误差方程合并，得到kalman滤波状态方程，如下所示：
Figure DEST_PATH_IMAGE097
(20)
即
Figure 878486DEST_PATH_IMAGE098
(21)
式中
Figure DEST_PATH_IMAGE099
为t时刻状态转移矩阵，
Figure 848716DEST_PATH_IMAGE100
为t时刻系统过程噪声输入矩阵，
Figure DEST_PATH_IMAGE101
为
Figure 921714DEST_PATH_IMAGE102
时刻系统过程噪声向量，系统过程噪声向量
Figure 420828DEST_PATH_IMAGE103
的方差强度阵记为
Figure DEST_PATH_IMAGE104
。
3.根据权利要求2所述的基于卫导伪距和伪距率的惯导校正方法，其特征在于，步骤2包括：
（1）建立伪距差模型
设惯导位置坐标为
Figure 579277DEST_PATH_IMAGE105
，当前时刻收到
Figure 323505DEST_PATH_IMAGE106
颗卫星信息；
第
Figure DEST_PATH_IMAGE107
颗卫星的位置坐标为
Figure 251009DEST_PATH_IMAGE108
，
Figure DEST_PATH_IMAGE109
，依次计算
Figure 452184DEST_PATH_IMAGE106
颗星伪距
Figure 566770DEST_PATH_IMAGE110
，计算公式如下：
Figure 613224DEST_PATH_IMAGE111
(22)
设载体的真实位置坐标为
Figure DEST_PATH_IMAGE112
，将伪距
Figure 899629DEST_PATH_IMAGE113
一阶泰勒展开，有：
Figure DEST_PATH_IMAGE114
(23)
令
Figure 271705DEST_PATH_IMAGE115
(24)
则有
Figure DEST_PATH_IMAGE116
(25)
Figure 404746DEST_PATH_IMAGE117
(26)
Figure DEST_PATH_IMAGE118
(27)
因此第
Figure 520469DEST_PATH_IMAGE107
颗卫星伪距
Figure 891408DEST_PATH_IMAGE110
模型为：
Figure 404691DEST_PATH_IMAGE119
(28)
卫星接收机输出的第
Figure 493870DEST_PATH_IMAGE107
颗卫星测量伪距
Figure DEST_PATH_IMAGE120
可表示为，
Figure 147705DEST_PATH_IMAGE121
(29)
第
Figure 638729DEST_PATH_IMAGE107
颗卫星计算伪距
Figure DEST_PATH_IMAGE122
与测量伪距
Figure 618187DEST_PATH_IMAGE123
之差记为伪距差
Figure DEST_PATH_IMAGE124
，模型如下，
Figure 958776DEST_PATH_IMAGE125
(30)
式中，
Figure DEST_PATH_IMAGE126
为伪距测量噪声；
上式写为矩阵形式为：
Figure 416302DEST_PATH_IMAGE127
(31)
公式（31）中矩阵仅列出第一颗星、第二颗星和第N颗星信息，第3颗星至第N-1颗星信息用省略号“
Figure 761833DEST_PATH_IMAGE129
”代替；
直角坐标和大地坐标之间的转换关系可由下式表示，
Figure DEST_PATH_IMAGE130
(32)
式中，R N 为卯酉圈内的地球曲率半径；由公式（32）可得坐标误差，
Figure 646612DEST_PATH_IMAGE131
(33)
将系数矩阵记为
Figure DEST_PATH_IMAGE132
，即
Figure 975962DEST_PATH_IMAGE133
则
Figure DEST_PATH_IMAGE134
(34)
将（34）式代入（30）式得到伪距差模型为：
Figure 738644DEST_PATH_IMAGE135
(35)
式中元素
Figure DEST_PATH_IMAGE136
计算公式为：
Figure 735419DEST_PATH_IMAGE137
(36)
(2)建立伪距率差模型
由（30）式可得伪距率差的模型为：
Figure DEST_PATH_IMAGE138
(37)
其中
Figure 525520DEST_PATH_IMAGE139
、
Figure DEST_PATH_IMAGE140
、
Figure 342167DEST_PATH_IMAGE141
为在地球直角坐标系中表示的速度误差，通过坐标变换矩阵将其变换到地理坐标系中，变换公式为，
Figure DEST_PATH_IMAGE142
(38)
水面舰船忽略垂向速度误差，则（38）式简写为下式
Figure 634171DEST_PATH_IMAGE143
(39)
令
Figure DEST_PATH_IMAGE144
则得到
Figure 485453DEST_PATH_IMAGE145
(40)
将（40）式代入（37）式得到伪距率差模型为：
Figure DEST_PATH_IMAGE146
(41)
式中元素
Figure 977614DEST_PATH_IMAGE147
计算公式为：
Figure DEST_PATH_IMAGE148
(42)
（3）建立Kalman滤波观测方程
基于伪距和伪距率的惯导与卫导紧耦合滤波器选择惯导和卫导的伪距差、伪距率差作为观测量，Kalman滤波观测方程记为：
Figure 783021DEST_PATH_IMAGE149
(43)
式中
Figure DEST_PATH_IMAGE150
为t时刻观测矩阵，
Figure 120461DEST_PATH_IMAGE151
为t时刻观测噪声向量，记系统过程噪声向量
Figure DEST_PATH_IMAGE152
的方差强度阵为
Figure 91829DEST_PATH_IMAGE153
t时刻第
Figure DEST_PATH_IMAGE154
颗卫星观测模型为：
Figure 489312DEST_PATH_IMAGE155
观测量计算公式为，
Figure DEST_PATH_IMAGE156
(44)
相应的，t时刻第
Figure 779085DEST_PATH_IMAGE157
颗卫星观测矩阵
Figure DEST_PATH_IMAGE158
的定义如下，
Figure 654638DEST_PATH_IMAGE159
。
4.根据权利要求3所述的基于卫导伪距和伪距率的惯导校正方法，其特征在于，步骤3中：
状态方程和观测方程组成随机连续系统，模型如下：
Figure DEST_PATH_IMAGE160
(45)
Figure 214932DEST_PATH_IMAGE161
(46)
进行Kalman滤波解算包括如下步骤：
（1）随机连续系统离散化
为了适应工程应用需要将随机连续系统（45）、（46）离散化，根据线性系统理论得到随机线性离散系统的状态方程，
Figure DEST_PATH_IMAGE162
(47)
式中，
Figure 48896DEST_PATH_IMAGE163
为k-1时刻至k时刻离散化的状态转移矩阵，以
Figure DEST_PATH_IMAGE164
为时间长度将k-1时刻至k时刻之间分为n段，每个
Figure 563316DEST_PATH_IMAGE165
时间内的状态转移矩阵为
Figure DEST_PATH_IMAGE166
，n个
Figure 508138DEST_PATH_IMAGE167
时间内的状态转移矩阵
Figure DEST_PATH_IMAGE168
相乘构成k-1时刻至k时刻状态转移矩阵
Figure 922939DEST_PATH_IMAGE169
，
Figure DEST_PATH_IMAGE170
计算公式如下，
Figure 927804DEST_PATH_IMAGE171
(48)
Figure DEST_PATH_IMAGE172
(49)
k时刻离散系统噪声矩阵
Figure 932450DEST_PATH_IMAGE173
计算公式如下，
Figure DEST_PATH_IMAGE174
(50)
Figure 415384DEST_PATH_IMAGE175
(51)
Figure DEST_PATH_IMAGE176
(52)
Figure 950271DEST_PATH_IMAGE177
为状态矩阵
Figure DEST_PATH_IMAGE178
更新时间间隔，与导航解算时间间隔相关，导航解算间隔
Figure 126037DEST_PATH_IMAGE179
；
（2）基于上述随机线性离散系统的状态方程进行Kalman滤波解算，解算公式包括：
状态一步预测
Figure DEST_PATH_IMAGE180
、一步预测误差方差阵
Figure 615049DEST_PATH_IMAGE181
、状态估计
Figure DEST_PATH_IMAGE182
、滤波增益矩阵
Figure 901674DEST_PATH_IMAGE183
、估计误差的方差阵
Figure DEST_PATH_IMAGE184
公式分别如下：
Figure 25488DEST_PATH_IMAGE185
(53)
Figure DEST_PATH_IMAGE186
(54)
Figure 372156DEST_PATH_IMAGE187
(55)
Figure DEST_PATH_IMAGE188
(56)
Figure 345535DEST_PATH_IMAGE189
(57)
Figure DEST_PATH_IMAGE190
为k-1时刻状态向量的滤波估计值，初值为零；
Figure 435850DEST_PATH_IMAGE191
为k-1时刻系统滤波估计方差矩阵，初值
Figure DEST_PATH_IMAGE192
与惯导系统误差相关；
Figure 679750DEST_PATH_IMAGE192
为
Figure DEST_PATH_IMAGE194
矩阵，非零元素为：
Figure 931740DEST_PATH_IMAGE195
，
Figure DEST_PATH_IMAGE196
，
Figure 395344DEST_PATH_IMAGE197
，
Figure DEST_PATH_IMAGE198
，
Figure 758193DEST_PATH_IMAGE199
，
Figure DEST_PATH_IMAGE200
，
Figure 122178DEST_PATH_IMAGE201
，
Figure DEST_PATH_IMAGE202
，
Figure 545069DEST_PATH_IMAGE203
，
Figure DEST_PATH_IMAGE204
，
Figure 994505DEST_PATH_IMAGE205
，
Figure DEST_PATH_IMAGE206
，
Figure 942736DEST_PATH_IMAGE207
，
Figure DEST_PATH_IMAGE208
，
Figure 895649DEST_PATH_IMAGE209
。
5.根据权利要求4所述的基于卫导伪距和伪距率的惯导校正方法，其特征在于，步骤4中：
依次处理k时刻第j颗卫星的观测信息，如下式：
Figure DEST_PATH_IMAGE210
(58)
第j颗卫星滤波计算公式为：
Figure 755020DEST_PATH_IMAGE211
(59)
Figure DEST_PATH_IMAGE212
(60)
Figure 426173DEST_PATH_IMAGE213
(61)
K时刻卫星信息全部计算后，将得到的
Figure DEST_PATH_IMAGE214
与
Figure 163447DEST_PATH_IMAGE215
分别赋值给这个k时刻校正后的状态及其均方误差阵，完成k时刻滤波估计，见式（62）和（63）；
Figure DEST_PATH_IMAGE216
(62)
Figure 236446DEST_PATH_IMAGE217
(63)。
6.根据权利要求5所述的基于卫导伪距和伪距率的惯导校正方法，其特征在于，步骤5中：
每4小时修正一次惯导系统，校正量包括惯导系统的经度误差
Figure DEST_PATH_IMAGE218
、纬度误差
Figure 266719DEST_PATH_IMAGE219
、东向速度误差
Figure DEST_PATH_IMAGE220
、北向速度误差
Figure 425167DEST_PATH_IMAGE221
、航向角误差
Figure DEST_PATH_IMAGE222
、北向陀螺常值漂移
Figure 697623DEST_PATH_IMAGE223
和方位陀螺常值漂移
Figure DEST_PATH_IMAGE224
；校正量为校正时刻的滤波估计值，见式（64）：
Figure 625128DEST_PATH_IMAGE225
（64）。
CN202211134731.6A 2022-09-15 2022-09-15 一种基于伪距和伪距率的惯导校正方法 Active CN115235513B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211134731.6A CN115235513B (zh) 2022-09-15 2022-09-15 一种基于伪距和伪距率的惯导校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211134731.6A CN115235513B (zh) 2022-09-15 2022-09-15 一种基于伪距和伪距率的惯导校正方法

Publications (2)

Publication Number Publication Date
CN115235513A true CN115235513A (zh) 2022-10-25
CN115235513B CN115235513B (zh) 2023-01-17

Family

ID=83680424

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211134731.6A Active CN115235513B (zh) 2022-09-15 2022-09-15 一种基于伪距和伪距率的惯导校正方法

Country Status (1)

Country Link
CN (1) CN115235513B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117214933A (zh) * 2023-11-07 2023-12-12 中国船舶集团有限公司第七〇七研究所 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法

Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN102998685A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种惯性/卫星导航系统伪距/伪距率紧组合方法
US20140152493A1 (en) * 2012-11-30 2014-06-05 Applanix Inc. Quasi tightly coupled gnss-ins integration process
CN103969672A (zh) * 2014-05-14 2014-08-06 东南大学 一种多卫星系统与捷联惯性导航系统紧组合导航方法
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
US20190129044A1 (en) * 2016-07-19 2019-05-02 Southeast University Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling
CN110907975A (zh) * 2019-12-13 2020-03-24 北京遥测技术研究所 一种基于序贯最小二乘的模糊度固定方法
CN111044075A (zh) * 2019-12-10 2020-04-21 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
WO2021151864A1 (fr) * 2020-01-27 2021-08-05 Safran Electronics & Defense Procede et systeme de navigation
CN113253325A (zh) * 2021-06-24 2021-08-13 中国人民解放军国防科技大学 惯性卫星序贯紧组合李群滤波方法
CN114964235A (zh) * 2022-05-19 2022-08-30 哈尔滨工业大学(鞍山)工业技术研究院 一种基于惯性/多普勒计程仪加阻尼状态的组合导航方法

Patent Citations (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101561496A (zh) * 2009-05-21 2009-10-21 北京航空航天大学 一种伪卫星和惯性组合导航系统的非线性补偿方法
CN102998685A (zh) * 2011-09-15 2013-03-27 北京自动化控制设备研究所 一种惯性/卫星导航系统伪距/伪距率紧组合方法
US20140152493A1 (en) * 2012-11-30 2014-06-05 Applanix Inc. Quasi tightly coupled gnss-ins integration process
CN103969672A (zh) * 2014-05-14 2014-08-06 东南大学 一种多卫星系统与捷联惯性导航系统紧组合导航方法
US20190129044A1 (en) * 2016-07-19 2019-05-02 Southeast University Cubature Kalman Filtering Method Suitable for High-dimensional GNSS/INS Deep Coupling
CN108426574A (zh) * 2018-02-02 2018-08-21 哈尔滨工程大学 一种基于zihr的航向角修正算法的mems行人导航方法
CN111044075A (zh) * 2019-12-10 2020-04-21 上海航天控制技术研究所 基于卫星伪距/相对测量信息辅助的sins误差在线修正方法
CN110907975A (zh) * 2019-12-13 2020-03-24 北京遥测技术研究所 一种基于序贯最小二乘的模糊度固定方法
WO2021151864A1 (fr) * 2020-01-27 2021-08-05 Safran Electronics & Defense Procede et systeme de navigation
CN113253325A (zh) * 2021-06-24 2021-08-13 中国人民解放军国防科技大学 惯性卫星序贯紧组合李群滤波方法
CN114964235A (zh) * 2022-05-19 2022-08-30 哈尔滨工业大学(鞍山)工业技术研究院 一种基于惯性/多普勒计程仪加阻尼状态的组合导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
YU HANG等: "《Data fusion for a GPS/INS tightly coupled positioning system with equality and inequality constraints using an aggregate constraint unscented Kalman filter》", 《JOURNAL OF SPATIAL SCIENCE》 *
刘攀: "《基于GPS/INS组合系统的卡尔曼滤波算法研究》", 《中国优秀硕士全文数据库 信息科技辑》 *
臧中原: "《基于伪距、伪距率的SINS/GPS紧组合导航系统研究》", 《中国优秀硕士学位论文全文数据库 信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117214933A (zh) * 2023-11-07 2023-12-12 中国船舶集团有限公司第七〇七研究所 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法
CN117214933B (zh) * 2023-11-07 2024-02-06 中国船舶集团有限公司第七〇七研究所 水面船用惯导/北斗紧耦合长周期惯导速度品质提升方法

Also Published As

Publication number Publication date
CN115235513B (zh) 2023-01-17

Similar Documents

Publication Publication Date Title
CN101949703B (zh) 一种捷联惯性/卫星组合导航滤波方法
CN109324330B (zh) 基于混合无导数扩展卡尔曼滤波的usbl/sins紧组合导航定位方法
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
JP4789216B2 (ja) ナビゲーション用途のための、改良されたｇｐｓ累積デルタ距離処理方法
CN103900565B (zh) 一种基于差分gps的惯导系统姿态获取方法
CN108226985B (zh) 基于精密单点定位的列车组合导航方法
CN109931955B (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN105371844B (zh) 一种基于惯性/天文互助的惯性导航系统初始化方法
CN105091907B (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN103344260B (zh) 基于rbckf的捷联惯导系统大方位失准角初始对准方法
CN106507913B (zh) 用于管道测绘的组合定位方法
CN103542854A (zh) 基于星载处理器的自主定轨方法
CN110567455B (zh) 一种求积更新容积卡尔曼滤波的紧组合导航方法
CN109507706B (zh) 一种gps信号丢失的预测定位方法
CN106840211A (zh) 一种基于kf和stupf组合滤波的sins大方位失准角初始对准方法
CN113375659B (zh) 一种基于星光角距测量信息的脉冲星导航方法
CN114777812B (zh) 一种水下组合导航系统行进间对准与姿态估计方法
CN103017787A (zh) 适用于摇摆晃动基座的初始对准方法
CN102116634A (zh) 一种着陆深空天体探测器的降维自主导航方法
CN111722295A (zh) 一种水下捷联式重力测量数据处理方法
Yoo et al. Improvement of TERCOM aided inertial navigation system by velocity correction
CN115235513B (zh) 一种基于伪距和伪距率的惯导校正方法
CN113253325A (zh) 惯性卫星序贯紧组合李群滤波方法
CN103575298A (zh) 基于自调节的ukf失准角初始对准方法
CN110926499A (zh) 基于李群最优估计的sins捷联惯性导航系统晃动基座自对准方法

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