[
  {
    "path": "README.md",
    "content": "# ImuFusion\n## EKF IMU Fusion Algorithms\n1. orien.m uses Kalman filter for fusing the gyroscope's and accelerometer's readings to get the IMU's attitude(quaternion).<br>\n2. zupt.m implenments the so called 'zero-velocity-update' algorithm for pedestrian tracking(gait tracking), it's also a ekf filter.<br>\n* Video: http://v.youku.com/v_show/id_XMTg2NjI4NTI4NA==.html\n\n\n## Usage\nExample data already included.<br>\nSimply run the orien.m or zupt.m. For zupt, set 'CreateVideo' as true if you'd like to save the results as a video.<br>\nNote that the datasets and the code for visualizing the results were from:\nhttps://github.com/xioTechnologies/Gait-Tracking-With-x-IMU\n\n## References:\n[1] S. Madgwick. An efficient orientation filter for inertial and inertial/magnetic sensor arrays.<br>\n[2] Fischer C, et. Implementing a Pedestrian Tracker Using inertial Sensors.<br>\n[3] Isaac Skog, et. Zero-Velocity Detection — An Algorithm Evaluation.<br>\n"
  },
  {
    "path": "SixDofAnimation.m",
    "content": "% Note that this animation code was stolen from:\n% https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU\n\nfunction fig = SixDOFanimation(varargin)\n\n    %% Create local variables\n\n    % Required arguments\n    p = varargin{1};                % position of body\n    R = varargin{2};                % rotation matrix of body\n    [numSamples dummy] = size(p);\n\n    % Default values of optional arguments\n    SamplePlotFreq = 1;\n    Trail = 'Off';\n    LimitRatio = 1;\n    Position = [];\n    FullScreen = false;\n    View = [30 20];\n    AxisLength = 1;\n    ShowArrowHead = 'on';\n    Xlabel = 'X';\n    Ylabel = 'Y';\n    Zlabel = 'Z';\n    Title = '6DOF Animation';\n    ShowLegend = true;\n    write_video = false;\n\n    for i = 3:2:nargin\n        if  strcmp(varargin{i}, 'SamplePlotFreq'), SamplePlotFreq = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Trail')\n            Trail = varargin{i+1};\n            if(~strcmp(Trail, 'Off') && ~strcmp(Trail, 'DotsOnly') && ~strcmp(Trail, 'All'))\n                error('Invalid argument.  Trail must be ''Off'', ''DotsOnly'' or ''All''.');\n            end\n        elseif  strcmp(varargin{i}, 'LimitRatio'), LimitRatio = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Position'), Position = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'FullScreen'), FullScreen = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'View'), View = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'AxisLength'), AxisLength = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'ShowArrowHead'), ShowArrowHead = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Xlabel'), Xlabel = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Ylabel'), Ylabel = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Zlabel'), Zlabel = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'Title'), Title = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'ShowLegend'), ShowLegend = varargin{i+1};\n        elseif  strcmp(varargin{i}, 'CreateVideo'), write_video = varargin{i+1};\n        else error('Invalid argument.');\n        end\n    end;\n\n    %% Reduce data to samples to plot only\n\n    p = p(1:SamplePlotFreq:numSamples, :);\n    R = R(:, :, 1:SamplePlotFreq:numSamples) * AxisLength;\n    if(numel(View) > 2)\n        View = View(1:SamplePlotFreq:numSamples, :);\n    end\n    [numPlotSamples dummy] = size(p);\n\n    %% Setup AVI file\n    % % % write video\n    if write_video\n        writeObj = VideoWriter('results');\n        writeObj.FrameRate = 60;\n        open(writeObj);\n    end\n\n    \n\n    %% Setup figure and plot\n\n    % Create figure\n    fig = figure();\n    if(FullScreen)\n        screenSize = get(0, 'ScreenSize');\n        set(fig, 'Position', [0 0 screenSize(3) screenSize(4)]);\n    elseif(~isempty(Position))\n        set(fig, 'Position', Position);\n    end\n    set(gca, 'drawmode', 'fast');\n    lighting phong;\n    set(gcf, 'Renderer', 'zbuffer');\n    hold on;\n    axis equal;\n    grid on;\n    view(View(1, 1), View(1, 2));\n    title(i);\n    xlabel(Xlabel);\n    ylabel(Ylabel);\n    zlabel(Zlabel);\n\n    % Create plot data arrays\n    if(strcmp(Trail, 'DotsOnly') || strcmp(Trail, 'All'))\n        x = zeros(numPlotSamples, 1);\n        y = zeros(numPlotSamples, 1);\n        z = zeros(numPlotSamples, 1);\n    end\n    if(strcmp(Trail, 'All'))\n        ox = zeros(numPlotSamples, 1);\n        oy = zeros(numPlotSamples, 1);\n        oz = zeros(numPlotSamples, 1);\n        ux = zeros(numPlotSamples, 1);\n        vx = zeros(numPlotSamples, 1);\n        wx = zeros(numPlotSamples, 1);\n        uy = zeros(numPlotSamples, 1);\n        vy = zeros(numPlotSamples, 1);\n        wy = zeros(numPlotSamples, 1);\n        uz = zeros(numPlotSamples, 1);\n        vz = zeros(numPlotSamples, 1);\n        wz = zeros(numPlotSamples, 1);\n    end\n    x(1) = p(1,1);\n    y(1) = p(1,2);\n    z(1) = p(1,3);\n    ox(1) = x(1);\n    oy(1) = y(1);\n    oz(1) = z(1);\n    ux(1) = R(1,1,1:1);\n    vx(1) = R(2,1,1:1);\n    wx(1) = R(3,1,1:1);\n    uy(1) = R(1,2,1:1);\n    vy(1) = R(2,2,1:1);\n    wy(1) = R(3,2,1:1);\n    uz(1) = R(1,3,1:1);\n    vz(1) = R(2,3,1:1);\n    wz(1) = R(3,3,1:1);\n\n    % Create graphics handles\n    orgHandle = plot3(x, y, z, 'k.');\n    if(ShowArrowHead)\n        ShowArrowHeadStr = 'on';\n    else\n        ShowArrowHeadStr = 'off';\n    end\n    quivXhandle = quiver3(ox, oy, oz, ux, vx, wx,  'r', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');\n    quivYhandle = quiver3(ox, oy, oz, uy, vy, wy,  'g', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');\n    quivZhandle = quiver3(ox, ox, oz, uz, vz, wz,  'b', 'ShowArrowHead', ShowArrowHeadStr, 'MaxHeadSize', 0.999999, 'AutoScale', 'off');\n\n    % Create legend\n    if(ShowLegend)\n        legend('Origin', 'X', 'Y', 'Z');\n    end\n    \n    % Set initial limits\n    Xlim = [x(1)-AxisLength x(1)+AxisLength] * LimitRatio;\n    Ylim = [y(1)-AxisLength y(1)+AxisLength] * LimitRatio;\n    Zlim = [z(1)-AxisLength z(1)+AxisLength] * LimitRatio;\n    set(gca, 'Xlim', Xlim, 'Ylim', Ylim, 'Zlim', Zlim);\n    \n    % Set initial view\n    view(View(1, :));\n\n    %% Plot one sample at a time\n\n    for i = 1:numPlotSamples\n\n        % Update graph title\n        if(strcmp(Title, ''))\n            titleText = sprintf('Sample %i of %i', 1+((i-1)*SamplePlotFreq), numSamples);\n        else\n            titleText = strcat(Title, ' (', sprintf('Sample %i of %i', 1+((i-1)*SamplePlotFreq), numSamples), ')');\n        end\n        title(titleText);\n\n        % Plot body x y z axes\n        if(strcmp(Trail, 'DotsOnly') || strcmp(Trail, 'All'))\n            x(1:i) = p(1:i,1);\n            y(1:i) = p(1:i,2);\n            z(1:i) = p(1:i,3);\n        else\n            x = p(i,1);\n            y = p(i,2);\n            z = p(i,3);\n        end\n        if(strcmp(Trail, 'All'))\n            ox(1:i) = p(1:i,1);\n            oy(1:i) = p(1:i,2);\n            oz(1:i) = p(1:i,3);\n            ux(1:i) = R(1,1,1:i);\n            vx(1:i) = R(2,1,1:i);\n            wx(1:i) = R(3,1,1:i);\n            uy(1:i) = R(1,2,1:i);\n            vy(1:i) = R(2,2,1:i);\n            wy(1:i) = R(3,2,1:i);\n            uz(1:i) = R(1,3,1:i);\n            vz(1:i) = R(2,3,1:i);\n            wz(1:i) = R(3,3,1:i);\n        else\n            ox = p(i,1);\n            oy = p(i,2);\n            oz = p(i,3);\n            ux = R(1,1,i);\n            vx = R(2,1,i);\n            wx = R(3,1,i);\n            uy = R(1,2,i);\n            vy = R(2,2,i);\n            wy = R(3,2,i);\n            uz = R(1,3,i);\n            vz = R(2,3,i);\n            wz = R(3,3,i);\n        end\n        set(orgHandle, 'xdata', x, 'ydata', y, 'zdata', z);\n        set(quivXhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', ux, 'vdata', vx, 'wdata', wx);\n        set(quivYhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', uy, 'vdata', vy, 'wdata', wy);\n        set(quivZhandle, 'xdata', ox, 'ydata', oy, 'zdata', oz,'udata', uz, 'vdata', vz, 'wdata', wz);\n\n        % Adjust axes for snug fit and draw\n        axisLimChanged = false;\n        if((p(i,1) - AxisLength) < Xlim(1)), Xlim(1) = p(i,1) - LimitRatio*AxisLength; axisLimChanged = true; end\n        if((p(i,2) - AxisLength) < Ylim(1)), Ylim(1) = p(i,2) - LimitRatio*AxisLength; axisLimChanged = true; end\n        if((p(i,3) - AxisLength) < Zlim(1)), Zlim(1) = p(i,3) - LimitRatio*AxisLength; axisLimChanged = true; end\n        if((p(i,1) + AxisLength) > Xlim(2)), Xlim(2) = p(i,1) + LimitRatio*AxisLength; axisLimChanged = true; end\n        if((p(i,2) + AxisLength) > Ylim(2)), Ylim(2) = p(i,2) + LimitRatio*AxisLength; axisLimChanged = true; end\n        if((p(i,3) + AxisLength) > Zlim(2)), Zlim(2) = p(i,3) + LimitRatio*AxisLength; axisLimChanged = true; end\n        if(axisLimChanged), set(gca, 'Xlim', Xlim, 'Ylim', Ylim, 'Zlim', Zlim); end\n        drawnow;\n\n        % Adjust view\n        if(numel(View) > 2)\n            view(View(i, :));\n        end\n\n        % Add frame to AVI object\n        if write_video\n            frame = getframe(fig);\n            writeVideo(writeObj, frame);\n        end \n\n    end\n\n    hold off;\n\n    % Close AVI file\n    if write_video\n        close(writeObj);\n    end\n\nend"
  },
  {
    "path": "compVq.m",
    "content": "function Vq = compVq(q, a)\n\nq1 = q(1);\nq2 = q(2);\nq3 = q(3);\nq4 = q(4);\n\nax = a(1);\nay = a(2);\naz = a(3);\n\nv_dot_q1 = [2*ax*q1 - 2*ay*q4 + 2*az*q3;...\n            2*ax*q4 + 2*ay*q1 - 2*az*q2;...\n            2*ay*q2 - 2*ax*q3 + 2*az*q1];\n\nv_dot_q2 = [2*ax*q2 + 2*ay*q3 + 2*az*q4;...\n            2*ax*q3 - 2*ay*q2 - 2*az*q1;...\n            2*ax*q4 + 2*ay*q1 - 2*az*q2];\n \nv_dot_q3 = [2*ay*q2 - 2*ax*q3 + 2*az*q1;...\n            2*ax*q2 + 2*ay*q3 + 2*az*q4;...\n            2*ay*q4 - 2*ax*q1 - 2*az*q3];\n\nv_dot_q4 = [2*az*q2 - 2*ay*q1 - 2*ax*q4;...\n            2*ax*q1 - 2*ay*q4 + 2*az*q3;...\n            2*ax*q2 + 2*ay*q3 + 2*az*q4];\n        \nVq = [v_dot_q1, v_dot_q2, v_dot_q3, v_dot_q4];"
  },
  {
    "path": "data/readme.txt",
    "content": "Note that the StraightLine.mat, SpiralStairs.mat, StairAndCorridor.mat were created from the csv files provided by:\nhttps://github.com/xioTechnologies/Gait-Tracking-With-x-IMU\n"
  },
  {
    "path": "orien.m",
    "content": "% This is an Extended Kalman Filter which fuses \n% accelerometer & gyroscope readings\n% to get the orientation.\n% Written by @TanTanTan\n\n% Note that we assume the biases of IMU do not change in the whole\n% process. This is approximately true when the sensors operate in a steady\n% state.\n\nclear; close all; clc;\n\n%--- Dataset parameters\ndeltat = 1/200;             % Sampling period\nnoise_gyro = 2.4e-3;        % Gyroscope noise(discrete), rad/s\nnoise_accel = 2.83e-2;      % Accelerometer noise, m/s^2\ngravity = 9.81007;          % Gravity magnitude, m/s^2\n\n%--- Load data\n% Each row of IMU data : \n% (timestamp, wx, wy, wz, ax, ay, az)\n% Each row of Ground truth data : \n% (time, position, quaternion, velocity, gyroscope bias, accelerometer bias)\ndata = load('data/attitude_data.mat');\nimu_data = data.imu_data;   % IMU readings\ngrt_data = data.grt_data;   % Ground truth (GT)\ngrt_q = grt_data(:, 5:8);   % GT quaternions\n\nbias_w = grt_data(1, 12:14);    % gyroscope bias\nbias_a = grt_data(1, 15:17);    % accelerometer bias\n\n\n%--- Container of the results\nN = length(imu_data);\nallX = zeros(N, 4);\n\n\n%--- Initialization\nx = grt_q(1,:)';            % Initial state (quaternion)\nP = 1e-10 * eye(4);         % Initial covariance\nallX(1,:) = x';\n\n\n% ---Here we go !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!\nfor k = 2 : N\n    \n    %--- 1. Propagation --------------------------\n    % Gyroscope measurements\n    w = (imu_data(k-1, 2:4) + imu_data(k, 2:4))/2;\n    w = w - bias_w;\n    \n    % Compute the F matrix\n    Omega =[0     -w(1)   -w(2)   -w(3);...\n            w(1)  0       w(3)    -w(2); ...\n            w(2)  -w(3)   0        w(1); ...\n            w(3)  w(2)    -w(1)    0  ];\n    F = eye(4) + deltat * Omega / 2;\n    \n    % Compute the process noise Q\n    G = [-x(2)  -x(3)  -x(4); ...\n          x(1)  -x(4)   x(3); ...\n          x(4)   x(1)  -x(2); ...\n         -x(3)   x(2)   x(1)] / 2;\n    Q = (noise_gyro * deltat)^2 * (G * G');\n    \n    % Propagate the state and covariance\n    x = F * x;\n    x = x / norm(x);    % Normalize the quaternion\n    P = F * P * F' + Q;\n    \n    \n    %--- 2. Update----------------------------------\n    % Accelerometer measurements\n    a = imu_data(k, 5:7);\n    a = a - bias_a;\n        \n    % We use the unit vector of acceleration as observation\n    ea = a' / norm(a);\n    ea_prediction = [2*(x(2)*x(4)-x(1)*x(3)); ...\n                     2*(x(3)*x(4)+x(1)*x(2)); ...\n                     x(1)^2-x(2)^2-x(3)^2+x(4)^2];\n    \n    % Residual\n    y = ea - ea_prediction;\n    \n    % Compute the measurement matrix H\n    H = 2*[-x(3)    x(4)    -x(1)   x(2); ...\n            x(2)    x(1)     x(4)   x(3); ...\n            x(1)   -x(2)    -x(3)   x(4)];\n        \n    % Measurement noise R\n    R_internal = (noise_accel / norm(a))^2 * eye(3);\n    R_external = (1-gravity/norm(a))^2 * eye(3);\n    R = R_internal + R_external;\n    \n    % Update\n    S = H * P * H' + R;\n    K = P * H' / S;\n    x = x + K * y;\n    P = (eye(4) - K * H) * P;\n    \n    \n    % 3. Ending\n    x = x / norm(x);    % Normalize the quaternion\n    P = (P + P') / 2;   % Make sure that covariance matrix is symmetric\n    allX(k,:) = x';     % Save the result\n    \nend\n\n\n%--- Compare the results with ground truth\nq_Ws0 = quatinv(grt_q(1,:));\nfor i=1:N\n    grt_q(i,:) = quatmultiply(q_Ws0, grt_q(i,:)); \n    allX(i,:) = quatmultiply(q_Ws0, allX(i,:));\nend\n[psi, theta, phi] = quat2angle(allX);\n[grt_psi, grt_theta, grt_phi] = quat2angle(grt_q);\n\nfigure, hold on\nplot(1:N, psi,   'r-.', 1:N, grt_psi,   'r');\nplot(1:N, theta, 'g-.', 1:N, grt_theta, 'g');\nplot(1:N, phi,   'b-.', 1:N, grt_phi,   'b');\n\n    \n"
  },
  {
    "path": "zupt.m",
    "content": "% This implements a zero velocity update EKF algorithm, which can be used\n% for pedestrian tracking.\n% By @TanTanTan\n% Email: jinaqiaoqiao@gmail.com\n\nclear; close all; clc;\n\n% --- Dataset parameters\ndt = 1/256;\ng = 9.81;\nsigma_acc  = 0.1;           % accelerometer noise (m/s^2)\nsigma_gyro = 0.1*pi/180;    % gyroscope noise (rad/s)\nsigma_vel = 0.01;           % zero-velocity update measurement noise (m/s)\n\n\n%% zero-velocity detector parameters\ncova  = 0.01^2; \ncovw  = (0.1*pi/180)^2;     \nW     = 5; % window size\ngamma  = 0.3e5;\n\n%% read data (Choose one)\n% % data = load('data/StaightLine.mat');\n% % data = load('data/StairsAndCorridor.mat');\ndata = load('data/SpiralStairs.mat');\nimu_data = data.imu_data';\nN = size(imu_data, 2);\n\n\n%% Since we have got all the data, we can run the zero velocity detection\n%%  for the whole dataset.\niszv = zeros(1, N);\nT = zeros(1, N-W+1);\nfor k = 1:N-W+1\n    mean_a = mean(imu_data(1:3,k:k+W-1), 2);\n    for l = k:k+W-1\n        temp = imu_data(1:3,l) - g * mean_a / norm(mean_a);\n        T(k) = T(k) + imu_data(4:6,l)'*imu_data(4:6,l)/covw + temp'*temp/cova;\n    end\nend\nT = T./W;\nfor k = 1:size(T,2)\n    if T(k) < gamma\n        iszv(k:k+W-1) = ones(1,W);\n    end\nend\n\n\n%%\n%=========================================================================%\n%==                             Initialization                           =%\n%=========================================================================%\n% We require that the IMU to be at static for first one second. So we can\n% estimate the initial roll/pitch angle, as well as the biases.\ninit_a = mean(imu_data(1:3,1:20),2);\ninit_a = init_a / norm(init_a);\n\ninit_psi =  0;\ninit_theta = -asin(init_a(1));\ninit_phi = atan2(init_a(2), init_a(3));\n\ninit_quat = angle2quat(init_psi, init_theta, init_phi);\n\n% Estimate sensor bias.\nRsw = quat2dcm(init_quat);\nas  = Rsw * [0;0;g];\nbias_a = mean(imu_data(1:3,1:500),2) - as;\nbias_w = mean(imu_data(4:6,1:500),2);\n\n% set the initial state vector\nx = zeros(10,1);\nx(7:10,1) = init_quat';\n\n% set the initial covariance\nP = diag([1e-10*ones(1,6), 1e-6*ones(1,4)]);\n\n%\nx_r = zeros(10,N);\nx_r(:,1) = x;\n\n% measurement matrix\nH = [zeros(3), eye(3), zeros(3,4)];\nR =  sigma_vel^2 * eye(3);\n\n%%\n%=========================================================================%\n%==                             Main  Loop                               =%\n%=========================================================================%\nfor k = 2:N\n    %% compute state transition matrix F and covariance Q\n    w = imu_data(4:6, k-1); % - bias_w;\n    quat = x(7:10,1);\n    a = imu_data(1:3, k-1); % - bias_a;\n    \n    % continuous state transition matrix\n    Ow = [0     -w(1)   -w(2)    -w(3);...\n          w(1)   0       w(3)    -w(2);...\n          w(2)  -w(3)    0        w(1);...\n          w(3)   w(2)   -w(1)     0  ];\n    Vq = compVq(quat, a);\n    \n    Fc = zeros(10);\n    Fc(1:3, 4:6) = eye(3);\n    Fc(4:10,7:10)= [Vq; 0.5*Ow];\n    \n    % continuous process covariance\n    Gq = 0.5* [-quat(2)  -quat(3)   -quat(4); ...\n                quat(1)  -quat(4)    quat(3); ...\n                quat(4)   quat(1)   -quat(2); ...\n               -quat(3)   quat(2)    quat(1)];       \n    Qc = zeros(10);\n    Qc(4:6, 4:6)  =  sigma_acc^2*eye(3);\n    Qc(7:10,7:10) =  sigma_gyro^2*(Gq*Gq');\n    \n    % discretilization\n    F = eye(10) + Fc* dt;\n    Q = Qc* dt;\n    \n    %% state propagation\n    R_S_n = quat2dcm(quat');\n    acc = R_S_n' * a - [0; 0;  g];\n    \n    x(1:3) = x(1:3) + x(4:6)* dt + 0.5*acc* dt^2;\n    x(4:6) = x(4:6) + acc* dt;\n    \n    quat = (eye(4) + 0.5*Ow* dt)*quat;\n    quat = quat/norm(quat);\n    x(7:10) = quat;\n    \n    %% covariance propagation\n    P = F*P*F' + Q;\n\n    %% zero-velocity update\n    if iszv(k) == 1\n        K = (P*H')/(H*P*H'+R);\n        y = -x(4:6);\n        \n        x = x + K*y;\n        x(7:10) = x(7:10)/norm(x(7:10));\n        \n        P = (eye(10)-K*H)*P;\n    end\n    \n    P = (P+P')/2;\n    \n    x_r(:,k) = x;\n      \nend\n\n\n%% View the results\n%% Plot the trajectory\nfigure(1), hold on\nplot(x_r(1,:),x_r(2,:));\nplot(x_r(1,1),x_r(2,1),'ro');\nlegend('Trajectory','Start point')\naxis equal\ngrid on\n\nfigure(2),\nplot(1:N,x_r(3,:));\ntitle('Height')\ngrid on\n\n\n\n%% Animation\n%% The animation code was stolen from xioTechnologies.\n%% https://github.com/xioTechnologies/Gait-Tracking-With-x-IMU\nL = size(x_r,2);\nSamplePlotFreq = 4;\nSpin = 120;\nSixDofAnimation(x_r(1:3,:)', quat2dcm(quatinv(x_r(7:10,:)')), ...\n                'SamplePlotFreq', SamplePlotFreq, 'Trail', 'All',...\n                'Position', [9 39 1280 768],...\n                'View', [(100:(Spin/(L-1)):(100+Spin))', 10*ones(L, 1)],...\n                'AxisLength', 0.1, 'ShowArrowHead', false, ...\n                'Xlabel', 'X (m)', 'Ylabel', 'Y (m)', 'Zlabel', 'Z (m)',... \n                'ShowLegend', false,...\n                'CreateVideo', false);\n\n\n"
  }
]