ZUPT matlab实现脚部zupt算法

matlab实现脚部zupt算法，计算航向代码，运行通过。

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%  用加速度判断积分起始结束时间
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% MTI数据格式——第1列：PacketCounter；第2列：SampleTimeFine；第3列：Acc_X；
%               第4列：Acc_Y；第5列：Acc_Z；第6列：Gyr_X；第7列：Gyr_Y；第8列：Gyr_Z
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  此方法摘自论文《A Comparison of Pedestrian Dead-Reckoning》
%  利用加速度获得积分时间
clear;
clc;
T=1.5;      %阈值
g=9.8;      %重力加速度
w=15;       %滑窗采样点

A=importdata（‘MTIoutput.txt‘）;
% W=sqrt（A（:6）.^2+A（:7）.^2+A（:8）.^2）;
a=sqrt（A（:3）.^2+A（:4）.^2+（A（:5））.^2）-g;
N=max（size（a））;
% B=zeros（N1）;
Acc=zeros（N1）;

if N>w                      %总的采样点数要大于滑窗长度
    rou（1:w）=a（1:w）;        %第w个滑窗前的点仍等于原始值
    for i=w+1:N-w           %第w+1到N-w点之间进行 滑窗处理
        rou1=0;             %下一个循环，rou1清0
  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        1641  2014-02-26 10:02  Get_A_G.m
     文件        1740  2014-02-26 10:02  Get_AttMatrix.m
     文件        1263  2014-02-26 10:02  Get_q.m
     文件        2046  2014-02-26 10:02  IntegralTime.m
     文件      524416  2014-02-26 10:02  MTIoutput.txt
     文件        2134  2014-02-26 10:02  ZUPT3.m
     文件        2726  2014-02-26 10:02  AngleVelocity.m