GPS与惯性捷联导航仿真实验报告西安交通大学Word格式.docx
《GPS与惯性捷联导航仿真实验报告西安交通大学Word格式.docx》由会员分享，可在线阅读，更多相关《GPS与惯性捷联导航仿真实验报告西安交通大学Word格式.docx（14页珍藏版）》请在冰豆网上搜索。
陀螺漂移：
，
（2）速度误差
（3）位置误差
（4）陀螺漂移
（5）加速度计误差
该式为一阶Markov过程噪声，
为相关时间
（6）综合方程
2．解算算法框图如图3-1所示
N
ba
Y
图3-1组合导航捷联解算算法流程图
四、实验要求:
1.1、完成INS与GPS位置组合导航的仿真;
2.2、画出组合导航后的位置误差、速度误差曲线;
3.3、画出原始轨迹与组合导航后的轨迹比较图；
4.（画图时，弧度制单位要转换成度分秒制单位）
5.4、结果分析
6.5、提交纸版实验报告（附上代码）
五、实验步骤
7.将已有程序导入到MATLAB中
8.将四元素法改为方向余弦法
9.修改求解F、Q、H矩阵函数
10.在INS导航解算主程序中加入卡尔曼滤波修正误差
11.得到速度误差、位置误差以及姿态误差图
12.进行结果分析
六、实验结果
1.组合导航位置误差曲线如图6-1所示
图6-1
2、组合导航速度误差曲线如图6-2所示
图6-2
3、原始轨迹与组合导航后的轨迹比较曲线如图6-3所示
图6-3
七、结果分析
1.平均误差：
（1）组合导航
经度误差均值为0；
维度误差均值为0；
高度误差均值为-0.6042。
东速度误差均值为-0.4927；
北速度误差均值为-0.6110；
天速度误差均值为-0.3059。
（2）INS导航
高度误差均值为-199.2368。
东速度误差均值为-1.1347；
北速度误差均值为-0.3161；
天速度误差均值为-1.631。
2.误差标准差：
经度误差标准差为0；
维度误差标准差为0；
高度误差标准差为22。
东速度误差标准差为1.8936；
北速度误差标准差为2.9869；
天速度误差标准差为1.0253
高度误差标准差为269.5201。
东速度误差标准差为0.9635；
北速度误差标准差为0.1972；
天速度误差标准差为1.3852
3.曲线效果比较：
INS导航轨迹与原始轨迹曲线如图7-1所示
图7-1INS导航轨迹与原始轨迹曲线
4.通过对单纯INS导航和INS-GPS组合导航结果的分析可知，组合导航在准确度和精度上都高于单纯INS导航。
在INS导航时间较长时会出现如图6-1所示的严重偏差，而INS-GPS组合导航的结果如图5-3所示能在时间较长时保持准确。
（代码见附录）
附录：
源代码
clc;
clear;
closeall;
%捷联惯导更新仿真，四元数法，一阶近似算法，不考虑圆锥补偿和划桨补偿
ts=0.1;
%采样时间
Re=6378160;
%地球长半轴
wie=7.2921151467e-5;
%地球自转角速率
f=1/298.3;
%地球扁率
g0=9.7803;
%重力加速度基础值
deg=pi/180;
%角度
min=deg/60;
%角分
sec=min/60;
%角秒
hur=3600;
%小时
dph=deg/hur;
%度/小时
%读取数据
wbibS=dlmread（'
dataWbibN.txt'
）;
fbS=dlmread（'
dataFbibN.txt'
posS=dlmread（'
dataPos.txt'
vtetS=dlmread（'
dataVn.txt'
p_gps=dlmread（'
dataGPSposN.txt'
%统计矩阵初始化
[mm,nn]=size（posS）;
posSta=zeros（mm,nn）;
vtSta=posSta;
attSta=posSta;
posC（:
1）=posS（:
1）;
%位置向量初始值
vtC（:
1）=vtetS（:
%速度向量初始值
attC（:
1）=[0;
0;
0.3491];
%姿态解算矩阵初始值
Qk=1e-6*diag（[0.01,0.01,0.01,0.01,0.01,0.01,0.9780,0.9780,0.9780]）.^2;
%系统噪声方差矩阵
Rk=diag（[1e-5,1e-5,10.3986]）.^2;
%测量噪声方差
Tg=3600*ones（3,1）;
%陀螺仪Markov过程相关时间
Ta=1800*ones（3,1）;
%加速度计Markov过程相关时间
GPS_Sample_Rate=10;
%GPS采样率太高效果也不好
StaNum=10;
%重复运行次数，用于求取统计平均值
forOutLoop=1:
StaNum
Pk=diag（[min,min,min,0.5,0.5,0.5,30./Re,30./Re,30,0.1*dph,0.1*dph,0.1*dph,0.1*dph,0.1*dph,0.1*dph,1.e-3,1.e-3,1.e-3].^2）;
%初始估计协方差矩阵
Xk=zeros（18,1）;
%初始状态
%%
N=size（posS,2）;
%j=1;
fork=1:
N-1
if（mod（k,10）==0）%if这一块是我加的
Zk=posC（:
k）-p_gps（:
k/10）;
[E_att,E_pos,E_vn,Xk,Pk]=kalman_GPS_INS_correct（Xk,Qk,Pk,F,G,H,ts,Zk,Rk）;
attC（:
k）=attC（:
k）-E_att;
posC（:
k）=posC（:
k）-E_pos;
vtC（:
k）=vtC（:
k）-E_vn;
end
si=sin（attC（1,k））;
ci=cos（attC（1,k））;
sj=sin（attC（2,k））;
cj=cos（attC（2,k））;
sk=sin（attC（3,k））;
ck=cos（attC（3,k））;
%k时刻姿态矩阵
M=[cj*ck+si*sj*sk,ci*sk,sj*ck-si*cj*sk;
-cj*sk+si*sj*ck,ci*ck,-sj*sk-si*cj*ck;
-ci*sj,si,ci*cj];
%即Cnb矩阵
q_1=[sqrt（abs（1.0+M（1,1）+M（2,2）+M（3,3）））/2.0;
sign（M（3,2）-M（2,3））*sqrt（abs（1.0+M（1,1）-M（2,2）-M（3,3）））/2.0;
sign（M（1,3）-M（3,1））*sqrt（abs（1.0-M（1,1）+M（2,2）-M（3,3）））/2.0;
sign（M（2,1）-M（1,2））*sqrt（abs（1.0-M（1,1）-M（2,2）+M（3,3）））/2.0];
fn（:
k）=M*fbS（:
k）;
%比力的坐标变换
%捷联惯导解算
wnie=wie*[0;
cos（posC（1,k））;
sin（posC（1,k））];
%地球系相对惯性系的转动角速度在导航系（地理系）的投影
%计算主曲率半径
Rm=Re*（1-2*f+3*f*sin（posC（1,k））^2）+posC（3,k）;
Rn=Re*（1+f*sin（posC（1,k））^2）+posC（3,k）;
wnen=[-vtC（2,k）/（Rm+posC（3,k））;
vtC（1,k）/（Rn+posC（3,k））;
vtC（1,k）*tan（posC（1,k））/（Rn+posC（3,k））];
%导航系相对相对地球系的转动角速度在导航系的投影
g=g0+0.051799*sin（posC（1,k））^2-0.94114e-6*posC（3,k）;
%重力加速度
gn=[0;
0;
-g];
%导航坐标系的重力加速度
wbnbC（:
k）=wbibS（:
k）-M\（wnie+wnen）;
%姿态角转动角速率计算
q=1.0/2*qmul（q_1,[0;
wbnbC（:
k）]）*ts+q_1;
%姿态四元数更新
q=q/sqrt（q'
*q）;
%四元数归一化
%方向余弦法
WbnbC=[0,-wbnbC（3,k）,wbnbC（2,k）;
wbnbC（3,k）,0,-wbnbC（1,k）;
-wbnbC（2,k）,wbnbC（1,k）,0];
a=WbnbC*ts;
T=M+M*exp（a）;
%T=M+M*WbnbC*ts;
%姿态角更新
%q11=q
（1）*q
（1）;
q12=q
（1）*q
（2）;
q13=q
（1）*q（3）;
q14=q
（1）*q（4）;
%q22=q
（2）*q
（2）;
q23=q
（2）*q（3）;
q24=q
（2）*q（4）;
%q33=q（3）*q（3）;
q34=q（3）*q（4）;
%q44=q（4）*q（4）;
%T=[q11+q22-q33-q44,2*（q23-q14）,2*（q24+q13）;
%2*（q23+q14）,q11-q22+q33-q44,2*（q34-q12）;
%2*（q24-q13）,2*（q34+q12）,q11-q22-q33+q44];
k+1）=[asin（T（3,2））;
atan（-T（3,1）/T（3,3））;
atan（T（1,2）/T（2,2））];
%横滚角gamma修正
ifT（3,3）<
ifattC（2,k+1）<
attC（2,k+1）=attC（2,k+1）+pi;
else
attC（2,k+1）=attC（2,k+1）-pi;
%航向角psi修正
ifT（2,2）<
ifT（1,2）>
attC（3,k+1）=attC（3,k+1）+pi;
attC（3,k+1）=attC（3,k+1）-pi;
ifabs（T（2,2））<
1.0e-20
attC（3,k+1）=pi/2.0;
attC（3,k+1）=-pi/2.0;
%速度更新
k+1）=vtC（:
k）+ts*（fn（:
k）+gn-cross（（2*wnie+wnen）,vtC（:
k）））;
%位置更新
posC（1,k+1）=posC（1,k）+ts*vtC（2,k）/（Rm+posC（3,k））;
%纬度
posC（2,k+1）=posC（2,k）+ts*vtC（1,k）/（（Rn+posC（3,k））*cos（posC（1,k）））;
%
升级会员 