纯惯性导航轨迹跟踪代码Matlab

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);