close?all;
clear?all;
%重力產(chǎn)生的加速度矢量
g=9.8;%單位9.8m/s^2
G=[00-g]‘;
%****************************讀入數(shù)據(jù)
%**********讀入陀螺儀的數(shù)據(jù)
gyro_x=load（‘gyrox.txt‘）;
gyro_y=load（‘gyroy.txt‘）;
gyro_z=load（‘gyroz.txt‘）;
%****************讀入加速度計的數(shù)據(jù)**************
%acc_rate=3/1024;
acc_x?=load（‘a(chǎn)cceleratex.txt‘）;
acc_y?=load（‘a(chǎn)cceleratey.txt‘）;
acc_z=load（‘a(chǎn)cceleratez.txt‘）;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%加速度數(shù)字轉(zhuǎn)換為模擬電壓
data_acc=[acc_x;acc_y;acc_z];
data_acc=data_acc/1024*3
%將數(shù)據(jù)轉(zhuǎn)換為相應(yīng)的加速度值
%
%*********************************************************



%加速度計三個軸向的零點電壓
%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_x2）-1）;
%個數(shù)num
n_point=size（gyro_x2）;
%圖1
figure
plot（tmdata_acc（1:）‘-‘tmdata_acc（2:）‘.‘tmdata_acc（3:）‘-.‘）;
title（‘加速度計的采樣曲線‘）;
legend（‘x_ACC‘‘Y_ACC‘‘Z_ACC‘）;
xlabel（‘Time?/?（10ms）‘）;
ylabel（‘Accelerate/?（m/s‘‘）‘）;
grid?on;
%plot（tmacc_x‘-‘tmacc_y‘.‘tmacc_z‘-.‘）;
%title（‘加速度的計的采樣曲線‘）:
%對采樣曲線進行低通濾波
a=[12421];
%gyro_x=filter（a/sum（a）1gyro_x）;
%gyro_y=filter（a/sum（a）1gyro_y）;
%gyro_z=filter（a/sum（a）1gyro_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個數(shù)的平均電壓
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;
%測試數(shù)據(jù)
accelerate=zeros（3n_point）;
accelerate（11:100）=10;
accelerate（1101:200）=-10;?
accelerate（1201:300）=0;
%陀螺儀數(shù)據(jù)
gyro_x=zeros（1n_point）;
gyro_y=zeros（1n_point）;
gyro_z=zeros（1n_point）;
gyro_z（1:100）=pi/3;
gyro_z（101:200）=-pi/3;

%重力軸始終有加速度
accelerate（3:）=accelerate（3:）+9.8;
figure
plot（tmaccelerate（1:）‘-‘tmaccelerate（2:）‘.‘tmaccelerate（3:）‘-.‘）;
title（‘加速度計的采樣曲線‘）;
legend（‘x_ACC‘‘Y_ACC‘‘Z_ACC‘）;
xlabel（‘Time?/?（10ms）‘）;
ylabel（‘Accelerate/?（m/s‘‘）‘）;
grid?on;


%畫出陀螺儀的采樣曲線
figure
plot（tmgyro_x‘r-‘tmgyro_y‘g.‘tmgyro_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];
%轉(zhuǎn)移矩陣--即方向余弦矩陣
T=eye（3）;?%T是3*3的單位矩陣初始轉(zhuǎn)移矩陣?
%四元數(shù)矩陣，存儲每步更新之后的四元數(shù)，方便以后繪圖
Q=zeros（4n_point）;
%四元數(shù)的初始值確定，假定一開始導(dǎo)航坐標系與載體坐標系是重合的，因此方向余弦矩陣，是單位矩陣，利用它們之間的關(guān)系確定四元數(shù)的初始值。

????Q（11）=0.5*sqrt（1+T（11）+T（22）+T（33））;
????Q（21）=0.5*sqrt（1+T（11）-T（22）-T（33））;
????Q（31）=0.5*sqrt（1-T（11）+T（22）-T（33））;
????Q（41）=0.5*sqrt（1-T（11）-T（22）+T（33））;
%參見捷聯(lián)慣性導(dǎo)航技術(shù)31頁3.64式?在旋轉(zhuǎn)90度時不適用????
%Q（11）=0.5*sqrt（1+T（11）+T（22）+T（