read and analyse the imu data from shoe

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

@@ -0,0 +1,247 @@
+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

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

@@ -0,0 +1,14 @@
+function pos_pca = pca_filter(input)
+    
+    meanXY =  mean(input,2); 
+    col_size = size(input,2);
+    
+%     input = input - [meanXY(1) * ones(1,col_size);meanXY(2) * ones(1,col_size)];
+    
+    C = input*input'./col_size;
+    
+    [V D] = eig(C);
+    disp(V)
+    disp(D)
+end
+

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

@@ -0,0 +1,29 @@
+clear ;%清除工作区的缓存变量
+close all; % 关闭当前的所有窗口
+clc; %清除命令行的数据
+
+data_file = xlsread('../imusavedata_zhengxuelong3.xlsx');
+
+gyr_data = data_file(:, 1:3) * pi/180;
+
+g = 9.7833; 
+acc_data = data_file(:, 4:6) /1.2098 * g;
+
+press_data = data_file(:,7:8)';
+
+data_size = size(gyr_data,1);
+
+pos_result =zeros(3,data_size);
+
+for i = 1 : data_size
+    pos = footPDR(i, gyr_data(i,:)', acc_data(i,:)',press_data(2,i));
+    
+    pos_result(:,i) = pos;
+end
+
+figure('NumberTitle', 'off', 'Name', 'Offset pos');
+hold on;
+plot(pos_result(1,:), 'r');
+plot(pos_result(2,:), 'g');
+plot(pos_result(3,:), 'b');
+grid on

liangjianbo/matlab/串口数据显示及读取/test_DOWN.asv

@@ -0,0 +1,67 @@
+clear all;
+close all;
+load imu_data1;
+
+gyr_data = imu_data(1:3,:);
+press_raw_data = imu_data(4,:);
+press_data = (imu_data(4,:)-9000000*ones(1,size(gyr_data,2)))*0.0000005;
+last_move_index = 0;
+zupt = zeros(1,size(gyr_data,2));
+press_zupt = 0;
+mutex_up = 0;
+mutex_down =0;
+press_index = 0;
+down_pass1 = 0;
+down_pass2 = 0;
+
+for i = 1:size(gyr_data,2)
+    
+    if(norm(gyr_data(:,i)) > 1.0)
+        last_move_index = i;
+    end
+    
+    if(i> 6 && press_raw_data(1, i) - min(press_raw_data(1, i-6:i-1)) > 100000 )
+        press_zupt = 1;
+        zupt(i) = 1;
+        press_index = i;
+        if()
+        if(abs(gyr_data(2,i)) > 1.5)
+            down_pass1 = 1;
+        end
+    elseif(i> 6 && press_raw_data(1, i) - max(press_raw_data(1, i-6:i-1)) < -100000)
+        zupt(i) = 2;
+        if(abs(gyr_data(2,i)) > 1.5)
+          down_pass2 = 1;
+        end
+    elseif(i > 1 && zupt(i-1)== 2 )
+        if(down_pass1 == 0 && down_pass2 == 0 )
+            zupt(i) = 3;
+        end
+        down_pass1 = 2 ;
+        down_pass2 = 2 ;
+    end
+    
+    
+end
+
+index = find(zupt ==1);
+
+figure;
+hold on
+plot(gyr_data(1,:),'r-')
+plot(gyr_data(2,:),'g-')
+plot(gyr_data(3,:),'b-')
+plot(press_data(1,:),'m-')
+plot(index,press_data(1,index),'ko')
+index = find(zupt == 2);
+plot(index,press_data(1,index),'ro')
+index = find(zupt == 3);
+plot(index,press_data(1,index),'k*')
+grid on
+axis on
+
+figure
+hold on
+plot(press_data(1,:),'r-')
+grid on
+axis on

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

@@ -0,0 +1,72 @@
+clear all;
+close all;
+load imu_data1;
+
+gyr_data = imu_data(1:3,:);
+press_raw_data = imu_data(4,:);
+press_data = (imu_data(4,:)-9000000*ones(1,size(gyr_data,2)))*0.0000005;
+last_move_index = 0;
+zupt = zeros(1,size(gyr_data,2));
+press_zupt = 0;
+mutex_up = 0;
+mutex_down =0;
+press_index = 0;
+down_pass1 = 0;
+down_pass2 = 0;
+
+for i = 1:size(gyr_data,2)
+    
+    if(norm(gyr_data(:,i)) > 1.0)
+        last_move_index = i;
+    end
+    
+    if(i> 6 && press_raw_data(1, i) - min(press_raw_data(1, i-6:i-1)) > 100000 )
+        press_zupt = 1;
+        zupt(i) = 1;
+        press_index = i;
+        if(down_pass1==2)
+            down_pass1 = 0;
+        end
+        if(abs(gyr_data(2,i)) > 1.5)
+            down_pass1 = 1;
+        end
+    elseif(i> 6 && press_raw_data(1, i) - max(press_raw_data(1, i-6:i-1)) < -100000)
+        zupt(i) = 2;
+        if(down_pass2==2)
+            down_pass2 = 0;
+        end
+        if(abs(gyr_data(2,i)) > 1.5)
+          down_pass2 = 1;
+        end
+    elseif(i > 1 && zupt(i-1)== 2 )
+        if(down_pass1 == 0 && down_pass2 == 0 )
+            zupt(i) = 3;
+        end
+        down_pass1 = 2 ;
+        down_pass2 = 2 ;
+    end
+    
+    
+end
+
+index = find(zupt ==1);
+
+figure;
+hold on
+plot(gyr_data(1,:),'r-')
+plot(gyr_data(2,:),'g-')
+plot(gyr_data(3,:),'b-')
+plot(press_data(1,:),'m-')
+plot(index,press_data(1,index),'ko')
+index = find(zupt == 2);
+plot(index,press_data(1,index),'ro')
+index = find(zupt == 3);
+plot(index,press_data(1,index),'k*')
+grid on
+axis on
+
+figure
+hold on
+plot(press_data(1,:),'r-')
+grid on
+axis on

liangjianbo/matlab/串口数据显示及读取/uart.asv

@@ -0,0 +1,171 @@
+clc
+close all;
+clear all;
+
+dt = 1;
+t=[0];
+% m=[0;0;0;0]
+m=[0;0;0;0;0;0;0]
+p = plot(t,m)
+
+data = [];
+buf = [];
+
+x=0;
+legend('x','y','z','压力')
+grid on;
+set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);
+
+delete(instrfindall('Type','serial'));%清理串口
+object = serial('com3','BaudRate',115200);%配置串口
+fopen(object);
+
+state = 0;
+Len = 0;
+Lenf = 0;
+
+Zarr = [];
+Zarr(1) = 0;
+Zarr_en  = 0;
+run = 0;
+left = 0;
+right = 0;
+acc3_b = 0;
+acc3_bb = 0;
+press_data = [];
+press_var = [];
+gyr_var = [];
+pos_data = [];
+
+imu_data = [];
+% for i=1:shuliang
+while true
+    data=fread(object);%读取数据
+    while ~isempty(data)
+        buf = [buf data(1)];
+        data(1) = [];
+        switch state
+            case 0
+               if length(buf)>=5
+                   if  buf(1)==170 && buf(2)==187 && buf(3)==204
+                       Len = buf(4);
+                       Lenf = 255 - buf(5);
+                       if Len==Lenf
+                          state = 1;
+                       else
+                           buf(1) = [];
+                       end
+                   else
+                       buf(1) = [];
+                   end
+               end
+            case 1
+                if length(buf)>=Len
+                  ver = 0;
+                   for i=1:(Len-1)
+                       ver = ver + buf(i);
+                   end
+                   ver = mod(ver,256);
+    %                disp(ver);
+    %                disp(buf(Len));
+                   if ver==buf(Len)
+                       if buf(6)==0 && buf(7)==0
+%                             acc(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11)));
+%                             acc(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15)));
+%                             acc(3)=double(bitshift(int32(buf(16)),24)+bitshift(int32(buf(17)),16)+bitshift(int32(buf(18)),8)+int32(buf(19)));
+%                             acc(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
+%                             acc(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
+%                             acc(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
+%                             disp(acc);
+                            
+                            gry(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
+                            gry(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
+                            gry(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
+%                             gyr_norm = [gyr_norm norm([gry(1),gry(2),gry(3)])./16.384*pi/180];
+                           
+                            acc(1)=double(bitshift(int16(buf(14)),8)+int16(buf(15)));
+                            acc(2)=double(bitshift(int16(buf(16)),8)+int16(buf(17)));
+                            acc(3)=double(bitshift(int16(buf(18)),8)+int16(buf(19)));
+%                             disp(gry);
+                            
+                             press(1)=double(bitshift(int32(buf(20)),24)+bitshift(int32(buf(21)),16)+bitshift(int32(buf(22)),8)+int32(buf(23)));
+                             press_data = [press_data press(1)];
+%                             disp(press(1));
+                            pos_res(1) = double(bitshift(int16(buf(24)),8)+int16(buf(25)));
+                            pos_res(2) = double(bitshift(int16(buf(26)),8)+int16(buf(27)));
+                            pos_res(3) = double(bitshift(int16(buf(28)),8)+int16(buf(29)));
+                            pos_data = [pos_data [pos_res(1)/10000;pos_res(2)/10000]];
+                            
+                            
+                            signal_out = int16(buf(30));
+                            if(signal_out == 2)
+                                 disp("MOTION_JUMP")
+                            elseif(signal_out == 3)
+                                disp("MOTION_DOWN")
+                                max(press_data(end-6:end)) - min(press_data(end-6:end))
+                            elseif(signal_out == 4)
+                                disp("MOTION_LEFT")
+                               
+                                
+                            elseif(signal_out == 5)
+                                disp("MOTION_RIGHT")
+                              
+                            end
+                            
+                            if(norm(pos_data(1:2,end)) > 0.3)
+%                                 pca_filter(pos_data(:,end-9:end));
+                            end
+                            
+%                              T = [-0.7949   -0.6067;0.6067   -0.7949];
+                            T = [1 0; 0 1];
+                            pos_data(1:2,end) = T * [pos_res(1)/10000;pos_res(2)/10000];
+                             
+%                             adc(1)=double(bitshift(int16(buf(20)),8)+int16(buf(21)));
+%                             adc(2)=double(bitshift(int16(buf(22)),8)+int16(buf(23)));
+                            
+                            
+%                             signal_out(1)=double(bitshift(int32(buf(24)),24)+bitshift(int32(buf(25)),16)+bitshift(int32(buf(26)),8)+int32(buf(27)));
+%                             signal_out(2)=double(bitshift(int32(buf(28)),24)+bitshift(int32(buf(29)),16)+bitshift(int32(buf(30)),8)+int32(buf(31)));
+%                             signal_out(3)=double(bitshift(int32(buf(32)),24)+bitshift(int32(buf(33)),16)+bitshift(int32(buf(34)),8)+int32(buf(35)));
+%                              norm([acc(1);acc(2);acc(3);]./4096/1.1992*9.88)
+%                             datestr(now,'mmmm dd,yyyy HH:MM:SS.FFF AM')
+%                              pos = footPDR(dt, [gry(1);gry(2);gry(3)]./16.384*pi/180, [acc(1);acc(2);acc(3);]./4096/1.1992*9.7833, press(1));
+                            
+                         
+%                              disp(press(1));
+                            
+                            dt = dt + 1;
+                            t=[t dt];
+                            if dt>250 
+                                x=x+1; 
+                            end
+                            imu_data = [imu_data [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;press(1);]];
+                            m=[m [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;(press(1)-9000000)*0.0000005;pos_data(1,end)+2;pos_data(2,end)-2;pos_res(3)/10000]];
+%                                  set(p(1),'XData',t,'YData',m(1,:))
+%                                   set(p(2),'XData',t,'YData',m(2,:))
+%                                       set(p(3),'XData',t,'YData',m(3,:))
+%                                             set(p(4),'XData',t,'YData',m(4,:))
+                                       set(p(1),'XData',t,'YData',m(5,:))
+                                        set(p(2),'XData',t,'YData',m(6,:))
+%                                    set(p(3),'XData',t,'YData',m(7,:))
+%                                  set(p(8),'XData',t,'YData',m(8,:))
+%                                 set(p(9),'XData',t,'YData',m(9,:))
+%                                set(p(10),'XData',t,'YData',m(10,:))
+                       end
+                        buf = buf(Len:end);
+                   else
+                       buf(1) = [];
+                   end
+                   state = 0;
+                end
+            otherwise
+                state = 0;
+        end
+    end
+    drawnow
+	axis([x x+300 -10 10]); 
+end
+
+fclose(object);
+delete(object);
+clear object;

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

@@ -0,0 +1,174 @@
+clc
+close all;
+clear all;
+
+dt = 1;
+t=[0];
+% m=[0;0;0;0]
+m=[0;0;0;0;0;0;0]
+p = plot(t,m)
+
+data = [];
+buf = [];
+
+x=0;
+legend('x','y','z','压力')
+grid on;
+set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);
+
+delete(instrfindall('Type','serial'));%清理串口
+object = serial('com3','BaudRate',115200);%配置串口
+fopen(object);
+
+state = 0;
+Len = 0;
+Lenf = 0;
+
+Zarr = [];
+Zarr(1) = 0;
+Zarr_en  = 0;
+run = 0;
+left = 0;
+right = 0;
+acc3_b = 0;
+acc3_bb = 0;
+press_data = [];
+press_var = [];
+gyr_var = [];
+pos_data = [];
+
+imu_data = [];
+% for i=1:shuliang
+while true
+    data=fread(object);%读取数据
+    while ~isempty(data)
+        buf = [buf data(1)];
+        data(1) = [];
+        switch state
+            case 0
+               if length(buf)>=5
+                   if  buf(1)==170 && buf(2)==187 && buf(3)==204
+                       Len = buf(4);
+                       Lenf = 255 - buf(5);
+                       if Len==Lenf
+                          state = 1;
+                       else
+                           buf(1) = [];
+                       end
+                   else
+                       buf(1) = [];
+                   end
+               end
+            case 1
+                if length(buf)>=Len
+                  ver = 0;
+                   for i=1:(Len-1)
+                       ver = ver + buf(i);
+                   end
+                   ver = mod(ver,256);
+    %                disp(ver);
+    %                disp(buf(Len));
+                   if ver==buf(Len)
+                       if buf(6)==0 && buf(7)==0
+%                             acc(1)=double(bitshift(int32(buf(8)),24)+bitshift(int32(buf(9)),16)+bitshift(int32(buf(10)),8)+int32(buf(11)));
+%                             acc(2)=double(bitshift(int32(buf(12)),24)+bitshift(int32(buf(13)),16)+bitshift(int32(buf(14)),8)+int32(buf(15)));
+%                             acc(3)=double(bitshift(int32(buf(16)),24)+bitshift(int32(buf(17)),16)+bitshift(int32(buf(18)),8)+int32(buf(19)));
+%                             acc(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
+%                             acc(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
+%                             acc(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
+%                             disp(acc);
+                            
+                            gry(1)=double(bitshift(int16(buf(8)),8)+int16(buf(9)));
+                            gry(2)=double(bitshift(int16(buf(10)),8)+int16(buf(11)));
+                            gry(3)=double(bitshift(int16(buf(12)),8)+int16(buf(13)));
+%                             gyr_norm = [gyr_norm norm([gry(1),gry(2),gry(3)])./16.384*pi/180];
+                           
+                            acc(1)=double(bitshift(int16(buf(14)),8)+int16(buf(15)));
+                            acc(2)=double(bitshift(int16(buf(16)),8)+int16(buf(17)));
+                            acc(3)=double(bitshift(int16(buf(18)),8)+int16(buf(19)));
+%                             disp(gry);
+                            
+                             press(1)=double(bitshift(int32(buf(20)),24)+bitshift(int32(buf(21)),16)+bitshift(int32(buf(22)),8)+int32(buf(23)));
+                             press_data = [press_data press(1)];
+%                             disp(press(1));
+                            pos_res(1) = double(bitshift(int16(buf(24)),8)+int16(buf(25)));
+                            pos_res(2) = double(bitshift(int16(buf(26)),8)+int16(buf(27)));
+                            pos_res(3) = double(bitshift(int16(buf(28)),8)+int16(buf(29)));
+
+                            
+                            
+                            signal_out = int16(buf(30));
+                            if(signal_out == 2)
+                                 disp("MOTION_JUMP")
+                            elseif(signal_out == 3)
+                                disp("MOTION_DOWN")
+                                max(press_data(end-6:end)) - min(press_data(end-6:end))
+                            elseif(signal_out == 4)
+                                disp("MOTION_LEFT")
+                               
+                                
+                            elseif(signal_out == 5)
+                                disp("MOTION_RIGHT")
+                              
+                            end
+                            
+%                             if(norm(pos_data(1:2,end)) > 0.3)
+% %                                 pca_filter(pos_data(:,end-9:end));
+%                             end
+                            
+%                              T = [-0.7949   -0.6067;0.6067   -0.7949];
+                            T = [1 0; 0 1];
+                             temp = T * [pos_res(1);pos_res(2)];
+                             pos_res(1) = temp(1);
+                             pos_res(2) = temp(2);
+                             
+                             
+%                             adc(1)=double(bitshift(int16(buf(20)),8)+int16(buf(21)));
+%                             adc(2)=double(bitshift(int16(buf(22)),8)+int16(buf(23)));
+                            
+                            
+%                             signal_out(1)=double(bitshift(int32(buf(24)),24)+bitshift(int32(buf(25)),16)+bitshift(int32(buf(26)),8)+int32(buf(27)));
+%                             signal_out(2)=double(bitshift(int32(buf(28)),24)+bitshift(int32(buf(29)),16)+bitshift(int32(buf(30)),8)+int32(buf(31)));
+%                             signal_out(3)=double(bitshift(int32(buf(32)),24)+bitshift(int32(buf(33)),16)+bitshift(int32(buf(34)),8)+int32(buf(35)));
+%                              norm([acc(1);acc(2);acc(3);]./4096/1.1992*9.88)
+%                             datestr(now,'mmmm dd,yyyy HH:MM:SS.FFF AM')
+%                              pos = footPDR(dt, [gry(1);gry(2);gry(3)]./16.384*pi/180, [acc(1);acc(2);acc(3);]./4096/1.1992*9.7833, press(1));
+                            
+                         
+%                              disp(press(1));
+                            
+                            dt = dt + 1;
+                            t=[t dt];
+                            if dt>250 
+                                x=x+1; 
+                            end
+                            imu_data = [imu_data [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;press(1);]];
+                            m=[m [gry(1)/16.384*pi/180;gry(2)/16.384*pi/180;gry(3)/16.384*pi/180;(press(1)-9000000)*0.0000005;pos_res(1)/100;pos_res(2)/100;pos_res(3)/100]];
+                                 set(p(1),'XData',t,'YData',m(1,:))
+                                  set(p(2),'XData',t,'YData',m(2,:))
+                                      set(p(3),'XData',t,'YData',m(3,:))
+%                                             set(p(4),'XData',t,'YData',m(4,:))
+%                                        set(p(1),'XData',t,'YData',m(5,:))
+%                                         set(p(2),'XData',t,'YData',m(6,:))
+%                                    set(p(3),'XData',t,'YData',m(7,:))
+%                                  set(p(8),'XData',t,'YData',m(8,:))
+%                                 set(p(9),'XData',t,'YData',m(9,:))
+%                                set(p(10),'XData',t,'YData',m(10,:))
+                       end
+                        buf = buf(Len:end);
+                   else
+                       buf(1) = [];
+                   end
+                   state = 0;
+                end
+            otherwise
+                state = 0;
+        end
+    end
+    drawnow
+	axis([x x+300 -2 2]); 
+end
+
+fclose(object);
+delete(object);
+clear object;