clc
clear
% 1、导入数据
load('imu.mat');
load('pos.mat');
% 2、常数定义
% 地球自转角速度
wie = 7.292115e-5;
% 3、初始化
Lati = 0.6598808117719910; % 纬度
Longi = 2.108563779145004; % 经度
Alti =0; % 高度恒定设为0
roll = -0.433487583374893*pi/180; % 滚动角 / 度
pitch = -0.507505809910771*pi/180; % 俯仰角 / 度
yaw = 21.397108162784374*pi/180; % 偏航角 / 度
% 初始速度恒定设置为零
Vel = [0 0 0]';
% 欧拉角2方向余弦阵
eulr=[roll pitch yaw ];
Cbn= eulr2dcm( eulr );
qua= dcm2qua( Cbn );
ST= 0.01; % 采样周期(s)
% 结果存放数组,分别是时间(1)、位置(3)、速度(3)、姿态(3)共10列
navi = zeros( floor(length(imu)/2), 10);
% 4、双子样导航计算
for i=1:2:length(imu)-1
% 数据准备
% 第一个子样的比力
fb1=[imu(i,2) imu(i,3) imu(i,4)]';
% 第二个子样的比力
fb2=[imu(i+1,2) imu(i+1,3) imu(i+1,4)]';
% 第一个子样的角速度
wibb1=[imu(i,5) imu(i,6) imu(i,7)]';
% 第二个子样的角速度
wibb2=[imu(i+1,5) imu(i+1,6) imu(i+1,7)]';
% 第二个子样的时间
t_imu=imu(i+1,1);
% 转换成增量信息
jiaodua1=wibb1.*ST;
jiaodua2=wibb2.*ST;
dvel_a1=fb1.*ST;
dvel_a2=fb2.*ST;
% 双子样求和
jiaodu=jiaodua1+jiaodua2;
dvel=dvel_a1+dvel_a2;
% 4.1、速度更新
% 4.1.1、比力积分增量计算
% 计算b系下的比力积分增量
bilijifen_b=dvel+(1/2)*cross(jiaodu,dvel)+(2/3)*(cross(jiaodua1,dvel_a2)+cross(dvel_a1,jiaodua2));
% b系下的比力积分增量转换到n系中
% 使用上一时刻的姿态矩阵Cbn
bilijifen_n=Cbn*bilijifen_b;
% 4.1.2、计算重力g
g = [0 0 gravity(Lati,Alti)]';
% 4.1.3、计算哥氏加速度
% 计算wien
wien=[wie*cos(Lati) 0 -wie*sin(Lati)]';
% 计算wenn
[Rn, Re]=CalRnRe(Lati);
wenn=[Vel(2)/(Re+Alti) -Vel(1)/(Rn+Alti) -Vel(2)*tan(Lati)/(Re+Alti)]';
% 计算哥氏加速度
ag=cross((2*wien+wenn),Vel);
% 4.1.4、计算新时刻的速度
Vel=Vel+bilijifen_n+0.02*(g-ag);
% 4.2、位置更新
Lati=Lati+Vel(1)*ST*2/(Rn+Alti);
Longi=Longi+Vel(2)*ST*2/((Re+Alti)*cos(Lati));
Alti=0;%Alti+Vel(3)*ST;
% 4.3、 姿态更新
% 4.3.1、利用新的位置信息计算 wien
% 利用新的速度和位置信息计算 wenn
% 更新 wien
wien=[wie*cos(Lati) 0 -wie*sin(Lati)]';
% 更新 wenn
[Rn, Re]=CalRnRe(Lati);
wenn=[Vel(2)/(Re+Alti) -Vel(1)/(Rn+Alti) -Vel(2)*tan(Lati)/(Re+Alti)]';
% 4.3.2、更新 winb
% 使用上一时刻的姿态矩阵Cbn
winb=Cbn'*(wien+wenn);
wnbb1=wibb1-winb;
wnbb2=wibb2-winb;
% 4.3.3、计算 wnbb*ST
t_heta1=wnbb1*ST;
t_heta2=wnbb2*ST;
t_heta=t_heta1+t_heta2;
% 4.3.4、等效旋转矢量计算,双子样算法
sigema=t_heta+cross(t_heta1,t_heta2)*2/3;
% 4.3.5、构造姿态更新的四元数rk
% 计算等效旋转矢量的大小
mosigema=norm(sigema);
% 构造姿态更新的四元数rk
ac=cos(mosigema/2);
as=sin(mosigema/2)/mosigema;
rk=[ac as*sigema(1) as*sigema(2) as*sigema(3)]';
% 4.3.6、姿态的四元数更新
qua=quamult(qua,rk);
qua=qua/norm(qua);
Cbn= qua2dcm( qua );
eulr= dcm2eulr( Cbn );
%高度恒设为零 Alti=0;
Alti=0;
% 4.4 结果保存
k = (i+1)/2;
navi(k,1) = t_imu;
navi(k,2) = Lati;
navi(k,3) = Longi;
navi(k,4) = Alti;
navi(k,5) = Vel(1);
navi(k,6) = Vel(2);
navi(k,7) = Vel(3);
navi(k,8) = eulr(1)*180/pi;
navi(k,9) = eulr(2)*180/pi;
navi(k,10) = eulr(3)*180/pi;
% 4.5 结果显示
if mod( k,50/ST) == 0 % 每100s输出一个数据
[Lati Longi Alti Vel(1) Vel(2) Vel(3) navi(k,8) navi(k,9) navi(k,10)]
end
end
fprintf('OK\n')
xnavi=navi(:,2);
ynavi=navi(:,3);
plot(ynavi,xnavi,'k');
hold on;
xpos=pos(:,2);
ypos=pos(:,3);
plot(ypos,xpos);
评论12