用四元数法的捷联惯性导航姿态解算程序

用四元数法的捷联惯性导航姿态解算程序closeall;clearall;%重力产生的加速度矢量g=9.80142;%单位9.8m/s^2G=[0,0,-g]';%****************************读入数据%**********读入陀螺仪的数据gyro_x=load('gyrox.txt');gyro_y=load('gyroy.txt');gyro_z=load('gyroz.txt');%****************读入加速度计的数据**************%acc_rate=3/1024;acc_x=load('acceleratex.txt');acc_y=load('acceleratey.txt');acc_z=load('acceleratez.txt');%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%加速度数字转换为模拟电压data_acc=[acc_x;acc_y;acc_z];data_acc=data_acc/1024*3%将数据转换为相应的加速度值%%*********************************************************%加速度计三个轴向的零点电压%zero_ax=?%zero_ay=?%zero_az=?%加速度计三个轴向的电压/加速度比值%rate_ax=?%单位是m/s^2/V%rate_ay=?%rate_az=?%acc_x=acc_x*acc_rate;%acc_y=acc_y*acc_rate;%acc_z=acc_z*acc_rate;aver_acc_x=mean(acc_x)aver_acc_y=mean(acc_y)aver_acc_z=mean(acc_z)%采样时间dtime=0.01;tm=0:dtime:0.01*(size(gyro_x,2)-1);%个数numn_point=size(gyro_x,2);%图1figureplot(tm,data_acc(1,:),'-',tm,data_acc(2,:),'.',tm,data_acc(3,:),'-.');title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time/(10ms)');ylabel('Accelerate/(m/s'')');gridon;%plot(tm,acc_x,'-',tm,acc_y,'.',tm,acc_z,'-.');%title('加速度的计的采样曲线'):%对采样曲线进行低通滤波a=[1,2,4,2,1];%gyro_x=filter(a/sum(a),1,gyro_x);%gyro_y=filter(a/sum(a),1,gyro_y);%gyro_z=filter(a/sum(a),1,gyro_z);%比例变换gyro_x=gyro_x/1024*3/0.6;gyro_y=gyro_y/1024*3/0.6;gyro_z=gyro_z/1024*3/0.6;%零点电压--陀螺仪，取前80个数的平均电压zero_gx=sum(gyro_x(1:80))/80zero_gy=sum(gyro_y(1:80))/80zero_gz=sum(gyro_z(1:80))/80%减去零点gyro_x=(gyro_x-zero_gx)/0.0125/180*pi;gyro_y=(gyro_y-zero_gy)/0.0125/180*pi;gyro_z=(gyro_z-zero_gz)/0.0125/180*pi;%gyro_x=(gyro_x-2.5)/0.0125/180*pi;%gyro_y=(gyro_y-2.5)/0.0125/180*pi;%gyro_z=(gyro_z-2.5)/0.0125/180*pi;%测试数据accelerate=zeros(3,n_point);accelerate(1,1:100)=10;accelerate(1,101:200)=-10;accelerate(1,201:300)=0;%陀螺仪数据gyro_x=zeros(1,n_point);gyro_y=zeros(1,n_point);gyro_z=zeros(1,n_point);gyro_z(1:100)=pi/3;gyro_z(101:200)=-pi/3;%重力轴始终有加速度accelerate(3,:)=accelerate(3,:)+9.8;figureplot(tm,accelerate(1,:),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),'-.');title('加速度计的采样曲线');legend('x_ACC','Y_ACC','Z_ACC');xlabel('Time/(10ms)');ylabel('Accelerate/(m/s'')');gridon;%画出陀螺仪的采样曲线figureplot(tm,gyro_x,'r-',tm,gyro_y,'g.',tm,gyro_z,'b-.');title('陀螺仪的采样曲线');legend('x_Gyro','Y_Gyro','Z_Gyro');xlabel('Time/(10ms)');ylabel('Angel_rate/(degree/s)');gridon;%size(gyro_x)%size(gyro_y)%size(gyro_z)data_gyro=[gyro_x;gyro_y;gyro_z];%转移矩阵--即方向余弦矩阵T=eye(3);%T是3*3的单位矩阵,初始转移矩阵%四元数矩阵，存储每步更新之后的四元数，方便以后绘图Q=zeros(4,n_point);%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。Q(1,1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3));Q(2,1)=0.5*sqrt(1+T(1,1)-T(2,2)-T(3,3));Q(3,1)=0.5*sqrt(1-T(1,1)+T(2,2)-T(3,3));Q(4,1)=0.5*sqrt(1-T(1,1)-T(2,2)+T(3,3));%参见捷联惯性导航技术31页3.64式在旋转90度时不适用%Q(1,1)=0.5*sqrt(1+T(1,1)+T(2,2)+T(3,3));%Q(2,1)=1/4/Q(1,1)*(T(3,2)-T(2,3));%Q(3,1)=1/4/Q(1,1)*(T(1,3)-T(3,1));%Q(4,1)=1/4/Q(1,1)*(T(2,1)-T(1,2));%求姿态角矩阵ANGLE=zeros(3,n_point);%angle[1]代表绕X轴转过的角度，2代表Y轴,3代表Z轴%方向余弦矩阵到欧拉角的转换关系，这里注意旋转顺序是Z-Y-X，参考文献一种全新的全角度元元数与欧拉角的转换算法%位置矩阵position=zeros(3,n_point);%位置矩阵velocity=zeros(3,n_point);%速度矩阵%重力加速度%acc_g=[0,0,-9.8]';qh=[0,0,0,0];fori=1:n_point%开始循环ifi1velocity(:,i)=((T*accelerate(:,i-1)+T*accelerate(:,i))/2+G)*dtime+velocity(:,i-1);%要考虑到重力的影响，假定重量方向与子轴方向一致position(:,i)=position(:,i-1)+(velocity(:,i-1)+velocity(:,i))*dtime/2;end%计算欧拉角,假定俯仰角在+_90度范围移动，而滚动角和偏航角在+-180度范围内取值%ANGLE(1,i)=atan(T(2,3)/T(3,3));%ANGLE(2,i)=asin(-T(1,3));%ANGLE(3,i)=atan(T(1,2)/T(1,1));ifT(3,3)0%根据物理意义不可能出现0ANGLE(1,i)=-atan(T(2,3)/T(3,3));elseANGLE(1,i)=-pi*sign(T(2,3))-atan(T(2,3)/T(3,3));end%俯仰角ANGLE(2,i)=-asin(-T(1,3));%偏航角ifT(1,1)0%公式似乎有误，直接按公式计算是负值ANGLE(3,i)=-atan(T(1,2)/T(1,1));elseANGLE(3,i)=-pi*sign(T(1,2))-atan(T(1,2)/T(1,1));endANGLE(1,i)=atan(T(3,2)/T(3,3));ANGLE(2,i)=asin(-T(3,1));ANGLE(3,i)=atan(T(2,1)/T(1,1));%更新四元数ifin_point%如果还没有到超出数组范围theta=data_gyro(:,i)*dtime;%角度向量dtheta=sqrt(theta'*theta);%i要保证当theta为零时算法仍有关效ifdtheta==0qh=[1,0,0,0];else%换用简化算法试验结果%qh=[cos(dtheta);theta*sin(dtheta/2)/dtheta];qh=[1;0.5*theta];end%更新四元数Q(:,i+1)=[qh(1),-qh(2),-qh(3),-qh(4);qh(2),qh(1),-qh(4),qh(3);qh(3),qh(4),qh(1),-qh(2);qh(4),-qh(3),qh(2),qh(1)]*Q(:,i);%更新方向方向余弦矩阵T=[1-2*(Q(3,i+1)*Q(3,i+1)+Q(4,i)*Q(4,i+1))2*(Q(2,i+1)*Q(3,i+1)-Q(1,i+1)*Q(4,i+1))2*(Q(2,i+1)*Q(4,i+1)+Q(1,i+1)*Q(3,i+1));2*(Q(2,i+1)*Q(3,i+1)+Q(1,i+1)*Q(4,i+1))1-2*(Q(2,i+1)*Q(2,i+1)+Q(4,i+1)*Q(4,i+1))2*(Q(3,i+1)*Q(4,i+1)-Q(1,i+1)*Q(2,i+1));2*(Q(2,i+1)*Q(4,i+1)-Q(1,i+1)*Q(3,i+1))2*(Q(3,i+1)*Q(4,i+1)+Q(1,i+1)*Q(2,i+1))1-2*(Q(2,i+1)*Q(2,i+1)+Q(3,i+1)*Q(3,i+1))];%得到姿态矩阵endendfigureANGLE=ANGLE*180/pi;plot(tm,ANGLE(1,:),'r-',tm,ANGLE(2,:),'g.',tm,ANGLE(3,:),'b-.');legend('PitchAngel','RollAngel','YawAngel');title('GestureCalculation');xlabel('Time/(10ms)');ylabel('Angel/(degree)');gridon;figureplot(tm,velocity)legend('NavigationCoordinate:X','NavigationCoordinate:Y','NavigationCoordinate:Z');title('VelocityCalculation');xlabel('Time/(10ms)');ylabel('Velocity/(m/s)');gridon;figureplot(tm,position);legend('NavigationCoordinate:X','NavigationCoordinate:Y','NavigationCoordinate:Z');title('Pos