惯性导航作业
一、数据说明：
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、初始姿态矩阵的确定：
根据初始姿态角求四元数：
0123cos cos cos sin sin sin 222222
cos
sin cos sin cos sin 222222cos cos sin sin sin cos 222222
cos
sin sin sin cos cos 222222
ab
ab ab
ab ab ab ab
ab q q q q ψψθγθγ
ψψθγθγ
ψψθγθγ
ψψθγθγ
=-=-=+=+
再根据四元数求方向余弦矩阵的初始矩阵：
()
()()
()()()
2222
0123120313022222
12030123
23012222130223010123
222222b 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 ωωω⎡⎤-⎢⎥⎢⎥
⎢⎥⎢⎥=+⎢⎥
⎢⎥⎢⎥
+⎢⎥⎣⎦
加速度计获得的比力信息b
ib f 为载体坐标系中各个轴向的比力，而我们需要的比力t it f 为地理坐标系中各个轴向的比力，它们之间应用矩阵t
b C 做变换：
()()
1t t b
it b ib
T t b b b
t
t
f C f C C
C -=⨯==
根据比力信息可以求出各个方向上的加速度：
()()()()(2sin tan())(2cos )(2sin tan())(2cos )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 ωωωω∙
∙
∙
=++-+=-++=+++-
因此可以求得速度为：
t t t t x
x x t t t t y y y V V dt V V V dt V ∙
∙
=+=+⎰⎰
载体所在位置的地理纬度L 、经度λ可由下列方程求得：
000
)sec(λλ+=+=⎰
⎰
dt L R V L dt R V L t
xt
xt
t
yt
yt
3、四元数姿态矩阵的更新：
b b b t tb ib t it C ωωω=-
式中，b
ib ω为陀螺所测得的角速度。
用毕卡逼近法更新b t C 的值，T 为采样时间
b
ib
T θω∆=⨯ []0
000x y
z x
z y y z x z
y
x θθθθ
θθθθθθθθθ-∆-∆∆⎡⎤
⎢⎥∆∆-∆⎢
⎥∆=⎢⎥
∆-∆∆⎢⎥
∆∆-∆⎢⎥⎣⎦
22220x y z θθθθ∆=∆+∆+∆
()()()()[]()242
0001118384248q n I q n θθθθ⎧⎫⎛⎫⎛⎫∆∆∆⎪⎪
+=-++-∆ ⎪ ⎪⎨⎬ ⎪ ⎪⎪⎪⎝⎭⎝⎭⎩⎭
4、姿态角的求解：
姿态角与姿态矩阵的关系：
