导航与制导实验报告INS与GPS位置组合导航.docx
- 文档编号:29759103
- 上传时间:2023-07-26
- 格式:DOCX
- 页数:17
- 大小:243.25KB
导航与制导实验报告INS与GPS位置组合导航.docx
《导航与制导实验报告INS与GPS位置组合导航.docx》由会员分享,可在线阅读,更多相关《导航与制导实验报告INS与GPS位置组合导航.docx(17页珍藏版)》请在冰豆网上搜索。
导航与制导实验报告INS与GPS位置组合导航
导航与制导原理实验报告
一.实验要求
1.完成INS与GPS位置组合导航的仿真;
2.画出组合导航后的位置误差、速度误差曲线;
3.画出原始轨迹与组合导航后的轨迹比较图;(画图时弧度制单位要转换成度分秒制单位)
4.结果分析
5.提交纸版实验报告(附上代码)
二.全局变量
R=6378160;%地球半径(长半轴)
f=1/298.3;%地球扁率
wie=7.2921151467e-5;%地球自转角速率
g0=9.7803267714;%重力加速度基础值
deg=π/180;%角度
min=deg/60;%角分
sec=min/60;%角秒
hur=3600;%小时
dph=deg/hur;%度/时
ts=0.1;%仿真采样时间
三.组合导航仿真变量
GPS_Sample_Rate=10;%GPS采样时间
Runs=10;%由于随机误差,使用Kalman滤波时,应多次滤波,以求平均值
Tg=3600;%陀螺仪Markov过程相关时间
Ta=1800;%加速度计Markov过程相关时间
四.KalmanFilter:
估计状态初始值:
Xk=zeros(18,1);
估计协方差初始值:
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);%18*18矩阵
系统噪声方差:
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
系数矩阵F,G,H的表示,参考课件6.2.1。
五.可能用到的公式
(1)四元数Q的即时修正(符号⨂表示四元数乘法)
式中
为向量扩展四元数,标量部分为0。
(2)四元数Q的归一化
(3)姿态矩阵
的计算
(4)提取姿态角
(5)比力的坐标变换
(6)速度计算
(7)地球速率及地理坐标系相对地球坐标系的转动角速率
机体系相对于地理系的转动角速率在机体系中的投影
(8)位置计算
(9)主曲率半径
(10)重力加速度
(11)连续系统离散化公式(简化形式)
其中,I是单位矩阵,T是仿真采样时间。
六.数据文件说明
dataWbibN.txt%叠加噪声的陀螺仪角速度输出
dataFbibN.txt%叠加噪声的加速度计比力输出
dataPos.txt%原始轨迹的位置数据(依次是纬度L,经度
,高度h)dataVn.txt%原始轨迹的速度数据(依次是东速度、北速度、天速度)att0=[0;0;0.3491]%姿态解算矩阵初始值(依次是俯仰角
,横滚角
,航向角ψ)
dataGPSposN.txt%叠加噪声的GPS位置数据(即等间隔采样原始轨迹的位置数据,采样间隔是10,即第10、20,……的数据,并叠加噪声)
七.仿真流程图
整个仿真过程流程图
在整体仿真流程图中,Kalman滤波可以由子函数kalman_GPS_INS_correct.m进行。
捷联解算可按照如下捷联解算流程图进行。
八.实验思路
根据上述两个流程图,要实现INS与GPS位置组合导航,需要在已给的INS导航代码中,加入当k为10的倍数时进行Kalman滤波,修正位置、速度数据环节,则添加代码如下(添加位置见完整程序):
%添加程序
[F,G,H]=GetConSis(vtC(:
k),posC(:
k),q_1,fn(:
k),Tg,Ta);%利用GetConSis得到F,G,H矩阵
ifrem(k,10)~=0%判断k是否为10的倍数,若是则进行kalman滤波,修正。
[E_att,E_pos,E_vn,Xk,Pk]=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk);%利用kalman_GPS_INS_correct进行kalman滤波
else
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,Rk,Zk);
end
attC(:
k)=attC(:
k)-E_att;
posC(:
k)=posC(:
k)-E_pos;
vtC(:
k)=vtC(:
k)-E_vn;%修正位置,速度数据
九.完整代码及注释
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(:
1);%速度向量初始值
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
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);%比力的坐标变换
%添加程序
[F,G,H]=GetConSis(vtC(:
k),posC(:
k),q_1,fn(:
k),Tg,Ta);%利用GetConSis得到F,G,H矩阵
ifrem(k,10)~=0%判断k是否为10的倍数,若是则进行kalman滤波,修正。
[E_att,E_pos,E_vn,Xk,Pk]=kalman_GPS_INS_correct(Xk,Qk,Pk,F,G,H,ts,Rk);%利用kalman_GPS_INS_correct进行kalman滤波
else
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,Rk,Zk);
end
attC(:
k)=attC(:
k)-E_att;
posC(:
k)=posC(:
k)-E_pos;
vtC(:
k)=vtC(:
k)-E_vn;%修正位置,速度数据
%捷联惯导解算
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);%四元数归一化
%姿态角更新
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];
attC(:
k+1)=[asin(T(3,2));atan(-T(3,1)/T(3,3));atan(T(1,2)/T(2,2))];
%横滚角gamma修正
ifT(3,3)<0
ifattC(2,k+1)<0
attC(2,k+1)=attC(2,k+1)+pi;
else
attC(2,k+1)=attC(2,k+1)-pi;
end
end
%航向角psi修正
ifT(2,2)<0
ifT(1,2)>0
attC(3,k+1)=attC(3,k+1)+pi;
else
attC(3,k+1)=attC(3,k+1)-pi;
end
end
ifabs(T(2,2))<1.0e-20
ifT(1,2)>0
attC(3,k+1)=pi/2.0;
else
attC(3,k+1)=-pi/2.0;
end
end
%速度更新
vtC(:
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)));%经度
posC(3,k+1)=posC(3,k)+ts*vtC(3,k);%高度
end
%%
attSta=attSta+attC;
vtSta=vtSta+vtC;
posSta=posSta+posC;
end
%对统计矩阵取平均
attC=1./StaNum*attSta;
posC=1./StaNum*posSta;
vtC=1./StaNum*vtSta;
%解算值与仿真值的误差,作图
%结算矩阵均为3x(N+1),需做处理
%N点数据,相邻两点采样间隔为0.1s,做作图横轴修正
fori=1:
N
time(i)=i*ts;
Rm=Re*(1-2*f+3*f*(sin(attC(1,i)))^2);
Rn=Re*(1+f*(sin(attC(1,i)))^2);
Latitude_error(i)=(posC(1,i)-posS(1,i))*Rm;
Longitude_error(i)=(posC(2,i)-posS(2,i))*Rn*cos(attC(1,i));
end
posCp=posC(:
1:
N);
figure;
subplot(131);plot(time,Latitude_error);title('纬度误差');xlabel('Time/s');ylabel('\it\deltaL/m');gridon;
subplot(132);plot(time,Longitude_error);title('经度误差');xlabel('Time/s');ylabel('\it\delta\phi/m');gridon;
subplot(133);plot(time,posCp(3,:
)-posS(3,:
));title('高度误差');xlabel('Time/s');ylabel('\it\deltah/m');gridon;
vtCp=vtC(:
1:
N);
figure;
subplot(131);plot(time,vtCp(1,:
)-vtetS(1,:
));title('东速度误差');xlabel('Time/s');ylabel('\it\deltavelocityeast/(m/s)');gridon;
subplot(132);plot(time,vtCp(2,:
)-vtetS(2,:
));title('北速度误差');xlabel('Time/s');ylabel('\it\deltavelocitynorth/(m/s)');gridon;
subplot(133);plot(time,vtCp(3,:
)-vtetS(3,:
));title('天速度误差');xlabel('Time/s');ylabel('\it\deltavelocityup/(m/s)');gridon;
%三维飞行轨迹图
figure;
plot3(posS(2,:
)/pi*180,posS(1,:
)/pi*180,posS(3,:
),'k');
holdon;
plot3(posCp(2,:
)/pi*180,posCp(1,:
)/pi*180,posCp(3,:
),'r');gridon;
ylabel('纬度L/arcdeg');xlabel('经度\phi/arcdeg');zlabel('高度h/m');title('黑线-仿真器飞行轨迹,红线-INS解算飞行轨迹');
十.实验结果及分析
以上所有结果与所给参考结果完全一致,仿真正确。
原始INS代码飞行轨迹仿真结果如图:
从以上两种方法的飞行轨迹仿真图可以看出,加入kalman滤波的INS/GPS组合导航的导航精度明显高于INS导航,飞行轨迹肉眼看来完全贴合原始轨迹,即组合导航结果基本可以满足一般及稍高精度要求的导航需求。
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 导航 制导 实验 报告 INS GPS 位置 组合