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

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

close

all;

clear

all;

%

重力产生的加速度矢量

单位

9.8m/s^2

G=[0,0,-g]';

%************** **************

读入数据

%**********

读入陀螺仪的数据

gyro_x=load('');

gyro_y=load('');

gyro_z=load('');

%********** ******

读入加速度计的数据

************* *

%acc_rate=3/1024;

acc_x

=load('');

acc_y

=load('');

acc_z=load(''); < /p>

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %

%

加速度数字转换为模拟电压

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);

%

个数

num

n_point=size(gyro_x,2);

%

图

1

figure

plot(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,ac c_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;

%< /p>

零点电压

--

陀螺仪，取前

80

个数的平均电压

zero_gx=sum(gyro_x(1:80))/80

zero_gy=sum(gyro_y(1:80))/80

zero_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;

figure

plot(tm,accelerate(1, :),'-',tm,accelerate(2,:),'.',tm,accelerate(3,:),' -.');

title('

加速度计的采样曲线

');

legend('x_ACC','Y_ACC','Z_ACC');

xlabel('Time

/

(10ms)');

ylabel('Accelerate/

(m/s'')');

grid

on;

%

画出陀螺仪的采样曲线

figure

plot(tm,gyro_x,'r-',t m,gyro_y,'g.',tm,gyro_z,'b-.');

title('

陀螺仪的采样曲线

');

legend('x_Gyro','Y_Gyro','Z_Gyro');

xlabel('Time

/

(10ms)');

ylabel('Angel_rate/

(degree/s)');

grid

on;

%size(gyro_x)

%size(gyro_y)

%size(gyro_z)

data_gyro=[gyro_x;gyro_y;gyro_z];