导航系统

第1 章绪论
1.1 导航的基本概念
导航是引导运载体到达预定目的地的过程。

导航分两类：(1)自主式导航，用飞行器或船舶上的设备导航，有惯性导航、多普勒导航和天文导航等；(2)非自主式导航，用于飞行器、船舶、汽车等交通设备与有关的地面或空中设备相配合导航，有无线电导航、卫星导航。

在军事上，导航还要配合完成武器投射、侦察、巡逻、反潜和援救等任务。

高效、高精度的导航系统更是我国这种发展中国家赶超发达国家的战略性资源和倍能器。

在军用方面，随着新时期军事战略方针的转变及高新技术武器装备的发展，导航定位定向系统已经成为我军现代化建设中一项不可缺少的重要军事技术装备，其重要性表现在：它是信息战必不可少的基础设备，是建立战场统一坐标的前提，是快速、准确火力部署的保障，同时又是实现武器精确打击能力的必要条件。

所以，导航定位定向系统对迅速提高我军的综合作战能力，加快数字化部队建设至关重要；在民用方面，国外的导航定位定向系统己在大地测量、定向钻并、隧道掘进、地面车辆导航、飞机进场着陆、航天航空遥感、机载重力测量、公路监测、地下油气管道监测、矿井监测、激光断面监测等方面得到广泛地的应用，并取得了巨大的经济效益。

在日常生活中我们经常接触到的导航是车载导航，车载导航属于非自主式导航，车载导航是利用车载GPS（全球定位系统）配合电子地图来进行的，汽车GPS导航系统由两部分组成：一部分由安装在汽车上的GPS接收机和显示设备组成；另一部分由计算机控制中心组成，两部分通过定位卫星进行联系。

1.2 惯性导航(INS)概述
通常说的惯性技术，是惯性器件、惯性测量、惯性导航、惯性制导和惯性稳定等技术的统称。

惯性技术既是一门学科，也是一门工程技术，在陆、海、空、天各个领域有着广泛应用。

惯性器件（陀螺仪和加速度计）、惯性仪表、惯性导航系统都是以牛顿力学定律为基础的。

惯性导航系统通过加速度计实时测量载体运动的加速度，经积分运算得到载体的实时速度和位置信息。

惯性技术是对载体进行导航的关键技术之一，惯性技术是利用惯性原理或其它有关原理，自主测量和控制运载体运动过程的技术，惯性测量和惯性敏感器技。

现代惯性技术在各国政府雄厚资金的支持下，己经从最初的军事应用渗透到民用领域。

惯性技术在国防装备技术中占有非常重要的地位。

对于惯性制导的中远程导弹，一般说来命中精度70%取决于制导系统的精度。

对于导弹核潜艇，由于潜航时间长，其位置和速度是变化的，而这些数据是发射导弹的初始参数，直接影响导弹的命中精度，因而需要提供高精度位置、速度和垂直对准信号。

目前适用于潜艇的唯一导航设备就是惯性导航系统。

1.3 卫星导航概述
卫星导航是采用导航卫星对地面、海洋、空中和空间用户进行导航定位的技术。

卫星导航系统通过测定的载体到导航卫星的距离、距离差等参数，并结合获取的或计算机得到的导航卫星瞬时位置来确定载体位置。

目前主要有美国的GPS、俄罗斯的GLONASS、欧洲的GALILEO和我国的北斗导航卫星系统。

这里主要介绍GPS导航定位。

GPS系统主要由空间部分、地面部分及用户设备组成，GPS的空间部分由24颗卫星组成，包括21颗工作星和三颗备份卫星，位于距地面20200km高度的近圆轨道上，24颗卫星均匀分布在6个轨道面上，轨道倾角为55度，这样可以确保地球上的任何一个地方都可以同时观测到6～11颗卫星。

布置在轨道上的备份星，可随时进入工作状态。

GPS卫星导航系统具有全球覆盖、全天候工作、定位精度高和用途广的优点。

地面控制部分由一个主控站、三个注入站和若干监测站组成，主控站通过注入站向所有GPS卫星注入新的导航信息，确保卫星的导航数据和时钟信息的精确性。

用户设备部分包括接收机硬件和机内软件以及GPS数据的后处理软件包，其主要功能是能够捕获到按一定卫星截止角所选择的的待测卫星，并跟踪这些卫星的运行。

当接收机捕获到跟踪的卫星信号后，就可测量出接收天线至卫星的伪距和伪距率，解调出卫星轨道参数等数据。

1.4 组合导航概述
组合导航是现代导航理论和技术发展的必然结果。

每种单一的导航系统都存在着各自的独特性能和局限性，几种不同的单一系统组合在一起，利用多种信息
源互相补充，构成一种具有更多余度和更高导航精度的多功能系统即为组合导航系统。

根据不同的应用要求与目的，可以构成不同的组合导航系统，由于惯性导航系统的自主性，目前多以惯性导航系统作为主导航系统构成组合导航系统。

1.5 组合导航系统平台开发的现状及发展趋势
鉴于各种单一的各种导航系统有这样那样的缺点，上世纪70年代，现代组合导航系统在航海、航空与航天等领域随着现代高科技的发展应运而生。

随着电子计算机技术特别是微机技术的迅猛发展和现代控制系统理论的进步，组合导航技术开始迅猛发展起来。

过去单独使用的各种导航设备，通过微型电子计算机有机的组合到一起，发展各自的特点、扬长避短，组合导航成为目前导航技术发展的重要方向之一。

从上个世纪80年代开始，英、美、法等国的军方和民用部门开始了对GPS/INS 组合导航平台的研究。

80年代后期欧美各国在MS Visual Studio C++6.0仿真软件平台上，实现了INS/GPS组合导航的解算，同时在Windows2000 操作界面上实现了曲线和数据的同步显示。

近年来我国各大高校采用Visual C++6.0编程，在Windows操作系统下实现了人机界面和直观的数据显示。

该平台将组合导航的理论方案与导航设备相结合，可以进行室内试验系统的调试和研究，还可以提供直观的界面显示、数据图表等。

1.6 课题研究的意义
惯性导航系统( INS) 通过加速度计实时测量载体运动的加速度，经积分运算得到载体的实时速度和位置信息，是一种完全自主的导航系统,具有不依赖外界信息、隐蔽性好、抗辐射性强、全天候等优点,是机载设备中能提供多种导航参数的重要导航设备。

惯性导航系统一般由惯性传感器模块、导航解算模块、电路系统、电源模块、滤波模块和外壳等部分组成。

但它的定位误差随时间而积累, 长时间工作后会产生大的误差, 使得惯性导航系统不宜作远距离导航。

全球定位系统(GPS)继惯性导航以后导航技术的又一大进展，其由空间卫星、地面监控设施以及用户接收机三部分组成。

具有较高的导航精度,但是该系统不能提供如载体姿态等导航参数,且在载体上使用时,由于载体的机动运动,常使接收机不易捕获和跟踪卫星的载波信号,甚至对已跟踪的信号失锁。

因此,为了克服惯性导航与全球定位系统的缺点,多根据INS和GPS的导航功能互补的特点,以适当的方法将两者组合来提高系统的整体导航精度及导航性能以及空中对准和再对准的能力GPS 接收机在惯导位置和速度信息的辅助下,也将改善捕获、跟踪和再捕获的能力,并在卫星分布条件差或可见星少的情况下导航精度不致下降过大。

由于优点显著,GPS/ INS 组合系统被一致认为是载体最理想的组合导航系统。

1.7 课题研究内容及方案
平台开发内容的程序包括对微惯性传感器/组件的实时采集、曲线绘制、显示和存储。

组合导航平台可以实现MEMS IMU和GPS等姿态、位置、速度等解算结果的数据显示，曲线显示，数据存储等功能。

研究方案如下：
1.捷联惯导系统的工作原理；
2.推导捷联惯导系统姿态、位置、速度更新计算，采用Matlab/Vb编程测试；
3. 构建INS/GPS导航系统界面；
4. 完成对组合导航系统的实时采集、曲线的绘制、显示和存储程序编制；
5. 针对不同导航任务要求，编制系统测试界面与后台运行程序。

第 2 章惯性导航系统及GPS基本原理
2.1 惯性导航系统的组成及分类
惯性导航系统一般由惯性传感器模块、导航解算模块、电路系统、电源模块、滤波模块和外壳等部分组成。

其中，惯性传感器模块是惯性导航系统的核心，一般由三个陀螺仪和加速度计通过正交装配构成。

根据构建导航坐标系方法和途径的不同，可将惯性导航系统分为两种类型：采用物理平台模拟导航坐标系的系统称为平台式惯性导航系统；采用数学算法确定导航坐标系的系统称为捷联式惯性导航系统。

平台式惯性导航系统是用物理平台直接模拟导航坐标系，导航计算比较简单。

陀螺稳定平台能够隔离载体的角运动，给惯性器件提供较好的工作环境，系统的精度较高，但是物理平台本身结构复杂、体积大、制造成本高。

捷联式惯性导航系统由于没有实体物理平台，结构简单、体积小、维护方便，但惯性器件直接安装在运载体上，工作环境恶劣，对惯性器件的环境适应性要求很高。

同时，由于加速度计输出的加速度分量是沿载体坐标系轴向的，需经计算转换到某种导航坐标系中去，计算量大。

惯性导航完全不依赖外面的声、光、电、磁等传播信号，可以实时、高精度地输出所需要的全部导航参数信息，自主地进行定位、导航，不受地域的限制，不受自然环境和人为干扰的影响，隐蔽性好，不论外太空、空间、地面、地下、水面及水下都能全天候的可靠工作，这是其他导航技术，如天文导航、无线电导航与定位、卫星导航等无法实现的。

这些独特的优点使其成为国防、航天、航空、船舶与海洋、陆地交通等领域十分重要、不可替代的导航手段。

2.2 捷联式惯性导航系统的工作原理
“捷联”（strapdown）这一术语的英文原意是“捆绑”因此所谓的捷联式惯性导航系统就是将惯性测量装置的敏感器（陀螺仪和加速度计）直接固联在载体上。

陀螺仪用于测量载体坐标系的3个轴的角速度信息，并将信息传输给导航计算机，经过误差补偿计算后进行姿态矩阵计算。

加速度计组件用于测量载体坐标系的3个轴的加速度信息，并将信息同样传输到导航计算机，经过误差补偿计算后进行比力坐标系的变换。

加速度计测量的是载体坐标系(b系)相对于惯性空间
的加速度在载体坐标系中的投影b ib a ，该测试量也称为比力。

而对于捷联惯导系
统，导航计算机要在导航坐标系中完成，因此，首先要将机体系中的测试量b ib a 转
换导航坐标系中的物理量n ib a ，即实现由机体坐标系到导航坐标系的坐标转换。

这一转换由姿态矩阵n b C 完成，而n b C 是利用陀螺仪的输出b ib ω即载体相对惯性空间
转动的角速率在载体坐标系下的投影计算得到。

姿态矩阵是随时间的变化而不断变化的。

另外，从姿态矩阵中可以单值的确定飞行器的姿态角。

捷联式惯导系统中需要实时地求取姿态矩阵，以便提取飞行器姿态角（俯仰角、滚动角、航偏角）以及变换比力。

捷联惯导系统原理简图如图2.1所示
加速度
组合比力坐标变换导航解算
陀螺组
合姿态阵解算航向，姿态解算
指令解算
速度，位置初值
航向姿态姿态阵
初值+-b f ~n f ~
b
ib ω~b in ω~b
nb ω
~·X 图2.1 捷联惯导系统原理简图
2.3 常用坐标系
惯性导航中所采用的坐标系可分为惯性坐标系与非惯性坐标系两类。

惯性导航区别于其它类型的导航方案(如无线电导航、天文导航等)的根本不同之处就在于其导航原理是建立在牛顿力学定律一一又可称为惯性定争一的基础上的，“惯性导航”也因此而得名。

然而牛顿力学定律是在惯性空间内成立的，这就首先有
必要引入惯性坐标系，作为讨论惯性导航基本原理的坐标基准。

我们知道，对飞行器进行导航的主要目的就是要实时地确定其导航参数，如飞行器的姿态、位置、速度等。

飞行器的导航参数就是通过各个坐标系之间的关系来确定的，这些坐标系是区别于惯性坐标系、并根据导航的需要而选取的。

我们称之为非惯性坐标系，如地球坐标系、地理坐标系、导航坐标系、平台坐标系以及机体坐标系等等。

在惯性导航中常用的坐标系有以下几种：
(1)地心惯性坐标系(下标为i)— Oexiyizi
惯性坐标系是符合牛顿力学定律的坐标系，即是绝对静止或只做匀速直线运动的坐标系。

地心惯性坐标系的原点Oe。

选在地球的中心，zi轴选在沿地轴指向北极的方向上，而xi、 yi轴则在地球的赤道平面内，并指向空间的两颗恒星，xi yi zi 构成右手坐标系。

三个坐标轴指向惯性空间固定不动，此坐标系是惯性仪表测量参考基准。

(2)地球坐标系(下标为“)—OXeYeZe
地球坐标系是固连在地球上的坐标系，它相对惯性坐标系以地球自转角速率ωe旋转，ωe =15.041070/小时。

地球坐标系的原点在地球中心Oe，OeZe轴与OeZi轴重合，OeXeYe在赤道平面内，Xe轴指向格林威治经线，Ye轴指向东经90度方向。

(3)地理坐标系(下标为t)— OXiYz地理坐标系是在载体上用来表示载体所在位置的东向、北向和垂线方向的坐标系。

地理坐标系的原点O选在载体重心处，Xi指向东，Yi指向北，Zi沿垂线方向指向天(东北天)。

(4)导航坐标系(下标为n)— OXnYnZn导航坐标系是在导航时根据导航系统一Z 作的需要而选取的作为导航基准的坐标系。

当把导航坐标系选得与地理坐标系重合时，可将这种导航坐标系成为指北方位系统;为了适应在极区附近导航的需要往往将导航坐标系的Zn轴仍选的与Zi 轴重合，而使Xn与Xi及Yn与Yi之间相差一个自由方位角或游动方位角a，这种导航坐标系可称为自由方位系统或游动自由方位系统。

(5)机体坐标系(下标为b)— OXbYbZb
机体坐标下是固连在机体上的坐标系。

机体坐标系的坐标原点O 位于飞行器的重心处，Xb 沿机体横轴指向右，Yb 沿机体纵轴指向前，Zb 垂直于OXb Yb ，并沿飞行器的竖轴指向上OXbYbZ 。

构成右手坐标系。

机体坐标系相对地理坐标系的方位为飞机的姿态和航向。

(6)平台坐标系(下标为p) -OXpYpZp
平台坐标系是用惯导系统来复现导航坐标系时所获得的坐标系。

平台坐标系的坐标原点。

位于飞行器的重心处。

当惯导系统不存在误差时，平台坐标系与导航坐标系相重合;当惯导系统出现误差时，平台坐标系就要相对导航坐标系出现误差角，就是平台的姿态角α、β、γ。

2.4 参数说明
(1)位置
φ ：当地纬度，λ：当地经度，h ：当地高度。

(2)姿态角
ψ:为载体的航向角。

载体纵轴在水平面上的投影与地理子午线N 之间的夹角即为载体航向角。

航向角的数值是以地理北向为起点逆时针方向计算的。

γ:为载体的横滚角(也称为倾斜角)。

载体纵向对称面与纵向铅垂平面之间的夹角即为横滚角。

横滚角从铅垂平面算起，右倾为正，左倾为负。

θ:为载体的俯仰角。

载体纵轴和纵向水平轴之间的夹角即为俯仰角，向上为正，向下为负。

(3)比力
比力是指单位质量受到的位移加速度和重力加速度代数和，即单位质量上所受到外力作用的代数和。

n f :导航坐标系下的比力b f :机体坐标系下的比力
(4)角速度
角速度用带有上下标的符号表示，如:b ib ，其下标含义为b 系(机体坐标系)
相对于i 系(惯性坐标系)的转动角速度，上标含义为此角速度在b 系(机体坐标系)中的投影。

其它角速度符号含义与此相似。

(5)坐标系变换矩阵
坐标系变换矩阵也用带有上下标的符号表示，如:C，其含义为n系(导航坐标系)到b系(机体坐标系)的变换矩阵。

其它坐标系变换矩阵符号的含义与此相似。

(6)地球半径
把地球看作一个椭球体时:地球长轴半径Re=6378.393km，椭圆度e=1/298.257。

把地球看作一个球体时:地球半径Re=6317km。

(7)地球自转角速度
=15.0411˚/h=7.29212 X10-5rad/s
ie
(8)重力加速度
在地球表面附近，如果忽略向心加速度的影响，重力加速度的大小与P点到地球球心间距离的平方成反比，可以近似用下式计算得到:
g(h)=g0 X ( 1一2h/Re)
式中:g0为赤道表面上的重力加速度值，g0 =9.8m/s2; h为P点离地面的高度。

2.5 GPS导航原理
GPS导航系统的基本原理是测量出已知位置的卫星到用户接收机之间的距离，然后综合多颗卫星的数据就可知道接收机的具体位置。

要达到这一目的，卫星的位置可以根据星载时钟所记录的时间在卫星星历中查出。

而用户到卫星的距离则通过记录卫星信号传播到用户所经历的时间，再将其乘以光速得到（由于大气层电离层的干扰，这一距离并不是用户与卫星之间的真实距离，而是伪距（PR）：当GPS卫星正常工作时，会不断地用1和0二进制码元组成的伪随机码（简称伪码）发射导航电文。

GPS系统使用的伪码一共有两种，分别是民用的C/A码和军用的P(Y)码。

C/A码频率1.023MHz，重复周期一毫秒，码间距1微秒，相当于300m；P 码频率10.23MHz，重复周期266.4天，码间距0.1微秒，相当于30m。

而Y码是在P码的基础上形成的，保密性能更佳。

导航电文包括卫星星历、工作状况、时钟改正、电离层时延修正、大气折射修正等信息。

它是从卫星信号中解调制出来，以50b/s调制在载频上发射的。

导航电文每个主帧中包含5个子帧每帧长6s。

前
三帧各10个字码；每三十秒重复一次，每小时更新一次。

后两帧共15000b。

导航电文中的内容主要有遥测码、转换码、第1、2、3数据块，其中最重要的则为星历数据。

当用户接受到导航电文时，提取出卫星时间并将其与自己的时钟做对比便可得知卫星与用户的距离，再利用导航电文中的卫星星历数据推算出卫星发射电文时所处位置，用户在WGS-84大地坐标系中的位置速度等信息便可得知。

可见GPS导航系统卫星部分的作用就是不断地发射导航电文。

然而，由于用户接受机使用的时钟与卫星星载时钟不可能总是同步，所以除了用户的三维坐标x、y、z外，还要引进一个Δt即卫星与接收机之间的时间差作为未知数，然后用4个方程将这4个未知数解出来。

所以如果想知道接收机所处的位置，至少要能接收到4个卫星的信号。

GPS接收机可接收到可用于授时的准确至纳秒级的时间信息；用于预报未来几个月内卫星所处概略位置的预报星历；用于计算定位时所需卫星坐标的广播星历，精度为几米至几十米（各个卫星不同，随时变化）；以及GPS系统信息，如卫星状况等。

GPS接收机对码的量测就可得到卫星到接收机的距离，由于含有接收机卫星钟的误差及大气传播误差，故称为伪距。

对0A码测得的伪距称为UA码伪距，精度约为20米左右，对P码测得的伪距称为P码伪距，精度约为2米左右。

第 3 章 捷联惯性导航算法及组合导航原理
3.1 捷联惯性导航数学编排
捷联惯导系统的数学模型编排方程主要包括导航位置方程和姿态方程。

对于捷联式惯导系统来说，需要导航计算机来计算导航平台。

对于使用地理坐标系的
导航系统而言，首先要用捷联式陀螺仪测量的角速度b ib ω和导航计算机计算的角
速度p ip ω来计算飞机的姿态矩阵p b C ，然后从姿态矩阵的元素中提取飞机的姿态和航向信息，并把捷联式加速度计测量的比力信息b f 用姿态矩阵变换到导航坐标系的比力p f ，这样就可以在导航坐标系中求解飞机的速度和位置信息。

捷联惯导系统由于没有机械平台，不能直接测出姿态角和航向角，所以要得到姿态角和航向角，必须在导航计算机中建立“数学平台”，即通过机体坐标系相对地理坐标系的方向余弦阵来计算机体的姿态角和航向角。

在对捷联惯导系统工作机理进行全面分析的基础上，可以画出系统的原理方框图，如图3-1所示。

图3-1捷联惯性导航系统原理方块图
3.2 捷联惯性导航数学模型算法公式
据参考文献[1]，图3.1中各个模块的计算公式如下:
1.四元数Q 的即时修正
我们选择四元数法作为捷联矩阵即时修正的算法。

四元数是指由一个实数单位1和三个虚数单位i, j,k 组成并具有下列实元的数。

k p j p i p q 3210+++=λ （3.1）
机体坐标系相对平台坐标系的转动可以用四元数Q 来表示，即
b b b k q j q i q q Q 3210+++= （3.2）
Q 的即时修正可通过解下面的四元数微分方程来实现:
⎥⎥⎥⎥⎦
⎤⎢⎢⎢⎢⎣⎡⎥⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎢⎣⎡------=⎥⎥⎥⎥⎦⎤⎢⎢⎢⎢⎣⎡32103210000021q q q q q q q q b
pbx b pby b pbz b pbx b pbz b pby b pby b pbz b
pbx b pbz b pby b pbx ωωωωωωωωωωωω （3.3） 在求解公式((4.2)时，需要用到四元数的初始值，否则上式无法求解。

确定四元数初始值的方法，可以根据初始对准中确定的姿态矩阵初始值中的元素，并利用四元数和姿态矩阵各对应元素相等的关系来确定。

2.捷联矩阵T 的计算
对于平台惯导系统，由于平台的存在，比力分量可以由加速度计直接测得，但是对于捷联惯导系统，加速度计是沿机体坐标系安装的，它只能测量沿机体坐标系的比力分量，因此引入捷联矩阵来实现机体坐标系到平台坐标系的坐标转换，由于根据捷联矩阵的元素可以确定飞机的姿态角，所以又可以叫做飞机姿态矩阵;由于捷联矩阵起到了平台的作用，所有又可以叫做“数学平台”。

由式((3.3)得出的四元数后，根据式((3.4)即可计算出捷联矩阵。

⎥⎥⎥⎦
⎤⎢⎢⎢⎣
⎡+--+-+-+-++---+=232221201032203110322322212030212031302123222120)(2)(2)(2)
(2)(2)(2q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q T （3.4） 3.四元数Q 的最佳归一化 由于计算机的算法误差会导致捷联矩阵称为非正交矩阵，对捷联矩阵进行正
交化处理可以消除引起非正交的算法误差源的影响。

实现四元数的归一化也就是完成了捷联矩阵T 的正交化。

以欧几里德范数最小为指标的四元数最佳归一化可由下式获得：
2322212032103210q q q q Q k q j q i q q
Q k q j q i q q Q b b b b
b b +++=+++=+++= (3.5)
4.比力的坐标转换
加速度计测量的比力b f 通过矩阵T 可转换为p f ，即： ⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡b z b y b x p z p y p x f f f T T T T T T T T T f f f 333231232221131211 (3.6) 5.速度V (即p V )的即时修正
惯性导航的基本方程为：
g V f V ep ep ie ep +⨯+-=)2(ωω
以上可以写成如下矩阵形式：
⎥
⎥⎥⎦⎤⎢⎢⎢⎣⎡∙⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡+-++-+-+⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡-⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡z y x p epx p iex p epy p iey
p epx p iex p iez p epy p iey p p z p y p x z y x V V V g f f f V V V iez 0)2(220)2(2000ωωωωωωωωωω (3.7) 6.位置矩阵的即时修正 位置矩阵C 是由地球坐标系转换到平台坐标系的方向余弦矩阵，它是纬度、精度与游动方位角的函数。

由于位置矩阵的改变是由平台坐标系相对地球坐标系
运动的角速率p ep ω (又称为位置速率)可所引起的。

所以位置矩阵C=p e C 可以通过求
解下列的矩阵微分方程而获得：
⎥⎥⎥⎦
⎤⎢⎢⎢⎣⎡∙⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡--=⎥⎥⎥⎦⎤⎢⎢⎢⎣⎡3332312322211312113332312322
2113121100000C C C C C C C C C C C C C C C C C C p epx p epy p epx p epy ωωωω （3.8）
将位置矩阵C 与姿态矩阵(即捷联矩阵)T 的即时修正相比较可以看出，由于位置矩阵的变化比姿态矩阵慢得多，所以它要求的即时修正的频率也要慢得多，而且。