jianbo_lib/matlab/new_scipt.m

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


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

data_file = csvread('LoggedData3.csv',1 ,0);
%把陀螺仪数据，加速度数据拿出来
gyr_data = data_file(:, 6:8) *pi/180;
acc_data = data_file(:, 3:5) * g;
time_stamp = data_file(:, 4) * 0.001;
board_pos = data_file(:, 12:14)';

%参数设置

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);

has_zupt = 0;

zupt_pos.x = 0;
zupt_pos.y = 0;
zupt_pos.z = 0;
zupt_pos.yaw = 0;

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;

gyr_norm_flag = 10000;

output_time = [];
output_index = 1;

for i = 2 : data_size
    %姿态矩阵更新, 其实有没有陀螺仪零片都没多大问题
    % w= gyrData - gyrBias

    w = gyr_data(i,:)';
    
    %陀螺仪K时刻求模，分析数据时候用
    gyr_norm(i) = norm(w);
    
    %当前时刻陀螺仪生成的叉乘矩阵，姿态矩阵迭代需要
    temp = [0, -w(3), w(2);
        w(3), 0, -w(1);
        -w(2), w(1), 0;]* dt;
    
    % 姿态矩阵迭代的经典公示了，也可以用其他的迭代公式
    C= C_prev * (2*eye(3)+temp)/(2*eye(3) - temp);
    
    %速度位置初步更新
    acc = acc_data(i,:)';
    
    %加速度计，分析数据时候用
    acc_norm(i) = norm(acc);
    
    %acc_n为当地的坐标系下的加速度表示，并减去了自身的重力加速度计
    acc_n(:,i) = C * acc;
    
    %导航坐标系下的速度向量迭代公式
    vel_n(:,i) = vel_n(:, i-1) + (acc_n(:,i)- [0,0,g]')*dt;
    
    %导航坐标系下的位置更新公式
    pos_n(:,i) = pos_n(:,i-1) + vel_n(:,i) *dt ;
    
    
    %状态转移矩阵
    S = [0, -acc_n(3,i), acc_n(2,i);
        acc_n(3,i), 0, -acc_n(1,i);
        -acc_n(2,i), acc_n(1,i), 0;];
    %状态向量的顺序为[delta_pitch, delta_roll, delta_yaw,
    %                 delta_px, delta_py, delta_pz,]
    %                 delta_vx, delta_vy, delta_vz,]
    % xk = F*x(k-1);
    % 式子的意义 第一行代表 姿态角的误差预测没有改变
    %            第二行代表 当前的位置误差 = 上一个时刻位置误差 + 上一个时刻速度误差*时间
    %            第三行代表 当前的速度误差 = 上一个时刻速度误差 - 加速度X姿态角误差*时间
    %                       简单来说是由于姿态误差引起的加速度测量误差，再乘以时间，就得到额外的速度误差
    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_a sigma_a sigma_a sigma_a sigma_a sigma_a]*dt).^2;
    
    %卡尔曼里面的协方差矩阵迭代公式了
    P = F*P*F' + Q;

    
    % 这里说明一下, if(norm(w)< 1.0) 是来判断IMU触碰到地上用的
    % 一般走路 用不着达到1.0,0.35就可以了，但是剧烈运动是达到1.0的
    % 用固定阈值来判断不是恰当的，可以根据前面一段（t-k:k） 陀螺仪数据是预测
    % 或者根据一步间的IMU数据来衡量跑步还是普通走路
    % 这里为了方便测试，用固定值1.0来算好了
    if(i > 15)
        max_window = max(gyr_norm(i-14:i));
        min_window = min(gyr_norm(i-14:i));
        
    end
    output_return = 0;
    
    yaw_data(i) = atan2(C(2,1),C(1,1));
    
    pitch_data(i) = asin(-C(3,1));
    roll_data(i) = atan2(C(3,2),C(3,3));
    
    pos_result(:,i) = pos_n(:,i) - last_pos;
    
    if( (((gyr_norm(i)< 0.6)  && (i > 15) && ((max_window - min_window) < 0.6))...
         || ((gyr_norm(i)< 1.5) && (i > 15) && (i- last_zupt_index > 25) &&(max_window -min_window) > 6.0))...
         || ((gyr_norm(i)< 1.5) && (i > 15) && (i- last_zupt_index > 50)  && (max_window - min_window) > 5.0))
    
            if(last_zupt == 1)

               stand_num = stand_num + 1;

               output_signal = 1;

                %用来记录零速的时刻, 可以忽略
                zupt(i) = 0.7;

                %计算卡尔曼滤波增益，公式公式
                % 说明一下H矩阵为观测矩阵，在线性系统下就是值1.观测量需要用到哪一个就在zeros（1,9）中对应的位置为1

                K = P*H'/(H*P*H' + R);


                delta_x = K * [vel_n(:,i)];

                P = P - K*H*P;

                %姿态矩阵补偿
                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(:,i) = pos_n(:,i) - delta_x(4:6);
                vel_n(:,i) = vel_n(:,i) - delta_x(7:9);
                %防止脚地上的时候，Z方向的速度过快
                yaw = 0.0;
                pitch = asin(-C(3,1));
                roll = roll_data(i);
                C = [cos(pitch),sin(pitch)*sin(roll), sin(pitch)*cos(roll);
              0.0, cos(roll), -1*sin(roll);
              -sin(pitch), cos(pitch)* sin(roll), cos(pitch)*cos(roll);];
                

                     last_pos = pos_n(:, i);
          
                %记录一步一步的位置移动
            end
                last_zupt = 1;

                last_running = i;
         
    else
       gyr_norm_flag = 10000;
        
        
        if(last_zupt == 1)
           last_zupt_index = i-1;
           last_zupt = 0;
       end
       
       if(output_signal == 1 && pos_result(3,i) < -0.01 && i-last_zupt_index>10 )
           
           if(abs(pos_result(1,i)) > 0.3 && abs(pos_result(2,i)) > 0.3 && abs(pos_result(3,i)) > 0.3)
               
           end
           
           if(pos_result(2,i) < - 0.3)
                disp(["MOVE_LEFT"]);
               output_return = 3;
               output_signal = 0;
               output_time(output_index)  = i;
               output_index = output_index + 1;
               
           elseif pos_result(2,i) >  0.3
                disp(["MOVE_RIGTH"]);
                output_return = 4;
                output_signal = 0;
                output_time(output_index)  = i;
                output_index = output_index + 1;
               
               
           elseif pos_result(1, i) > 0.3
                disp(["MOVE_BACKWARD"]);
               output_return = 2;
               output_signal = 0;
               output_time(output_index)  = i;
               output_index = output_index + 1;
               
           elseif pos_result(1, i) < -0.3
                 disp(["MOVE_FORWARD"]);
               output_return = 5;
               output_signal = 0;
               output_time(output_index)  = i;
               output_index = output_index + 1;
               
            elseif pos_result(3, i) < -0.3
               disp(["MOVE_JUMP"]);
               output_return = 8;
               output_signal = 0;
               output_time(output_index)  = i;
               output_index = output_index + 1;
           
            elseif  i - last_running > 30
                 disp(["MOVE_RUNNING"]);
                output_return = 7;
                last_running = i;
%               output_time(output_index)  = i;
%               output_index = output_index + 1;
               
           end
       end
       
       

        
    end
    
    
    if(output_signal == 1 && stand_num > 100)
           output_signal = 0;
           output_return = 9;
           stand_num = 0;
           disp(["MOVE_stand"]);
     end
    %计算速度的均方值
    vel_n_norm(i) = norm(vel_n(:, i));
    
    
    %保持协方差矩阵的对称性
    P = (P+P')/2;
    
    C_prev = C;
    
    
    vel_local(:,i) = [cos(yaw_data(i)), -sin(yaw_data(i)) ;
                      sin(yaw_data(i)),   cos(yaw_data(i))]*vel_n(1:2, i);
    
    
    C_buff(:,:,i) = C;
    
    %输出

end


% 其实做这种游戏的姿态估计,能大概判断第K步相对于第K-1偏移多少就好（方向，x,y位置）
% 例如判断左、右、前、后，移动的情况，可以根据
% theta_yaw = yaw(k)-yaw(k-1);
% R = sqrt((pos_n(1,k)- pos_n(1,k-1))^2 + (pos_n(2,k)- pos_n(2,k-1))^2 )
% [x, y] =[cos(theta),sin(theta)]*R;就可以根据XY的符号判断左右前后了
% 跳的动作根据v_z, p_z来判断，初步是可以的；

figure('NumberTitle', 'off', 'Name', 'Offset pos');
hold on;
plot(pos_result(1,:), 'r');
plot(pos_result(2,:), 'g');
plot(pos_result(3,:), 'b');
plot(output_time,pos_result(3,output_time),'k*');
hold on;
grid;
xlabel('sample');
ylabel('m');
title('Offset pos');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', 'board pos');
hold on;
plot(board_pos(1,:), 'r');
plot(board_pos(2,:), 'g');
plot(board_pos(3,:), 'b');
plot(output_time,board_pos(3,output_time),'k*');
hold on;
grid;
xlabel('sample');
ylabel('m');
title('Offset pos');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', 'Linear Velocity');
hold on;
plot(vel_n(1,:), 'r');
plot(vel_n(2,:), 'g');
plot(vel_n(3,:), 'b');

index = find(zupt > 0);

plot(index,vel_n(3,index),'k*');

grid;
xlabel('sample');
ylabel('m/s');
title('local velocity');
legend('X', 'Y', 'Z');

figure('NumberTitle', 'off', 'Name', 'Linear Position');
hold on;
plot(pos_n(1,:), 'r');
plot(pos_n(2,:), 'g');
plot(pos_n(3,:), 'b');
grid;
xlabel('sample');
ylabel('m');
title('local position');
legend('X', 'Y', 'Z');

% figure
% plot3(pos_n(1,:), pos_n(2,:), pos_n(3,:));
% 
% pose_diff = step_pos(1:2, 2:end) - step_pos(1:2, 1:end-1);
% figure
% plot( step_pos(2,:))
% 
figure('Name', 'IMU zupt')
plot(gyr_norm(1,:),'r')
hold on
plot((acc_norm(1,:)-9.8)/5,'g');
index = find(zupt > 0);
plot(index,gyr_norm(index),'ko')

grid on

figure
plot3(pos_n(1,:), pos_n(2,:), pos_n(3, :),'r');grid on


figure('NumberTitle', 'off', 'Name', 'IMU vel');
hold on;
plot(vel_local(1,:), 'r');
plot(vel_local(2,:), 'g');

%% Animation
%% The animation code was stolen from xioTechnologies.
%% https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU

% L = data_size;
% SamplePlotFreq = 4;
% Spin = 120;
% SixDofAnimation(pos_result(1:3,:)', C_buff, ...
%                 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'All',...
%                 'Position', [9 39 1280 768],...
%                 'View', [(100:(Spin/(L-1)):(100+Spin))', 10*ones(L, 1)],...
%                 'AxisLength', 0.1, 'ShowArrowHead', false, ...
%                 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)',... 
%                 'ShowLegend', false,...
%                 'CreateVideo', false);