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