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

用四元数法的捷联惯性导航姿态解算程序 close all;clear all;%重力产生的加速度矢量g=9.8;%单位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));grid on;%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,:),-,t