惯性导航作业
一、数据说明：
1：惯导系统为指北方位的捷连系统。初始经度为 116.344695283 度、纬度为
39.975172 度,高度 h 为 30 米。初速度
v0=[-9.993908270;0.000000000;0.348994967]。
2：jlfw 中为 600 秒的数据，陀螺仪和加速度计采样周期分别为为 1/100 秒和
1/100 秒。
3：初始姿态角为[2 1 90]（俯仰，横滚，航向,单位为度）,jlfw.mat 中保存的为比
力信息 f_INSc(单位 m/s^2)、陀螺仪角速率信息 wib_INSc(单位 rad/s)，排列顺序
为 一~三行分别为 X、Y、Z 向信息.
4: 航向角以逆时针为正。
5:地球椭球长半径 re=6378245;地球自转角速度 wie=7.292115147e-5;重力加速度
g=g0*(1+gk1*c33^2)*(1-2*h/re)/sqrt(1-gk2*c33^2)；
g0=9.7803267714;gk1=0.00193185138639;gk2=0.00669437999013;c33=sin(lat 纬
度);
二、作业要求：
1：可使用 MATLAB 语言编程，用 MATLAB 编程时可使用如下形式的语句读
取数据：load D:\...文件路径...\jlfw,便可得到比力信息和陀螺仪角速率信息。用角
增量法。
2：(1) 以系统经度为横轴，纬度为纵轴（单位均要转换为：度）做出系统位置
曲线图；
(2) 做出系统东向速度和北向速度随时间变化曲线图（速度单位：m/s，时间
单位：s）；
(3) 分别做出系统姿态角随时间变化曲线图（俯仰，横滚，航向,单位转换为：
度，时间单位：s）；
以上结果均要附在作业报告中。
3：在作业报告中要写出“程序流程图、现阶段学习小结”，写明联系方式。
(注意程序流程图不是课本上的惯导解算流程，而是你程序分为哪几个模块、
是怎样一步步执行的,什么位置循环等,让别人根据该流程图能够编出相应程序)
(学习小结按条写，不用写套话)
4：作业以纸质报告形式提交，附源程序。
三、基本原理和公式
1、初始姿态矩阵的确定：
根据初始姿态角求四元数：
0
1
2
3
cos cos cos sin sin sin
2 2 2 2 2 2
cos sin cos sin cos sin
2 2 2 2 2 2
cos cos sin sin sin cos
2 2 2 2 2 2
cos sin sin sin cos cos
2 2 2 2 2 2
ab ab
ab ab
ab ab
ab ab
q
q
q
q
y y
q g q g
y y
q g q g
y y
q g q g
y y
q g q g
= -
= -
= +
= +
再根据四元数求方向余弦矩阵的初始矩阵：
( ) ( )
( ) ( )
( ) ( )
2 2 2 2
0 1 2 3 1 2 0 3 1 3 0 2
2 2 2 2
1 2 0 3 0 1 2 3 2 3 0 1
2 2 2 2
1 3 0 2 2 3 0 1 0 1 2 3
2 2
2 2
2 2
b
t
q q q q q q q q q q q q
C q q q q q q q q q q q q
q q q q q q q q q q q q
é ù
+ - - + -
ê ú
= - - + - +
ê ú
ê ú
+ - - - +
ë û
2、指北方位系统的运动解算：
“平台”指令角速度为：
( )
( )
cos
sin tan( )
t
y
yt
t
t
x
it ie
xt
t
x
ie
xt
V
R
V
L
R
V
L L
R
w w
w
é ù
-
ê ú
ê ú
ê ú
ê ú
= +
ê ú
ê ú
ê ú
+
ê ú
ë û
加速度计获得的比力信息
b
ib
f
为载体坐标系中各个轴向的比力，而我们需
要的比力
t
it
f
为地理坐标系中各个轴向的比力，它们之间应用矩阵
t
b
C
做变换：
( ) ( )
1
t t b
it b ib
T
t b b
b t t
f C f
C C C
-
= ´
= =
根据比力信息可以求出各个方向上的加速度：
( ) ( )
( )
( )
(2 sin tan( )) (2 cos )
(2 sin tan( ))
(2 cos )
t t
t t t t
x x
x ibx ie y ie z
xt xt
t
t
y
t t t t
x
y iby ie x z
xt yt
t
t
y
t t t t
x
z ibz ie x y
xt yt
V V
V f L L V L V
R R
V
V
V f L L V V
R R
V
V
V f L V V g
R R
w w
w
w
·
·
·
= + + - +
= - + +
= + + + -
因此可以求得速度为：
0
0
0
0
t
t t t
x x x
t
t t t
y y y
V V dt V
V V dt V
·
·
= +
= +
ò
ò
载体所在位置的地理纬度 L、经度
�
可由下列方程求得：
0
0
0
0
)sec(
��
��
��
�
�
dtL
R
V
Ldt
R
V
L
t
xt
xt
t
yt
yt
3、四元数姿态矩阵的更新：
b b b t
tb ib t it
C
w w w
= -
式中，
b
ib
w
为陀螺所测得的角速度。
用毕卡逼近法更新
b
t
C
的值，T 为采样时间
b
ib
T
q w
D = ´
[ ]
0
0
0
0
x y z
x z y
y z x
z y x
q q q
q q q
q
q q q
q q q
-D -D D
é ù
ê ú
D D -D
ê ú
D =
ê ú
D -D D
ê ú
D D -D
ê ú
ë û
2 2 2 2
0 x y z
q q q q
D = D + D + D
( )
( ) ( ) ( )
[ ]
( )
2 4 2
0 0 0
1
1 1
8 384 2 48
q n I q n
q q q
q
ì ü
æ ö æ ö
D D D
ï ï
+ = - + + - D
ç ÷ ç ÷
í ý
ç ÷ ç ÷
ï ï
è ø è ø
î þ
4、姿态角的求解：
姿态角与姿态矩阵的关系：
