zupt.m 4.8 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183
  1. % This implements a zero velocity update EKF algorithm, which can be used
  2. % for pedestrian tracking.
  3. % By @TanTanTan
  4. % Email: jinaqiaoqiao@gmail.com
  5. clear; close all; clc;
  6. % --- Dataset parameters
  7. dt = 1/256;
  8. g = 9.81;
  9. sigma_acc = 0.1; % accelerometer noise (m/s^2)
  10. sigma_gyro = 0.1*pi/180; % gyroscope noise (rad/s)
  11. sigma_vel = 0.01; % zero-velocity update measurement noise (m/s)
  12. %% zero-velocity detector parameters
  13. cova = 0.01^2;
  14. covw = (0.1*pi/180)^2;
  15. W = 5; % window size
  16. gamma = 0.3e5;
  17. %% read data (Choose one)
  18. % % data = load('data/StaightLine.mat');
  19. % % data = load('data/StairsAndCorridor.mat');
  20. data = load('data/SpiralStairs.mat');
  21. imu_data = data.imu_data';
  22. N = size(imu_data, 2);
  23. %% Since we have got all the data, we can run the zero velocity detection
  24. %% for the whole dataset.
  25. iszv = zeros(1, N);
  26. T = zeros(1, N-W+1);
  27. for k = 1:N-W+1
  28. mean_a = mean(imu_data(1:3,k:k+W-1), 2);
  29. for l = k:k+W-1
  30. temp = imu_data(1:3,l) - g * mean_a / norm(mean_a);
  31. T(k) = T(k) + imu_data(4:6,l)'*imu_data(4:6,l)/covw + temp'*temp/cova;
  32. end
  33. end
  34. T = T./W;
  35. for k = 1:size(T,2)
  36. if T(k) < gamma
  37. iszv(k:k+W-1) = ones(1,W);
  38. end
  39. end
  40. %%
  41. %=========================================================================%
  42. %== Initialization =%
  43. %=========================================================================%
  44. % We require that the IMU to be at static for first one second. So we can
  45. % estimate the initial roll/pitch angle, as well as the biases.
  46. init_a = mean(imu_data(1:3,1:20),2);
  47. init_a = init_a / norm(init_a);
  48. init_psi = 0;
  49. init_theta = -asin(init_a(1));
  50. init_phi = atan2(init_a(2), init_a(3));
  51. init_quat = angle2quat(init_psi, init_theta, init_phi);
  52. % Estimate sensor bias.
  53. Rsw = quat2dcm(init_quat);
  54. as = Rsw * [0;0;g];
  55. bias_a = mean(imu_data(1:3,1:500),2) - as;
  56. bias_w = mean(imu_data(4:6,1:500),2);
  57. % set the initial state vector
  58. x = zeros(10,1);
  59. x(7:10,1) = init_quat';
  60. % set the initial covariance
  61. P = diag([1e-10*ones(1,6), 1e-6*ones(1,4)]);
  62. %
  63. x_r = zeros(10,N);
  64. x_r(:,1) = x;
  65. % measurement matrix
  66. H = [zeros(3), eye(3), zeros(3,4)];
  67. R = sigma_vel^2 * eye(3);
  68. %%
  69. %=========================================================================%
  70. %== Main Loop =%
  71. %=========================================================================%
  72. for k = 2:N
  73. %% compute state transition matrix F and covariance Q
  74. w = imu_data(4:6, k-1); % - bias_w;
  75. quat = x(7:10,1);
  76. a = imu_data(1:3, k-1); % - bias_a;
  77. % continuous state transition matrix
  78. Ow = [0 -w(1) -w(2) -w(3);...
  79. w(1) 0 w(3) -w(2);...
  80. w(2) -w(3) 0 w(1);...
  81. w(3) w(2) -w(1) 0 ];
  82. Vq = compVq(quat, a);
  83. Fc = zeros(10);
  84. Fc(1:3, 4:6) = eye(3);
  85. Fc(4:10,7:10)= [Vq; 0.5*Ow];
  86. % continuous process covariance
  87. Gq = 0.5* [-quat(2) -quat(3) -quat(4); ...
  88. quat(1) -quat(4) quat(3); ...
  89. quat(4) quat(1) -quat(2); ...
  90. -quat(3) quat(2) quat(1)];
  91. Qc = zeros(10);
  92. Qc(4:6, 4:6) = sigma_acc^2*eye(3);
  93. Qc(7:10,7:10) = sigma_gyro^2*(Gq*Gq');
  94. % discretilization
  95. F = eye(10) + Fc* dt;
  96. Q = Qc* dt;
  97. %% state propagation
  98. R_S_n = quat2dcm(quat');
  99. acc = R_S_n' * a - [0; 0; g];
  100. x(1:3) = x(1:3) + x(4:6)* dt + 0.5*acc* dt^2;
  101. x(4:6) = x(4:6) + acc* dt;
  102. quat = (eye(4) + 0.5*Ow* dt)*quat;
  103. quat = quat/norm(quat);
  104. x(7:10) = quat;
  105. %% covariance propagation
  106. P = F*P*F' + Q;
  107. %% zero-velocity update
  108. if iszv(k) == 1
  109. K = (P*H')/(H*P*H'+R);
  110. y = -x(4:6);
  111. x = x + K*y;
  112. x(7:10) = x(7:10)/norm(x(7:10));
  113. P = (eye(10)-K*H)*P;
  114. end
  115. P = (P+P')/2;
  116. x_r(:,k) = x;
  117. end
  118. %% View the results
  119. %% Plot the trajectory
  120. figure(1), hold on
  121. plot(x_r(1,:),x_r(2,:));
  122. plot(x_r(1,1),x_r(2,1),'ro');
  123. legend('Trajectory','Start point')
  124. axis equal
  125. grid on
  126. figure(2),
  127. plot(1:N,x_r(3,:));
  128. title('Height')
  129. grid on
  130. %% Animation
  131. %% The animation code was stolen from xioTechnologies.
  132. %% https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU
  133. L = size(x_r,2);
  134. SamplePlotFreq = 4;
  135. Spin = 120;
  136. quat2dcm(quatinv(x_r(7:10,:)'))
  137. SixDofAnimation(x_r(1:3,:)', quat2dcm(quatinv(x_r(7:10,:)')), ...
  138. 'SamplePlotFreq', SamplePlotFreq, 'Trail', 'All',...
  139. 'Position', [9 39 1280 768],...
  140. 'View', [(100:(Spin/(L-1)):(100+Spin))', 10*ones(L, 1)],...
  141. 'AxisLength', 0.1, 'ShowArrowHead', false, ...
  142. 'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)',...
  143. 'ShowLegend', false,...
  144. 'CreateVideo', false);