clear ;%清除工作区的缓存变量
 close all; % 关闭当前的所有窗口
clc; %清除命令行的数据


g = 9.7833; 
%尝试读取csv文件

data_file = csvread('LoggedData2_CalInertialAndMag.csv',1 ,0);
%把陀螺仪数据，加速度数据拿出来
gyr_data = data_file(:, 2:4) *pi/180;
acc_data = data_file(:, 5:7) * g;
press_data = data_file(:,8:9)';

%参数设置

data_size = size(gyr_data,1);
 
dt = 1/100;

%当地重力加速度

%设置过程噪声
% 状态量为【姿态误差，速度误差，位置误差】
% 相应的过程噪声协方差
sigma_omega = 1e-2; sigma_a = 1e-2;
% 观测噪声协方差,只观测速度误差
sigma_v = 1e-2;
R = diag([sigma_v sigma_v sigma_v*0.1]).^2;
R_xy = diag([sigma_v sigma_v sigma_v sigma_v sigma_v sigma_v]).^2;

%初始化姿态矩阵
%先根据加速度计算俯仰角，翻滚角，航向角

pitch = asin(-acc_data(1,1)/g);

roll = atan2(acc_data(1,2), acc_data(1,3));

yaw = 0;

C_prev = [cos(yaw)*cos(pitch), -sin(yaw)*cos(roll)+cos(yaw)*sin(pitch)*sin(roll), sin(yaw)*sin(roll)+ cos(yaw)*sin(pitch)*cos(roll);
          sin(yaw)*cos(pitch), cos(yaw)*cos(roll)+sin(yaw)*sin(pitch)*sin(roll), -cos(yaw)*sin(roll)+sin(yaw)*sin(pitch)* cos(roll);
          -sin(pitch), cos(pitch)* sin(roll), cos(pitch)*cos(roll);];
C = C_prev;
acc_n = zeros(3, data_size);
acc_n(:,1) = C*acc_data(1,:)'-[0,0,g]';
vel_n = zeros(3, data_size);
vel_n_norm = zeros(1,data_size);
pos_n = zeros(3, data_size);

P = zeros(9,9);

H = [zeros(3,3), zeros(3,3), eye(3);
 ];

H_xy= [zeros(3,3), zeros(3,3), eye(3);
    zeros(3,3),eye(3),zeros(3,3);
    ];

gyr_norm = nan(1,data_size);
acc_norm = nan(1,data_size);
zupt = zeros(1,data_size);

pitch_data = nan(1,data_size);
roll_data = nan(1, data_size);
yaw_data = nan(1, data_size);

C_buff = zeros(3,3,data_size);

C_buff(:,:,1) = C;


last_zupt = 0;
mini_update = 0;
step_pos = zeros(3,1);


last_step_xy = [0;0];
last_step_xy_tmp = [0;0];


pos_result = zeros(3,data_size);

last_pos = [0,0,0]';

last_zupt_index = 30;

%以下为记录输出所用
last_record = 0;

output_signal = 0;

output_return = 0;

stand_num = 0;

vel_local = zeros(2, data_size);

last_running  = 1;

output_index = 1;

maximum_window = [] ;
window_size = 1;

maximum_point = zeros(1,data_size);

last_maximum_point = -1;

for i = 3 : data_size
    %姿态矩阵更新, 其实有没有陀螺仪零片都没多大问题
    % w= gyrData - gyrBias
    w = gyr_data(i,:)';
    
    %陀螺仪K时刻求模，分析数据时候用
    gyr_norm(i) = norm(w);
    
    if(gyr_norm(i-2) < gyr_norm(i-1) && gyr_norm(i-1) > gyr_norm(i) && gyr_norm(i-1) > 1.5)
%         zupt(i-1) = 1.0;
        if(window_size == 1)
            maximum_window(window_size) = i - 1;
            window_size = window_size + 1;
        end
        if(window_size > 1)
            if(i - maximum_window(window_size - 1) > 10)
                maximum_window(window_size) = i-1;
                window_size = window_size + 1;
                
            elseif(gyr_norm(maximum_window(window_size - 1)) < gyr_norm(i-1) )
                maximum_window(window_size-1) = i-1;
                
            end
        end
    
        if(window_size > 4)
            if(gyr_norm(maximum_window(window_size-1)) > (gyr_norm(maximum_window(window_size-2)) + 2)...
                && gyr_norm(maximum_window(window_size-1)) > (gyr_norm(maximum_window(window_size-3)) + 2))
                maximum_point(i-1) = 1;
                last_maximum_point = i-1;
            end
        end

        
    end
    
    if(i > 10 )
        max_mum = max(gyr_norm(i-9:i));
        min_mum = min(gyr_norm(i-9:i));
    end
    
    if((i > 10 && i - last_maximum_point < 10 && max_mum - min_mum > 4.0 && gyr_norm(i) < 1.5) || ...
        (((gyr_norm(i)< 0.6)  && (i > 15) && ((max_mum - gyr_norm(i)) < 0.6))))
        zupt(i) = 1.0;
        
    end
    

end
index = find(zupt > 0);

maximum_index = find(maximum_point > 0);
figure;
plot(gyr_norm, 'r')
grid on
% hold on
% plot(index, gyr_norm(index), 'k*')

hold on

plot(maximum_window(:), gyr_norm(maximum_window(:)), 'bo', 'LineWidth',3)