oulajiao.rar_matlab 欧拉_matlab欧拉角法_oulajiao_姿态_欧拉角

%欧拉角 clc clear all %%度数与弧度转换%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% dtor=pi/180; rtod=180/pi; %采样时间 Hz 采样时间的调整可以适当将曲线拟合的更好 T=0.01; %%%%%%%%%%%%%%%%%%%%圆锥运动给出角速率%%%%%%%%%%%%%%%%%%%%%% n = 6000; W = 2*pi; %圆锥运动角速度（频率） a = 1*dtor; %半锥角 for t=0:1:n gyro_w(1,t+1) = -2*W*((sin(a/2))^2); %x轴角速度 gyro_w(2,t+1) = -W*(sin(a))*(sin(W*t*T)); %y轴角速度 gyro_w(3,t+1) = W*(sin(a))*(cos(W*t*T)); %z轴角速度 Q_real(:,t+1) = [cos(a/2) 0 sin(a/2)*cos(W*t*T) sin(a/2)*sin(W*t*T)]'; %4行n+1列 end %%%%%%%%%%%%%%%%%%%开始（坐标系东北天）%%%%%%%%%%%%%%%%%%%%%%%%%%% %各种参数的初始值 Wie=7.292115147e-5; %地球自转角速度（rad/s） Re=6378254; %地球椭球长半径m f=1/298.3; %扁率 e=1-(1-f)^2; %第一偏心率 % Lamda=116*dtor; %初始经度（弧度） % L=40*dtor; %初始纬度（弧度） % Vx=0; %x通道速度 % Vy=0; %y通道速度 % Vz=0; %z通道速度 %%初始角度解算%%%%%%%%%%%%%%%%%%%%%%%%%% e0=Q_real(1,1);e1=Q_real(2,1);e2=Q_real(3,1);e3=Q_real(4,1); Q=[e0 e1 e2 e3]'; Cbn=[e0^2+e1^2-e2^2-e3^2 2*(e1*e2-e0*e3) 2*(e1*e3+e0*e2); 2*(e1*e2+e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3-e0*e1); 2*(e1*e3-e0*e2) 2*(e2*e3+e0*e1) e0^2-e1^2-e2^2+e3^2]; Cnb= (Cbn)'; Seita_real(1)=asin(Cbn(3,2)); %%%%横滚角计算 if (abs(Cbn(3,3))>eps) Gama_real(1)=atan(-Cbn(3,1)/Cbn(3,3)); if(Gama_real(1)>0 && Cbn(3,3)<0) Gama_real(1)=Gama_real(1)-pi; elseif(Gama_real(1)<0 && Cbn(3,3)<0) Gama_real(1)=Gama_real(1)+pi; else Gama_real(1)=Gama_real(1); end elseif Cbn(3,1)<0 Gama_real(1)=pi/2; else Gama_real(1)=-pi/2; end %%%%航向角计算(eps=2.2204e-016(10的多少次方)很小的数) if (abs(Cbn(2,2))>eps) Ksai_real(1)=atan(Cbn(1,2)/Cbn(2,2)); if (Cbn(2,2)>0) Ksai_real(1)=Ksai_real(1); elseif (Cbn(1,2)> 0) Ksai_real(1)=Ksai_real(1)+pi; else Ksai_real(1)=Ksai_real(1)-pi; end elseif Cbn(1,2)>0 Ksai_real(1)=pi/2; else Ksai_real(1)=-pi/2; end Seita(1)=Seita_real(1); %真实俯仰角（弧度）x Gama(1)=Gama_real(1); %真实横滚角（弧度）y Ksai(1)=Ksai_real(1); %真实航向角（弧度）z % Rm=Re*((1-e)^2)*((1-(2-e)*e*((sin(L))^2))^(-1.5)); %沿子午圈的曲率半径（精确的） % Rn=Re*((1-(2-e)*e*((sin(L))^2))^(-0.5)); %沿卯酉圈的曲率半径 % % Wetx_n=-Vy/Rm;Wety_n=Vx/Rn;Wetz_n=Vx*tan(L)/Rn; %在地理坐标系的位移角速率 % Wen_n=[Wetx_n Wety_n Wetz_n]'; % Wie_n=[0 Wie*cos(L) Wie*sin(L)]'; %在地理坐标系的地球角速率 for i=1:n %%%%%%%%%%%i+1将用当前角速率值算当前姿态角（i是用前一时刻的角速率值算当前姿态角）%%%%%%%%%%%%%%%%%%%%%%% Wtb_b=[gyro_w(1,i+1) gyro_w(2,i+1) gyro_w(3,i+1)]'; %陀螺仪测的角速率值 % Wtb_b=Wib_b-Cnb*(Wie_n+Wen_n); %姿态矩阵角速率 %%%%%欧拉角法（解欧拉角微分方程） Ksai(i+1)=Ksai(i)+(sin(Gama(i))/cos(Seita(i))*Wtb_b(1)-cos(Gama(i))/cos(Seita(i))*Wtb_b(3))*T; %航向角 Seita(i+1)=Seita(i)+(cos(Gama(i))*Wtb_b(1)+sin(Gama(i))*Wtb_b(3))*T; %俯仰角 Gama(i+1)=Gama(i)+(sin(Gama(i))*tan(Seita(i))*Wtb_b(1)+Wtb_b(2)-cos(Gama(i))*tan(Seita(i))*Wtb_b(3))*T;%横滚角 %更新姿态矩阵 Cnb=[cos(Gama(i+1))*cos(Ksai(i+1))+sin(Gama(i+1))*sin(Ksai(i+1))*sin(Seita(i+1)) -cos(Gama(i+1))*sin(Ksai(i+1))+sin(Gama(i+1))*cos(Ksai(i+1))*sin(Seita(i+1)) -sin(Gama(i+1))*cos(Seita(i+1)); sin(Ksai(i+1))*cos(Seita(i+1)) cos(Ksai(i+1))*cos(Seita(i+1)) sin(Seita(i+1)); sin(Gama(i+1))*cos(Ksai(i+1))-cos(Gama(i+1))*sin(Ksai(i+1))*sin(Seita(i+1)) -sin(Gama(i+1))*sin(Ksai(i+1))-cos(Gama(i+1))*cos(Ksai(i+1))*sin(Seita(i+1)) cos(Gama(i+1))*cos(Seita(i+1))]; Cbn= (Cnb)'; %%计算真实姿态角和航向角%% r0=Q_real(1,i+1);r1=Q_real(2,i+1);r2=Q_real(3,i+1);r3=Q_real(4,i+1); Cbn_real=[r0^2+r1^2-r2^2-r3^2 2*(r1*r2-r0*r3) 2*(r1*r3+r0*r2); 2*(r1*r2+r0*r3) r0^2-r1^2+r2^2-r3^2 2*(r2*r3-r0*r1); 2*(r1*r3-r0*r2) 2*(r2*r3+r0*r1) r0^2-r1^2-r2^2+r3^2]; %%%%俯仰角计算 Seita_real(i+1)=asin(Cbn_real(3,2)); %%%%横滚角计算 if (abs(Cbn_real(3,3))>eps) Gama_real(i+1)=atan(-Cbn_real(3,1)/Cbn_real(3,3)); if(Gama_real(i+1)>0 && Cbn_real(3,3)<0) Gama_real(i+1)=Gama_real(i+1)-pi; elseif(Gama_real(i+1)<0 && Cbn_real(3,3)<0) Gama_real(i+1)=Gama_real(i+1)+pi; else Gama_real(i+1)=Gama_real(i+1); end elseif Cbn_real(3,1)<0 Gama_real(i+1)=pi/2; else Gama_real(i+1)=-pi/2; end %%%%航向角计算(eps=2.2204e-016(10的多少次方)很小的数) if (abs(Cbn_real(2,2))>eps) Ksai_real(i+1)=atan(Cbn_real(1,2)/Cbn_real(2,2)); if (Cbn_real(2,2)>0) Ksai_real(i+1)=Ksai_real(i+1); elseif (Cbn_real(1,2)> 0) Ksai_real(i+1)=Ksai_real(i+1)+pi; else Ksai_real(i+1)=Ksai_real(i+1)-pi; end elseif Cbn_real(1,2)>0 Ksai_real(i+1)=pi/2; else Ksai_real(i+1)=-pi/2; end %%end%%%%%%%%%%%%%%%%%%%% end %%%结束%%% %%%误差使用弧度为单位%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%俯仰角误差 Seita_err = Seita-Seita_real; %%%横滚角 Gama_err = Gama-Gama_real; %%%航向角 Ksai_err = Ksai-Ksai_real; %绘制曲线图 t=0:T:(n*T); %绘制俯仰角变化曲线图 figure(1); subplot(3,1,1) plot(t,Seita_err) grid on % xlabel('时间/秒'); ylabel('俯仰角误差/rad'); title('俯仰角误差变化曲线图'); %绘制横滚角变化曲线图 subplot(3,1,2) plot(t,Gama_err) grid on %网格 % xlabel('时间/秒'); ylabel('横滚角误差/rad'); title('横滚角误差变化曲线图'); %绘制航向角变化曲线图 subplot(3,1,3) plot(t,Ksai_err) grid on xlabel('t/s'); ylabel('航向角误差/rad'); title('航向角误差变化曲线图'); %%end%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%