clear;
clc;
%%依据初始对准的欧拉角递推四元数,得到姿态欧拉角的递推值
%四元数归一化
L=28.249159657618680;
T=0.01;
e=0.0818191908426; R=6378137.0; RN=R*(1-e^2)/(1-e^2*(sind(L))^2)^1.5; RE=R/(1-e^2*(sind(L))^2)^0.5; Omega=7.2921151467e-5; g=9.7803267711905*(1+0.00193185138639*(sind(L))^2)/(1-0.00669437999013*(sind(L))^2)^0.5;
wien=[Omega*cosd(L);0;-Omega*sind(L)];
tic;
%%载入数据文件
load PUREINS_00.dat
da1=PUREINS_00;
da=[da1(:,9) da1(:,11) -da1(:,10) da1(:,12) da1(:,14) -da1(:,13)]; %将数据文件转换到前右下载体坐标系
%%对十分钟之后的数据应用双子样四元数法进行递推结算
j=0;
eul=[-0.651093042529;-0.339463783558;92.2743253296]; %给定初始姿态
vn=[0;0;0]; %给定地理系下的初始速度
L=0.493040846947*180/pi;Lamuda=1.97419090135*180/pi;h=41.69; %给定初始纬经高
att=zeros(40725,3);vel=zeros(40725,3);pos=zeros(40725,3);time=zeros(40725,1); %变化矩阵赋初值
for i=1:2:81450
j=j+1; %递推次数
%%姿态解算
cita1=[da(i,1);da(i,2);da(i,3)];
cita2=[da(i+1,1);da(i+1,2);da(i+1,3)];
cigama=cita1+cita2+2/3*(cross(cita1,cita2)); %求解双子样算法,对应圆锥补偿下的角度增量
%%四元数更新
q=eul2qua(eul); %求解前一时刻四元数
cbn1=eul2dcm(eul); %求解前一时刻载体系到导航系旋转矩阵
%载体系的四元数更新
ncigama=norm(cigama);
qb=[cos(ncigama/2);sin(ncigama/2)/ncigama*cigama];
%地理系的四元数更新
wenn=[vn(2)/RE;-vn(1)/RN;-vn(2)/RE*tand(L)]; %求解地理系相对地球系的转动角速度
winn=wenn+wien; %地理系相对惯性系的转动角速度,由于wien为常量,这里设为全局变量
yita=winn*2*T; nyita=norm(yita); %注意,这里用的时间是四元数更新周期2T!!
qn=[cos(nyita/2); -sin(nyita/2)/nyita*yita]; %地理系下的更新四元数
%姿态四元数的更新
q=qcal(qcal(qn,q),qb); %当前时刻四元数更新运算
q=q/norm(q);
cbn2=qua2dcm(q); %四元数转到当前时刻载体系到导航系旋转矩阵
%旋转矩阵转到姿态欧拉角
eul=dcm2eul(cbn2); %地理系下的更新欧拉角
%将欧拉角表示到姿态矩阵中
att(j,1)=eul(1);
att(j,2)=eul(2);
att(j,3)=eul(3);
%%速度结算
vk1=[da(i,4);da(i,5);da(i,6)];
vk2=[da(i+1,4);da(i+1,5);da(i+1,6)];
vk=vk1+vk2; %速度更新周期内的常速度增量,加在一起是否好
cigamak=cita1+cita2; %速度更新周期内角速度增量
vrot=1/2*cross(cigamak,vk); %旋转效应补偿项
vscul=2/3*(cross(cita1,vk2)+cross(vk1,cita2)); %划摇效应补偿项
%导航系变化的旋转矩阵
cn=eye(3)-chazhen(yita);
%比力积分增量
dvf=cn*cbn1*(vk+vrot+vscul);
%重力加速度和哥氏加速度增量
dvg=([0;0;g]-cross((2*wien+wenn),vn))*2*T;
%%地理系下相对地球速度更新值
vn1=vn;
vn=vn+dvf+dvg;
vn(3)=0;
%转到速度矩阵中
vel(j,1)=vn(1);
vel(j,2)=vn(2);
vel(j,3)=vn(3);
%%地理系下的经纬度计算
L=L+(((vn(1)+vn1(1))/RN)*T)*180/pi; %纬度
Lamuda=Lamuda+(vn(2)+vn1(2))/(RE*cosd(L))*T*180/pi; %经度
h=h+(vn(3)+vn1(3))*T;
%转到位置矩阵中
pos(j,1)=L;
pos(j,2)=Lamuda;
pos(j,3)=h;
%解算时间
time(j,1)=j*0.02;
end
toc;
%%绘制导航结算图线
%姿态时间曲线
roll=att(:,1);pitch=att(:,2);yaw=att(:,3);
figure(1);
plot(time,roll,'b');
hold on;
plot(time,pitch,'r');
legend('roll','pitch');
title('滚转角与俯仰角-时间曲线');
xlabel('时间(s)');
ylabel('滚转角,俯仰角(。)');
figure(2);
plot(time,yaw,'g');
legend('yaw');
title('偏航角-时间曲线');
xlabel('时间(s)');
ylabel('偏航角(。)');
vn=vel(:,1);ve=vel(:,2);vd=vel(:,3);
%速度时间曲线
figure(3);
plot(time,vn,'b');
hold on;
plot(time,ve,'r');
hold on;
plot(time,vd,'g');
legend('vn','ve','vd');
title('北东地系下速度-时间曲线');
xlabel('时间(s)');
ylabel('速度(m/s)');
rn=pos(:,1);re=pos(:,2);rd=pos(:,3);
%平面位置曲线
figure(4);
plot(re,rn,'g');
hold on;
plot(re(1),rn(1),'d');
hold on;
plot(re(40725),rn(40725),'d'); %对起点和终点进行标记
legend('行进轨迹曲线');
title('经度-纬度平面轨迹');
xlabel('经度(。)');
ylabel('纬度(。)');
matlab_采用实测数据实现纯惯导解算,程序包含数据
版权申诉
5星 · 超过95%的资源 63 浏览量
2022-05-06
23:02:14
上传
评论 2
收藏 6.89MB ZIP 举报
wouderw
- 粉丝: 272
- 资源: 2960
- 1
- 2
前往页