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