搜索
%姿态的解算%采用CalibratedSensorData,newrunge-kuta%初始化clearall%DATA=textread('e:\Mt9data\shixiong\MT9_cal_000071C5_002.log');%readdataDATA=textread('f:\xx\MT_cal_00300507_002.log');%Mtidata%DATA=textread('e:\Mt9data\new525\MT9_cal_000071C5_003.log');%readdataWib1=DATA(:,(5:7));fb1=DATA(:,(2:4));%n*3sizeDATA=size(DATA);clearDATA;jingdu=2.02;weidu=0.68;gaodu=0;%初始经度为2.02rad，纬度0.68rad,高度为0(longitude116degrees,latitude39)wie=7.292*10^-5;%地球自转角速度取为7292115.1467*10^-11rad/syoudong=0;%游动角为0R=6378393;%theearthradiusV=[0,0,0]';%初始速度Vep为0%初始姿态矩阵Cbp,由初始对准得到。没有对准时，可以先给定一个值：假设初始完全对准，即各姿态角为0%T=[%%0.17920.9838-0.0108%-0.98380.1791-0.0113%-0.00920.01260.9999];%zh=[0-10;100;001];%transfermatrixqq=[0.9880950.005767-0.0021470.153724]';%四元数初赋qqq=zh*[qq(2);qq(3);qq(4)];q=[qq(1);qqq];T=[q(1)^2+q(2)^2-q(3)^2-q(4)^2,2*(q(2)*q(3)-q(1)*q(4)),2*(q(2)*q(4)+q(1)*q(3));2*(q(2)*q(3)+q(1)*q(4)),q(1)^2-q(2)^2+q(3)^2-q(4)^2,2*(q(3)*q(4)-q(1)*q(2));2*(q(2)*q(4)-q(1)*q(3)),2*(q(3)*q(4)+q(1)*q(2)),q(1)^2-q(2)^2-q(3)^2+q(4)^2]%T即Cbp的计算C=[-sin(youdong)*sin(weidu)*cos(jingdu)-cos(youdong)*sin(jingdu),-sin(youdong)*sin(weidu)*sin(jingdu)+cos(jingdu)*cos(youdong),sin(youdong)*cos(weidu);-cos(youdong)*sin(weidu)*cos(jingdu)+sin(youdong)*sin(jingdu),-cos(youdong)*sin(weidu)*sin(jingdu)-sin(youdong)*cos(jingdu),cos(youdong)*cos(weidu);cos(weidu)*cos(jingdu),cos(weidu)*sin(jingdu),sin(weidu)];%初始位置矩阵CepWie=C*[0,0,wie]';%求Wiep初始Wep=[0,0,0]';%Wep初始化g=(9.7803+0.051799*C(3,3)^2);%重力加速度g初始值，高度h取为0k=8;linshi=0;linshiw=0;linshiV=0;h=0.02;hV=0.02;hc=0.02;flag=1;%flagofqm=4;i=1;%初始化完成whilei=sizeDATA(1,1)-1%td1;%t1%读入数据，该数据need经过坐标变换，由北西天变到了东北天Wib=zh*(Wib1(i+1,:)');%输入陀螺测得的数据Wpb=Wib-T'*(Wep+Wie);%Wib速率提取与姿态速率Wpb的计算%加入Q的及时修正%四元数更新W=[0,-Wpb(1),-Wpb(2),-Wpb(3);Wpb(1),0,Wpb(3),-Wpb(2);Wpb(2),-Wpb(3),0,Wpb(1);Wpb(3),Wpb(2),-Wpb(1),0];A=0.5*W;%四元数微分方程的矩阵形式switch(flag)case(1)A1=A;flag=2;i=i+1;continuecase(2)A3=A;flag=1;endA2=(A1+A3)/2;qn=q;%前一时刻的q值赋给当时刻Ik1=A1*q;%求k1q=qn+(h/2)*k1;k2=A2*q;q=qn+(h/2)*k2;k3=A2*q;q=qn+h*k3;k4=A3*q;q=qn+(h/6)*(k1+2*k2+2*k3+k4);%四元数的归一化，t3%ifmod(i,k)==0;q=q/sqrt((q(1)^2+q(2)^2+q(3)^2+q(4)^2));%endT=[q(1)^2+q(2)^2-q(3)^2-q(4)^2,2*(q(2)*q(3)-q(1)*q(4)),2*(q(2)*q(4)+q(1)*q(3));2*(q(2)*q(3)+q(1)*q(4)),q(1)^2-q(2)^2+q(3)^2-q(4)^2,2*(q(3)*q(4)-q(1)*q(2));2*(q(2)*q(4)-q(1)*q(3)),2*(q(3)*q(4)+q(1)*q(2)),q(1)^2-q(2)^2-q(3)^2+q(4)^2];%T即Cbp的计算%%t2%用二阶龙格－库塔法解速度微分方程%ifmod(i,m)==0;linshiV=linshiV+1;%调试速度程序时引入的临时变量VA=[0,2*Wie(3),-(2*Wie(2)+Wep(2));-2*Wie(3),0,2*Wie(1)+Wep(1);2*Wie(2)+Wep(2),-(2*Wie(1)+Wep(1)),0];%书上115页，公式6.4.9fb=zh*(fb1(i,:)');%输入acce测得的数据，acce采集的是和当前时刻gyro同步的值fp1=T*fb;fb=zh*(fb1(i+1,:)');%输入acce测得的数据，acce采集的是和当前时刻gyro同步的值fp2=T*fb;Vn=V;k1=fp1-[0,0,g]'+VA*V;V=Vn+k1*hV;k2=fp2-[0,0,g]'+VA*V;V=Vn+(hV/2)*(k1+k2);VM(:,linshiV)=V;%temporarylinshiV%a11=-2*0.003367*C(1,3)*C(2,3)/6378393;%-1/taoa%a12=-1/(6378393*(1-0.003367*C(3,3)^2+2*0.003367*C(2,3)^2));%-1/Ryp%a21=1/(6378393*(1-0.003367*C(3,3)^2+2*0.003367*C(1,3)^2));%1/Rxp%a22=-a11;%aa=[a11,a12;a21,a22;0,0];%Vxy=[V(1),V(2)]';%Wep=aa*Vxy;%Wep位置速率的计算%Wep=[000]';Wep=[-V(2)/R;V(1)/R;V(1)*tan(weidu)/R];%地球系为参考系，求出的位置速率CA=[0,Wep(3),-Wep(2);-Wep(3),0,Wep(1);Wep(2),-Wep(1),0];%用欧拉法解位置矩阵微分方程difC=CA*C;C=C+difC*hc;wie=7.292*10^-5;%地球自转角速度取为7292115.1467*10^-11rad/sWie=C*[0,0,wie]';%Wiep的计算g=(9.7803+0.051799*C(3,3)^2);%重力加速度g，高度h取为0%%else%continue%end%t4之姿态角的计算%ifmod(i,2*m)==0;%T=zh'*T;linshi=linshi+1;fuyang=asin(T(3,2));%俯仰角值为其主值gunzhuanm=atan(-T(3,1)/T(3,3));hangxiangm=atan(-T(1,2)/T(2,2));%三个姿态角的主值ifT(3,3)0;gunzhuan=gunzhuanm;elseif(T(3,3)0)&(gunzhuanm0);gunzhuan=gunzhuanm+pi;else(T(3,3)0)&(gunzhuanm0);gunzhuan=gunzhuanm-pi;end%滚转角if(T(2,2)0)&(hangxiangm0);hangxiang=hangxiangm;elseif(T(2,2)0)&(hangxiangm0);hangxiang=hangxiangm+2*pi;elseT(2,2)0;hangxiang=hangxiangm+pi;end%航向角hangxiangM(linshi,1)=hangxiang*180/pi;fuyangM(linshi,1)=fuyang*180/pi;gunzhuanM(linshi,1)=gunzhuan*180/pi;%CM(:,:,linshi)=C;%position%ifmod(i,(2*m))==0;linshiw=linshiw+1;jingdum=atan(C(3,2)/C(3,1));weidu=asin(C(3,3));%latitude(-90,90)youdongm=atan(C(1,3)/C(2,3));%经纬度，游动角主值计算ifC(3,1)0;jingdu=jingdum;elseif(C(3,1)0)&(jingdum0);jingdu=jingdum+pi;else(C(3,1)0)&(jingdum0);jingdu=jingdum-pi;end%经度计算,(-180,180)if(C(2,3)0)&(youdongm0);youdong=youdongm;elseif(C(2,3)0)&(youdongm0);youdong=youdongm+2*pi;elseC(2,3)0;youdong=youdongm+pi;end%youdong(0,360)%地速SpeedM(linshiw,1)=sqrt(V(1)^2+V(2)^2);%jingduM(linshiw,1)=jingdu*180/pi;weiduM(linshiw,1)=weidu*180/pi;youdongM(linshiw,1)=youdong*180/pi;%end%else%continue%end%draw3-Dwithcuboid%ifmod((i-1),m)==0%redsurfacesarealongwithxaxis,yellowsurfacesarealongwithyaxis,garezaxis%mtiplotcuboid(T,V,jingdu,weidu,youdong,i);%plot30s，没有画初始时刻的图，从第一个更新T开始画图%mti_BoxPlot3(T)%endendclearflagzhqqqqqkmhhVhc;figuresubplot(2,1,1),plot(hangxiangM)holdonplot(fuyangM,'g')plot(gunzhuanM,'r')title('姿态角')subplot(2,1,2),plot(VM(1,:),'r')holdonplot(VM(2,:),'y')plot(VM(3,:),'g')title('速度')