www.pudn.com > sins-code-navigation.rar > SINS-navigation.m, change:2009-03-15,size:6598b


close all; 
clear all; 
%重力产生的加速度矢量 
g=9.8;%单位9.8m/s^2 
G=[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); 
%个数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,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))/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-',tm,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]; 
%转移矩阵--即方向余弦矩阵 
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]; 
for i=1:n_point %开始循环 
    if i>1 
  velocity(:,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)); 
  if T(3,3)>0  %根据物理意义不可能出现0 
    ANGLE(1,i)=-atan(T(2,3)/T(3,3)); 
else 
    ANGLE(1,i)=-pi*sign(T(2,3))-atan(T(2,3)/T(3,3)); 
  end 
%俯仰角 
ANGLE(2,i)=-asin(-T(1,3)); 
%偏航角 
if T(1,1)>0%公式似乎有误,直接按公式计算是负值 
    ANGLE(3,i)=-atan(T(1,2)/T(1,1)); 
else 
    ANGLE(3,i)=-pi*sign(T(1,2))-atan(T(1,2)/T(1,1)); 
end 
   
   
  ANGLE(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)); 
%更新四元数 
   if i<n_point %如果还没有到超出数组范围 
       theta=data_gyro(:,i)*dtime;%角度向量 
       dtheta=sqrt(theta'*theta); 
       %i要保证当theta为零时算法仍有关效 
       if dtheta==0 
           qh=[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))]; %得到姿态矩阵 
    end 
        
        
        
end 
 figure 
 ANGLE=ANGLE*180/pi; 
plot(tm,ANGLE(1,:),'r-',tm,ANGLE(2,:),'g.',tm,ANGLE(3,:),'b-.'); 
legend('Pitch Angel','Roll Angel','Yaw Angel'); 
title('Gesture Calculation'); 
xlabel('Time / (10ms)'); 
ylabel('Angel/ (degree)'); 
grid on; 
 
figure 
plot(tm,velocity) 
legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z'); 
title('Velocity Calculation'); 
xlabel('Time / (10ms)'); 
ylabel('Velocity/ (m/s)'); 
grid on; 
 
 
figure  
plot(tm,position); 
legend('Navigation Coordinate: X','Navigation Coordinate: Y','Navigation Coordinate: Z'); 
title('Position Calculation'); 
xlabel('Time / (10ms)'); 
ylabel('Position/ (m)'); 
grid on; 
figure 
 plot3(position(1,:),position(2,:),position(3,:)); 
 grid on 
  %roll:横滚角 x轴 
  %pitch:俯仰解: y axis 
  %yaw偏航角   z axis