%% %%%%% 
% @brief：惯性测量单元数据处理
% @date：
% @Copyright：
% @Author:
%% 系统初始化
clear all;
clc;
close all;
%% 数据导入
% Origin_Data=importdata('Imu_Data.dat');
% Data=Origin_Data.data(1:end,:);
load data1
Data=Data0901
[Row,Line]=size(Data);   %%判断数据维数
%% 变量初始化
dt=0.005;  %%积分时间
Time=Data(:,1)*0.005;   %系统时间
Gyro=Data(:,2:4)*pi/180; %陀螺仪数据
Acc=Data(:,5:7);  %加速度计数据
Att=zeros(Row,3);
%% 初始对准
f_x=mean(Acc(1:5,1));
f_y=mean(Acc(1:5,2));
f_z=mean(Acc(1:5,3));

roll=atan2(-f_y,-f_z);
pitch=asin(f_x/sqrt(f_x^2+f_y^2+f_z^2));
yaw=50;
Att(1,1:3)=[yaw,pitch,roll]*180/pi;
attitude=[roll pitch 0]';
Rb2t=Rt2b(attitude)';
q=dcm2q(Rb2t);

L_G=[Time(1500) Time(1500);-180 100];
%% 数据处理
for i=1:1:Row
    if i>1
       %dt=(Data(i,1)-Data(i-1,1))*0.001;
    end
    dt=0.005;
    w_tb=Gyro(i,1:3);

    P=w_tb(1)*dt;   %一阶比卡算法
    Q=w_tb(2)*dt;
    R=w_tb(3)*dt;

    OMEGA=zeros(4);
    OMEGA(1,1:4)=0.5*[0 R -Q P];
    OMEGA(2,1:4)=0.5*[-R 0 P Q];
    OMEGA(3,1:4)=0.5*[Q -P 0 R];
    OMEGA(4,1:4)=0.5*[-P -Q -R 0];

    v=norm(w_tb)*dt;

    if v~=0
        q=(cos(v/2)*eye(4)+2/v*sin(v/2)*OMEGA )*q;
        q=q./norm(q);
    end
    
    Rb2t=q2dcm(q);
    % roll
    roll=atan2(Rb2t(3,2),Rb2t(3,3));

    % pitch
    pitch=-atan(Rb2t(3,1)/sqrt(1-Rb2t(3,1)^2));

    %yaw
    yaw=atan2(Rb2t(2,1),Rb2t(1,1));   
    
    Att(i,1:3)=[yaw,pitch,roll]*180/pi;

end
%% 数据存储
%% 显示输出
figure(1)
plot(Time,Att(:,1),'r','LineWidth',2);
hold on;
plot(Time,Att(:,2),'g','LineWidth',1.5);
plot(Time,Att(:,3),'b','LineWidth',1.5);
legend('偏航角','俯仰角','滚转角');
% plot(L_G(1,:),L_G(2,:),'k','LineWidth',1.5);