单子样惯导和松组合.zip

%捷联惯导程序，依据加速度计和陀螺仪的输出数据来求解姿态 clc; clear; format long;%设置数据精度为15位小数 load('a'); initpara; imudata=importdata('a.mat'); Data=calimudata(imudata);%标定数据，先进行初始化，运行initpara.m [row, columu] = size(Data); pos = importdata("pos_back.mat"); %采样时间为0.01秒 Delta_t=0.01; %定义陀螺仪输出的角度增量 ang_add = zeros(3,1); %定义加速度计输出的比力增量 f_add = zeros(3,1); L = zeros(1,1331997); L(1) = 5.072430935313897e-001; %纬度用L表示，纬度的初值化为弧度形式 L(2) = 5.072430935313897e-001; %备用 Lamda = 1.947737191682270e+000; %经度用Lamda表示，经度的初始值化为弧度形式 h = 6.393491936288774e+001; V = [0;0;0];%导航坐标系中东北天初始速度为零 %定义速度值 Vx = 0; Vy = 0; Vz = 0; %定义姿态角-0.89615°,-0.026739°,-0.32493°北天东（东北天-0.32493°-0.89615°-0.026739°） Theta = -0.32493*pi/180; Gama = -0.89615*pi/180; Fai = -0.026739*pi/180; fprintf('十分钟对准结束，初始姿态角为横滚角-0.89615°，俯仰角-0.32493°，航偏角-0.026739°\n'); %定义地球相关参数（部分参考初始化程序定义） Re = 6378254; Rp = 6356803; e = (Re-Rp)/Re; e = sqrt(e*(2-e)); Wie = 7.292115e-5; %定义各要素矩阵,供画图使用 Theta_Matrix = zeros(1,1331996); Gama_Matrix = zeros(1,1331996); Fai_Matrix = zeros(1,1331996); L_Matrix = zeros(1,1331997); L_Matrix(1) = 5.072430935313897e-001/pi*180; Lamda_Matrix = zeros(1,1331996); Ve_Matrix = zeros(1,1331996); Vn_Matrix = zeros(1,1331996); Vu_Matrix = zeros(1,1331996); Wibb = zeros(3,1331996); Wnbb = zeros(3,1331996); %计算姿态矩阵初始值 Cbn = [cos(Gama)*cos(Fai)+sin(Gama)*sin(Theta)*sin(Fai) cos(Theta)*sin(Fai) sin(Gama)*cos(Fai)-cos(Gama)*sin(Theta)*sin(Fai); -cos(Gama)*sin(Fai)+sin(Gama)*sin(Theta)*cos(Fai) cos(Theta)*cos(Fai) -sin(Gama)*sin(Fai)-cos(Gama)*sin(Theta)*cos(Fai); -sin(Gama)*cos(Theta) sin(Theta) cos(Gama)*cos(Theta)]; %姿态矩阵换算到四元数 Q0 = 1/2*sqrt(1+Cbn(1,1)+Cbn(2,2)+Cbn(3,3)); Q1 = 1/4/Q0*(Cbn(3,2)-Cbn(2,3)); Q2 = 1/4/Q0*(Cbn(1,3)-Cbn(3,1)); Q3 = 1/4/Q0*(Cbn(2,1)-Cbn(1,2)); Q = [Q0;Q1;Q2;Q3]; Q = Q/sqrt(Q0*Q0+Q1*Q1+Q2*Q2+Q3*Q3); %求位置矩阵初值，更新后计算经纬度 %Ce2n表示地球系到导航系的转化矩阵 Ce2n=[-sin(Lamda) cos(Lamda) 0; -sin(L(1))*cos(Lamda) -sin(L(1))*sin(Lamda) cos(L(1)); cos(L(1))*cos(Lamda) cos(L(1))*sin(Lamda) sin(L(1))]; %实时更新姿态矩阵、速度矩阵和位移矩阵 %假设对准结束，花费10分钟对准姿态角（-0.89615°,-0.026739°,-0.32493°） flag_time = 1; for k = 60001:1:row i = k-60000; if(i>5*flag_time*60/Delta_t) fprintf('\tnavigation %d min;\n',5*flag_time) flag_time=flag_time+1; end Wibb(1,i) = Data(k,4); Wibb(2,i) = Data(k,2); Wibb(3,i) = Data(k,3); Rm=Re*(1-2*e+3*e*sin(L(i+1))*sin(L(i+1))); Rn=Re*(1+e*sin(L(i+1))*sin(L(i+1))); LL=3/2*L(i+1)-1/2*L(i); F=[0 -1/(Rm+h) 0; 1/(Rn+h) 0 0; tan(LL)/(Rn+h) 0 0]; Wen2n=F*V; Wie2n=[0;Wie*cos(L(i+1));Wie*sin(L(i+1))]; Winn = Wen2n + Wie2n; Cnb = Cbn'; Wnbb(:,i) = Wibb(:,i) - Cnb * Winn * Delta_t; ang_add(1) = Wnbb(1,i); ang_add(2) = Wnbb(2,i); ang_add(3) = Wnbb(3,i); f_add(1) = Data(k,7); f_add(2) = Data(k,5); f_add(3) = Data(k,6); %毕卡法求解四元数更新矩阵 square=ang_add'*ang_add; ang_Matric=[0 -ang_add(1) -ang_add(2) -ang_add(3); ang_add(1) 0 ang_add(3) -ang_add(2); ang_add(2) -ang_add(3) 0 ang_add(1); ang_add(3) ang_add(2) -ang_add(1) 0]; Bike_Matric=(1-square/8+square^2/348)*eye(4)+(0.5-square/48)*ang_Matric; Q = Bike_Matric * Q; Q0 = Q(1); Q1 = Q(2); Q2 = Q(3); Q3 = Q(4); %更新姿态矩阵 Cbn = [Q0*Q0+Q1*Q1-Q2*Q2-Q3*Q3 2*(Q1*Q2-Q0*Q3) 2*(Q1*Q3+Q0*Q2) 2*(Q1*Q2+Q0*Q3) Q0*Q0-Q1*Q1+Q2*Q2-Q3*Q3 2*(Q2*Q3-Q0*Q1) 2*(Q1*Q3-Q0*Q2) 2*(Q2*Q3+Q0*Q1) Q0*Q0-Q1*Q1-Q2*Q2+Q3*Q3]; %T = T'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求解三个姿态角%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Theta_Main = asin(Cbn(3,2)); Gama_Main = atan(-Cbn(3,1)/Cbn(3,3)); Fai_Main = atan(Cbn(1,2)/Cbn(2,2)); Theta = Theta_Main; if(Cbn(3,3)>0) Gama = Gama_Main; else if(Cbn(3,3)==0) Gama = Gama_Main - pi; else if(Gama_Main>0) Gama = Gama_Main + pi; end end end if(Cbn(2,2)>0) Fai = Fai_Main; else if(Cbn(2,2)==0) if(Cbn(1,2)>0) Fai = pi/2; else Fai = -pi/2; end else if(Cbn(1,2)>0) Fai = Fai_Main + pi; else Fai = Fai_Main - pi; end end end %储存姿态角到矩阵供画图使用 Theta_Matrix(i) = Theta*180/pi; Gama_Matrix(i) = Gama*180/pi; if(Fai<2*pi) Fai_Matrix(i) = Fai*180/pi; else Fai_Matrix(i) = Fai*180/pi-360; end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求解东北天速度%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% g=9.7803+0.051799*sin(L(i+1))*sin(L(i+1))-0.94114e-006*h; G=[0;0;-g]; Wen2n=F*V; Wie2n=[0;Wie*cos(L(i+1));Wie*sin(L(i+1))]; W=2*Wie2n+Wen2n; %W的反对称矩阵 W_X=[0 -W(3) W(2); W(3) 0 -W(1); -W(2) W(1) 0]; Vsfm = [1 -ang_add(3) ang_add(2);ang_add(3) 1 -ang_add(1);-ang_add(2) ang_add(1) 1]*[f_add(1);f_add(2);f_add(3)]; V = V + Delta_t*Cbn*Vsfm + Delta_t*(G-W_X*V); V(3) = 0;%天向速度置零减小误差 Ve_Matrix(i) = V(1); Vn_Matrix(i) = V(2); Vu_Matrix(i) = V(3); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%求解所在经纬度%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Ep_matrix = F*V*Delta_t; %矩阵位置实时更新 Ce2n = (eye(3) -[0 -Ep_matrix(3) Ep_matrix(2);Ep_matrix(3) 0 -Ep_matrix(1);-Ep_matrix(2) Ep_matrix(1) 0])*Ce2n; %通过位置矩阵实时更新经纬度 %定义一个把L用角度表示的矩阵 L(i+2)=asin(Ce2n(3,3)); L_Matrix(i+1)= L(i+2)*180/pi; Lamda_Main = atan(Ce2n(3,2)/Ce2n(3,1)); if(Ce2n(3,1)>0) Lamda = Lamda_Main; else if(Lamda_Main<0) Lamda=Lamda_Main + pi; else Lamda=Lamda_Main - pi; end end Lamda_Matrix(i)=Lamda*180/pi; end fprintf('导航结束，正在作图！\n'); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%画图程序%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% i = 1:1:row-60000;%绘制三轴姿态变化图线 figure(1) subplot(3,1,1); plot(i/100,Fai_Matrix(i)); title('偏航角');grid on subplot(3,1,2); plot(i/100,Theta_Matrix(i)); title('俯仰角');grid on subplot(3,1,3); plot(i/100,Gama_Matrix(i)); title('滚动角'); grid on figure(2); subplot(2,1,1); plot(i/100,Ve_Matrix(i)); xlabel('Time(second)'); ylabel('Velocity(m/s)'); title('Ve（东向速度）'); grid on; subplot(2,1,2); plot(i/100,Vn_Matrix(i)); xlabel('Time(second)'); ylabel('Velocity(m/s)'); title('Vn（北向速度）'); grid on; figure(3); subplot(3,1,1); plot(i/100,Lamda_Matrix(i),'b'); xlabel('Time(second)'); ylabel('Degree'); title('LongitudeLamda(经度)'); grid on; subplot(3,1,2); plot(i/100,L_Matrix(i),'b'); xlabel('Time(second)'); ylabel('Degree'); title('LatitudeL(纬度)'); grid on; subplot(3,1,3); plot(Lamda_Matrix(i),L_Matrix(i),'b'); xlabel('LongitudeLamda(经度)'); ylabel('LatitudeL(纬度)'); title('position(位置)'); grid on; j = 1577:1:14895; figure(4) subplot(3,1,1); plot(pos(1,j)-600,pos(10,j)/pi*180); title('经度');grid on subplot(3,1,2); plot(pos(1,j)-600,pos(8,j)/pi*180); title('纬度');grid on subplot(3,1,3); plot(pos(10,j)/pi*180,pos(8,j)/pi*180); title('位置');grid on figure(5) subplot(2,1,1); plot(pos(1,j)-600,pos(7,j)); title('东向速度');grid on subplot(2,1,2); plot(pos(1,j)-600,pos(5,j)); title('北向速度');grid on l = (j-976)/0.01-60000; figure(6) subplot(2,1,1); plot(po