n
一次积分：
V V n dt V n (0)
n o
t
第二次积分：


t o t o
VN dt (0) (0) 分别表 V n (0) 、 (0) 、 R 示速度矢量、纬度及经度 VE sec dt (0) 的初始值。 R
船用惯性导航系统
Pk / k 1 ( k ,k 1 J k 1H k 1 ) Pk 1 ( k ,k 1 J k 1H k 1 )
k 1Qk 1k 1 J k 1Rk 1 J k 1
1 J k 1 k 1Sk 1Rk 1
GPS/INS 组合系统
Vehicle acceleration
Accel
Coordinate Transform
Velocity Integration
Position
Accelerometer Velocity Quaternion Calculation Accelerate Integration
N avigation Com puter
组合系统的状态方程可以描述为:
x(t ) F (t ) x(t ) w(t )
FINS (t ) 0 其中 F (t ) 0 F ( t ) GPS 1111
GPS/INS 组合系统

测量方程 GPS伪距表达式为:
[( X X ) (Y Y ) (Z Z ) ] dtu Vi
船用惯性导航系统
加速度计测量的是比力 ，即载体惯性力与地球引力之差：
f n Cin [ri Gi ] Cinri Gn
Cin 表示从惯性坐标系i到地理坐标系n的方向余弦矩阵，以下类 同；r为载体的地心位臵矢量；G为地球引力。
地理坐标系内的速度及加速度 ：
n e i Vn Ce r Cin (ri Ωi r ) ie i i i i Vn Cin [ri (Ωien 2Ωie )Cin Vn Ωie Ωie r] n n f n Vn (Ωen 2Ωie )Vn gn
船用惯性导航系统
3.
位臵误差方程 : 采用摄动法，直接得到位臵误差方程：
R VN VE cos sec tg R R
VN
卡尔曼滤波器
卡尔曼滤波算法是一种线性最小方差估计的递推算法，相对 其它几种最优估计方法，卡尔曼滤波具有如下特点：
i i G i 2 i G i 2 i G 1 i 2 2
采用变分法,导出伪距误差方程： i ei1 X ei 2 Y ei3 Z tu Vi
其中 tu 为GPS接收机时钟偏差等效的距离误差 ， Vi 为测量白噪声
i Xi i X G ei1 i X ri i YGi Y i ei 2 i Y ri i Zi i ZG ei 3 i Z ri

算法是递推的 采用动力学方程描述被估计量的动态变化规律 卡尔曼滤波具有连续型和离散型两类算法
卡尔曼滤波器
离散系统的卡尔曼滤器 动态方程
xk k ,k 1xk 1 k 1k 1
量测方程
状态预测估计 方差预测 状态估计 方差迭代 滤波增益
zk Hk xk k
ˆk / k 1 k ,k 1x ˆk 1 x
惯导系统提高GPS接收机的跟踪能力
解决周跳问题 组合系统将降低对惯导系统的要求
船用惯性导航系统
船用平台式惯性导航系统 一般采用水平指北方案。由陀螺仪（三个单自由或二个双 自由）构成稳定平台，跟踪并稳定在当地地理坐标系内。在惯性 平台上安装两个敏感轴互相垂直、分别沿东西向和南北向放臵的 加速度计，用以测量这两个方向的加速度信息。

GPS/INS 组合系统
采用GPS伪距误差为观测量，因此需对GPS接收机的时钟 偏差进行估计。时钟偏差用一阶马尔可夫过程描述：
tu u tu u
于是，选择状态变量为:
u 为白噪声
x [ VE VN x y z tu ]T [xINS xGPS ]T
在船用惯性导航系统的使用中，通过对常值陀螺漂移的补偿， 消除了常值陀螺漂移造成的系统位臵、速度和姿态误差，但随机 漂移的影响依然存在。惯导和GPS组合滤波，在对系统输出作最 优估计的同时，还可对随机的漂移进行估计和补偿，从而提高惯 导系统本身的精度。
GPS/INS 组合系统
惯导系统的误差方程
状态方程
卫星导航
Global Navigation Satellite System
第八章 船用惯性导航系统 与GPS的组合
本章内容安排
概述 船用惯性导航系统 卡尔曼滤波器 GPS/INS组合导航 双差分GPS/INS组合导航系统 推算船位与GPS的组合
概述
惯性导航系统（INS）： 优点： 完全自主的导航 可提供多种导航信息 缺点： 精度逐渐降低。 初始对准时间长
G yroscope
Vehicle angular rate
Rate
Rate Integration
Euler Angle Calculation
Attitude
船用惯性导航系统
船用惯性导航系统误差方程的推导 1. 平台姿态误差方程
n n in n n n in
k 1
P k / k 1 k ,k 1P k 1k ,k 1 k 1Qk 1k 1
ˆk x ˆk / k 1 Kk ( zk Hk x ˆk / k 1 ) x
Pk Pk / k 1 Pk / k 1H k [ H k Pk / k 1H k Rk ]1 H k Pk / k 1
ˆk / k 1 k ,k 1x ˆk 1 Bk 1uk 1 J k 1 ( zk 1 yk 1 Hk 1x ˆk 1 ) x
1 Kk P k / k 1Hk ( Hk P k / k 1Hk Rk )
Pk ( I Kk H k ) Pk / k 1
卫星导航系统（GPS）： 优点： 定位精度高 精度不随时间变化 缺点： 卫星信号不易捕获和跟踪 抗干扰能力差易产生周跳
GPS系统与惯导系统具有互补的特点
因而被认为是目前导航领域和大地测量领域 最理想的组合方式。
概述
组合导航优点
GPS/INS组合对改善系统精度有利 GPS/INS组合加强系统的抗干扰能力
n ie {0， ie sin ，iecos}
n en {
VN VE tg
R ，
V ， VE E sec 2 } R R R
f n 比力误差在地理坐标系的投影，主要是由加速度计零位误差▽和平
台（或捷联式惯导的“数学平台”）偏离地理坐标系所致
船用惯性导航系统
2.
速度误差方程 采用摄动法，得到速度误差方程：
n n n n V n (en 2ie )V n (en 2ie )V n g n f n
重力无异常时， gn 0
n n en 和ie
n n en 和ie 的反对称矩阵
r [( X X ) (Y Y ) ( Z Z ) ]
i i G i 2 i G i 2 i G
1 i 2 2
GPS/INS 组合系统
i 表示成经纬度坐标系中的位臵误差 把伪距误差 、 的函数有 下式成立： C11 C12 e11 e12 e13 R sin cos R sin cos C C21 C22 e12 e22 e23 R sin sin R cos cos C e e R cos 0 C e 31 32 13 32 33
n in {
Vin , (
VE 1 sec 2 Ωie cos ) tg VE } R R
陀螺漂移 在地理坐标系的投影： n 平台惯导：
n
n
n 捷联惯导： n Cb
船体坐标系到地理坐标系的方向余弦阵：Cb
动态方程 量测方程 滤波计算 预测计算 滤波增益 滤波误差协方差 预测误差协方差
xk k ,k 1xk 1 Bk 1uk 1 k 1k 1
zk H k xk yk k ˆk x ˆk / k 1 Kk ( zk yk Hk x ˆk / k 1 ) x
T T P k ( I Kk Hk ) P k / k 1 ( I Kk Hk ) Kk Rk Kk
1 T 1 Pk1 Pk / k 1 H k Rk H k
③ ④
ˆk 是无偏估计 卡尔曼滤波估计 x K k 与Qk 1 成正比，而与 Pk 成反比
卡尔曼滤波器
白噪声情形下一般线性系统滤波
1 VN R VE 1 tg sec sec VE R R 1 1 2 VE 2ie cos VN VEVN sec VN tg VE R R 1 (2ie sin VN VE tg VN ) Ay g R 2 V V VN 2ie cos VE E sec 2 g Ax (2ie sin 2 E tg ) VE R R VN VE VE ie cos ie sin tg x R R R VN 1 1 ie sin VE ie sin VE tg y R R R VN VE VE 1 2 ie cos sec tg VE ie cos z R R R R