sins－navigation.m

基于Matlab平台开发的的针对捷联式惯性到导航的仿真

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