📄 mti_calibrated.m
字号:
%姿态的解算
%采用Calibrated Sensor Data, new runge-kuta
%初始化
clear all
%DATA=textread('e:\Mt9data\shixiong\MT9_cal_000071C5_002.log'); %read data
DATA=textread('e:\Mtidata\MT_cal_00300507_002.log'); % Mti data
%DATA=textread('e:\Mt9data\new525\MT9_cal_000071C5_003.log'); %read data
Wib1=DATA(:,(5:7));
fb1=DATA(:,(2:4)); %n*3
sizeDATA=size(DATA);
clear DATA;
jingdu=2.02;
weidu=0.68;
gaodu=0; %初始经度为2.02rad，纬度0.68rad,高度为0(longitude 116 degrees,latitude 39)
wie=7.292*10^-5; %地球自转角速度取为7292115.1467*10^-11rad/s
youdong=0; %游动角为0
R=6378393; %the earth radius
V=[0,0,0]'; %初始速度Vep为0
%初始姿态矩阵Cbp,由初始对准得到。没有对准时，可以先给定一个值：假设初始完全对准，即各姿态角为0
%T =[
%
% 0.1792 0.9838 -0.0108
% -0.9838 0.1791 -0.0113
% -0.0092 0.0126 0.9999];
%
zh=[0 -1 0;
1 0 0;
0 0 1]; %transfer matrix
qq=[0.988095 0.005767 -0.002147 0.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)];
%初始位置矩阵Cep
Wie=C*[0,0,wie]'; %求Wiep初始
Wep=[0,0,0]'; %Wep初始化
g=(9.7803+0.051799*C(3,3)^2); %重力加速度g初始值，高度h取为0
k=8;
linshi=0;linshiw=0;linshiV=0;
h=0.02;
hV=0.02;
hc=0.02;
flag=1; %flag of q
m=4;
i=1;
%初始化完成
while i<=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;
continue
case(2)
A3=A;
flag=1;
end
A2=(A1+A3)/2;
qn=q; %前一时刻的q值赋给当时刻I
k1=A1*q; %求k1
q=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
%if mod(i,k)==0;
q=q/sqrt((q(1)^2+q(2)^2+q(3)^2+q(4)^2));
%end
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的计算
%
%t2
%用二阶龙格－库塔法解速度微分方程
%if mod(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.9
fb=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; %temporary linshiV
%a11=-2*0.003367*C(1,3)*C(2,3)/6378393; %-1/tao a
%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=[0 0 0]';
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/s
Wie=C*[0,0,wie]'; %Wiep的计算
g=(9.7803+0.051799*C(3,3)^2); %重力加速度g，高度h取为0
%
%else
%continue
%end
%t4之姿态角的计算
%if mod(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)); %三个姿态角的主值
if T(3,3)>0;
gunzhuan=gunzhuanm;
elseif (T(3,3)<0)&(gunzhuanm<0);
gunzhuan=gunzhuanm+pi;
else (T(3,3)<0)&(gunzhuanm>0);
gunzhuan=gunzhuanm-pi;
end %滚转角
if (T(2,2)>0)&(hangxiangm>0);
hangxiang=hangxiangm;
elseif (T(2,2)>0)&(hangxiangm<0);
hangxiang=hangxiangm+2*pi;
else T(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
%if mod(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)); % 经纬度，游动角主值计算
if C(3,1)>0;
jingdu=jingdum;
elseif (C(3,1)<0)&(jingdum<0);
jingdu=jingdum+pi;
else (C(3,1)<0)&(jingdum>0);
jingdu=jingdum-pi;
end %经度计算,(-180,180)
if (C(2,3)>0)&(youdongm>0);
youdong=youdongm;
elseif (C(2,3)>0)&(youdongm<0);
youdong=youdongm+2*pi;
else C(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
%draw 3-D with cuboid
%if mod((i-1),m)==0 % red surfaces are along with x axis, yellow surfaces are along with y axis,g are z axis
% mtiplotcuboid(T,V,jingdu,weidu,youdong,i); % plot 30 s ，没有画初始时刻的图，从第一个更新T开始画图
%mti_BoxPlot3(T)
%end
end
clear flag zh qq qqq k m h hV hc;
figure
subplot(2,1,1),plot(hangxiangM)
hold on
plot(fuyangM,'g')
plot(gunzhuanM,'r')
title('姿态角')
subplot(2,1,2),plot(VM(1,:),'r')
hold on
plot(VM(2,:),'y')
plot(VM(3,:),'g')
title('速度')
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -