Angular Rate Energy Detector Code

%Alex Haufler, afh18
%creation and implementation of a zero-velocity update algorithm. In this
%case, since the best results were from angular rate energy detector. This
%is going to be implemented

%u - The data in each column is arranged as x, y, and z axis
%specfic force components; x, y, and z axis angular rates.

% gamma = Threshold used in the zero-velocity detector. If the test statistics
%are below this value, the zero-velocity hypothesis is chosen.


function T = AREdetector(u, W)
sigma_g = sqrt(10)*pi/180;               %a standard deviation of the gyro data
var = sigma_g^2;
%W = 3;                              %The window size
N=length(u);                        %The length of the data
T=zeros(1,N-W+1);
%gamma=0.3e5;

for k=1:N-W+1
    for l=k:k+W-1
        T(k)=T(k)+norm(u(l,1:3))^2;  
    end  
end
T = T./(var*W);                 %scaling of the test statistic, can be absorbed
                                %by a different value of gamma, but for the
                                %gamma determined through the statistics of
%the ratio of pdfs of the observed hypotheses (no velocity or some nonzero
%velocity, using the maximum likelihood estimates) this scaling by the
%gyro variance is part of the maximum likelihood est. determination