close all;
clear;

%% 读取数据集
load ekinox_imu;
load ref

imu=ekinox_imu;
to = (ref.t(end) - ref.t(1));
fprintf('运行总时间为 %.2f minutes or %.2f seconds. \n', (to/60), to)

LI=length(imu.t)

%% 初始化 %%
gravity = 9.8;
R0=diag(ones(3,1));

S=1; %%起始时刻
N=10000; %%结束时刻
trac = zeros(9, N);

%% 初始化位姿，因为数据集中传感器的初始速度和初始姿态不为0，所以需要
%% 初始化位姿，使得位姿同数据集中传感器位姿的起点相同。
%% 把初始姿态设置成同数据集中传感器初始姿态相同
init_roll=imu.roll(S);
init_pitch=imu.pitch(S);
init_yaw=imu.yaw(S);

Rx=[1 0 0; 0 cos(init_roll) -sin(init_roll); 0 sin(init_roll) cos(init_roll)];

Ry=[cos(init_pitch) 0 sin(init_pitch); 0 1 0; -sin(init_pitch) 0  cos(init_pitch)];

Rz=[cos(init_yaw) -sin(init_yaw) 0; sin(init_yaw) cos(init_yaw)  0; 0  0  1];

Rt=R0*Rz*Ry*Rx;

%% 把初始速度设置成数据集中传感器的初始速度相同
xt=[0,0,0,ref.vel(S,1),ref.vel(S,2),ref.vel(S,3)]';

%% 对四元数进行初始化
qt=DCM2Quat(Rt);


%% 设置零速检测的参数Zero velocity detection
W=20;
gamma_a=0.3;
gamma_w=0.2;


%% 惯性导航计算过程
for i=S:1:N
       
    wt= imu.wb(i,:)';
    at=  imu.fb(i,:)';
    dt =0.005;

   method='eular';  %使用欧拉角公式更新
   method='R1';      %使用罗德里格斯公式更新
   method='R2';
   method='exp';   
   method='quaternion';
  
    
    %% 使用四元数
    [xt, Rt, qt]=state_update(xt, Rt, qt, wt, dt, at, gravity, method);
    
    trac(1:6,i)=xt;
    trac(7:9,i)=dcm2euler(Rt);
    if(i>W)
        at_window= imu.fb(i-W+1:i,:)'+[0,0,gravity]';
        wt_window=imu.wb(i-W+1:i, :)';
        
        [Ta(i), Tw(i), accMag(i), gyroMag(i), zupt(i)]=online_zero_velocity_detector(W, at_window, wt_window, gamma_a, gamma_w);
    end
    
end
figure;
%% 绘制roll, pitch, yaw姿态追踪的误差
subplot(3,1,1);
title('roll');
plot(S:N, trac(7,S:N),'r-', S:N, imu.roll(S:N),'b-');
legend('estimated', 'groundtruth');

subplot(3,1,2);
title('pitch');
plot(S:N, trac(8,S:N),'r-', S:N, imu.pitch(S:N),'b-');
legend('estimated', 'groundtruth');

subplot(3,1,3);
title('yaw');
plot(S:N, trac(9,S:N),'r-', S:N, imu.yaw(S:N),'b-');
legend('estimated', 'groundtruth');
hold off;

%% 绘制velocity追踪的误差
figure;
subplot(3,1,1);
title('vx');
plot(S:N, trac(4,S:N),'r-');
hold on;
plot(S:200:N, ref.vel(S:N/200,1),'ko');
legend('estimated', 'groundtruth');

subplot(3,1,2);
title('vy');
plot(S:N, trac(5,S:N),'r-');
hold on;
plot(S:200:N, ref.vel(S:N/200,2),'ko');
legend('estimated', 'groundtruth');

subplot(3,1,3);
title('vz');
plot(S:N, trac(6,S:N),'r-');
hold on;
plot(S:200:N, ref.vel(S:N/200,3),'ko');
legend('estimated', 'groundtruth');
hold off;

%% 绘制acc
figure;
subplot(5,1,1);
title('accx');
plot(S:N, imu.fb(S:N,1),'r-');
legend('accx');

subplot(5,1,2);
title('accy');
plot(S:N, imu.fb(S:N,2),'b-');
legend('accy');

subplot(5,1,3);
title('accz');
plot(S:N, imu.fb(S:N,3),'g-');
legend('accz');

subplot(5,1,4);
title('zupt');
plot(S:N, accMag(S:N),'k-','linewidth',4);
axis([S,N,0,10]);
legend('zupt');

subplot(5,1,5);
title('zupt');
plot(S:N, gyroMag(S:N),'k-','linewidth',4);
axis([S,N,0,0.1]);
legend('zupt');
hold off;

%% 绘制gyro 
figure;
subplot(6,1,1);
title('gyrox');
plot(S:N, imu.wb(S:N,1),'r-');
legend('gyrox');

subplot(6,1,2);
title('gyroy');
plot(S:N, imu.wb(S:N,2),'b-');
legend('gyroy');


subplot(6,1,3);
title('gyroz');
plot(S:N, imu.wb(S:N,3),'g-');
legend('gyroz');

subplot(6,1,4);
title('zupt');
plot(S:N, Ta(S:N),'k-','linewidth',4);
axis([S,N,0,10]);
legend('zupt');

subplot(6,1,5);
title('zupt');
plot(S:N, Tw(S:N),'k-','linewidth',4);
axis([S,N,0,0.02]);
legend('zupt');

subplot(6,1,6);
title('zupt');
plot(S:N, zupt(S:N),'k-','linewidth',4);
axis([S,N,0,1]);
legend('zupt');
hold off;