shoes/liangjianbo/matlab/串口数据显示及读取/footPDR.m

function pos_result = footPDR(index, gyr, acc, press)
global C;
global vel_n;
global pos_n;
global P;
global H;
global R;
global sigma_omega;
global sigma_a;
global sigma_v;
global press_data;
global gyr_norm;
global ZUPT_STATUS;
global stand_num;
global last_position;
global last_yaw;
global last_zupt;
global gyr_data;

global g;

global gyrBias;

global can_output;
global stay;

global stay_status;

global acc_buff;

global acc_size;

global acc_data;

if(index  == 1)
    sigma_v = 0.01;
    sigma_a = 0.01;
    sigma_omega = 0.01;
    g = 9.80665; 
    pitch = asin(-acc(1)/g);

    roll = atan2(acc(2), acc(3));

    yaw = 0;
    
   C= [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);];
      
   H = [zeros(3,3), zeros(3,3), eye(3);];
   R = diag([sigma_v*0.1 sigma_v*0.1 sigma_v*0.1]).^2;
   P = zeros(9,9);
   vel_n = [0;0;0];
   pos_n = [0;0;0];
   pos_result = [0;0;0;0;0];
   ZUPT_STATUS = 0;
   
   press_data = [press];
   gyr_norm = [norm(gyr)];
   
   stand_num  = 0;
   last_position = [0;0;0;];
   last_yaw = 0;
   
   last_zupt = 0;
   
   gyr_data = [gyr];
   gyrBias = [0;0;0];
   acc_buff = [norm(acc)];
   acc_data = [acc];
   
   can_output = 0;
   stay_status = 0;
   
   acc_size = 1;
else
    
    gyr_norm = [gyr_norm, norm([gyr(1);gyr(3)])];
    acc_buff = [acc_buff norm(acc)];
    press_data = [press_data, press];
    gyr_data = [gyr_data gyr];
    w = gyr;
    if(index>500)
        if(max(gyr_data(1,index-500:index)) - min(gyr_data(1,index-500:index)) < 0.04 && max(gyr_data(2,index-500:index)) - min(gyr_data(2,index-500:index)) < 0.04 && max(gyr_data(3,index-500:index)) - min(gyr_data(3,index-500:index)) < 0.04 )
            gyrBias = mean(gyr_data(:,index-500:index),2);
            acc_size = 9.80665/mean(acc_buff(:,index-500:index));
            
        end
    end
    
    w = w - gyrBias;
    
    acc = acc.*acc_size;
    
    dt = 1/100;
    %当前时刻陀螺仪生成的叉乘矩阵，姿态矩阵迭代需要
    
    temp = [0, -w(3), w(2);
        w(3), 0, -w(1);
        -w(2), w(1), 0;]* dt;
    
    C= C * (2*eye(3)+temp)/(2*eye(3) - temp);
    
    acc_n = C * acc;
    acc_data = [acc_data acc_n-[0;0;g   ]];
    
    vel_n = vel_n + (acc_n - [0;0;g])*dt;
    
    pos_n = pos_n + vel_n* dt;
    
    S = [0, -acc_n(3), acc_n(2);
        acc_n(3), 0, -acc_n(1);
        -acc_n(2), acc_n(1), 0;];
    
    F = [eye(3), zeros(3,3), zeros(3,3);
        zeros(3,3), eye(3), dt*eye(3);
        -dt*S,      zeros(3,3), eye(3)];
    
    Q = diag([sigma_omega sigma_omega sigma_omega sigma_omega sigma_omega sigma_omega sigma_a sigma_a sigma_a]*dt).^2;
    
    P = F*P*F' + Q;
    
    if(index > 10 )
        max_mum = max(gyr_norm(index-9:index));
%         min_mum = min(gyr_norm(index-9:index));
    end
    
    if(index> 6 && press_data(index) - min(press_data(index-6:index-1)) > 100000)
        ZUPT_STATUS = 1;
        
    elseif(index> 6 && press_data(index) - max(press_data(index-6:index-1)) < -100000 )
        
        ZUPT_STATUS = 2;
        
    end
    
    stand_zupt = 0;
    if(((gyr_norm(index)< 0.4)  && (index > 15) && ((max_mum - gyr_norm(index)) < 0.35)))
        stand_zupt = 1;
    end
    
    run_zupt = 0;
    if((index> 10 && ZUPT_STATUS == 1 && norm([gyr(1);gyr(3)]) < 0.5))
        run_zupt = 1;
    end
    
    if( run_zupt== 1 || stand_zupt == 1)
%      if((((gyr_norm(index)< 0.4)  && (index > 15) && ((max_mum - gyr_norm(index)) < 0.35))))
     if(var(gyr_norm(index-4:index),1) <0.001 && var(press_data(index-4:index)./10000,1)>1.0)
         disp("下蹲");
     end
      zupt = 0;
      
      K = P*H'/(H*P*H' + R);
      
      delta_x = K * [vel_n;];
      
       P = P - K*H*P;
       
      
       delta_x(3) = atan2(C(2,1),C(1,1));

       C_fix = [0, -delta_x(3), delta_x(2);
                delta_x(3), 0, -delta_x(1);
                    -delta_x(2), delta_x(1), 0];
       C = ((2*eye(3)-C_fix)/(2*eye(3) + C_fix))* C;

       pos_n = pos_n - delta_x(4:6);
       vel_n = vel_n - delta_x(7:9);
       
       last_position = pos_n;
       
       last_yaw = atan2(C(2,1),C(1,1));
       
       last_zupt = 1;
       can_output  = 1;
       stay_status = stay_status+ 1;
    else
        zupt = 1;
        stay_status = 0;

%      delta_yaw = atan2(C(2,1), C(1,1)) - last_yaw;
%      
%      pos_result(2) = norm(pos_n(1:2))*sin(delta_yaw);
%      pos_result(1) = norm(pos_n(1:2))*cos(delta_yaw) - norm(last_position(1:2));

%      pos_result = pos_n - last_position;
    end
   
    pos_result(1:3) = pos_n(:) - last_position(:);
           
%     pos_result(1:3) = ((2*eye(3)-C_fix)/(2*eye(3) + C_fix))*pos_n(:) - ((2*eye(3)-C_fix)/(2*eye(3) + C_fix))*last_position(:);
    
%      pos_result(1:3) = pos_n(:) - last_position(:);
    
%      pos_result(1) = distance'*pos_n(1:2)/norm(pos_n(1:2));
% %      pos_result(2) = (distance(1)*pos_n(2) - distance(2)*pos_n(1))/norm(pos_n(1:2));
%      
     theta = 0.12;

     temp = [cos(theta), sin(theta);-sin(theta), cos(theta)];
     pos_temp = temp*[pos_result(1);pos_result(2)];
    
     pos_result(1) =pos_temp(1);
     pos_result(2) =pos_temp(2);
     
%      if index > 5
%          pos_result = mean(acc_data(:,index - 5 :index),2);
%      end
%      pos_result(1) = 0;
%      pos_result = acc_n - [0;0;g];
%         pos_result(1) = atan2(C(2,1),C(1,1));
%     
%         pos_result(2) = asin(-C(3,1));
%         pos_result(3) = atan2(C(3,2),C(3,3));
%      pos_result(2) = 2 * pos_result(2);
     if(can_output == 1 && norm(pos_result(1:2)) >0.1)
         if(pos_result(2) < -0.2)
             disp("右跳");
             can_output = 0;
         elseif(pos_result(2) > 0.2)
             disp("左跳")
             can_output = 0;
         elseif(pos_result(3) > 0.2)
             disp("跳高")
             can_output = 0;
         end
     end
     
    

%     distance = pos_n(1:2) - last_position(1:2);
%     pos_result(1) = last_position(1:2)' * distance(1:2)/norm(last_position(1:2));
%     pos_result(2) = (last_position(1)*distance(2)-distance(1)*last_position(2))/norm(last_position(1:2));
%      pos_result(3) = pos_n(3) - last_position(3);
     
%     pos_result(1:3)= (pos_result(1:3)+ 6) * 1000000;
    pos_result(4) = norm([gyr(1);gyr(3)]);
    pos_result(5) = var(gyr_norm(index-4:index),1)  ;
%     pos_result(3) = norm(gyr) * 1000000;
    P = (P+P')/2;
    

end


end