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

close all;
clear all;
%重力产生的加速度矢量
g=9.8;%单位9.8m/s^2
G=[00-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_x2）-1）;
%个数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个数的平均电压
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（3n_point）;
accelerate（11:100）=10;
accelerate（1101:200）=-10; 
accelerate（1201:300）=0;
%陀螺仪数据
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];
%转移矩阵--即方向余弦矩阵
T=eye（3）; %T是3*3的单位矩阵初始转移矩阵 
%四元数矩阵，存储每步更新之后的四元数，方便以后绘图
Q=zeros（4n_point）;
%四元数的初始值确定，假定一开始导航坐标系与载体坐标系是重合的，因此方向余弦矩阵，是单位矩阵，利用它们之间的关系确定四元数的初始值。

    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））;
%参见捷联惯性导航技术31页3.64式 在旋转90度时不适用    
%Q（11）=0.5*sqrt（1+T（11）+T（22）+T（