# sculling_minor_v000

```%sculling (essentially sculling+rotation) correction for each minor
%interval
%gyro & acc must be increment type (3,length) outputs. alg is the desired
%sculling algorithm
function [vinc, ainc, corr]=sculling_minor(gyro, acc, alg)
inlen=size(acc,2);
switch (alg)
case (0)
%ignagni(1998):algorithm 1 -> (1996):algo2
outlen=floor((inlen-3)/2)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=3:2:inlen
vinc(:,ind)=sum(acc(:,i-1:i),2);
ainc(:,ind)=sum(gyro(:,i-1:i),2);

rot=0.5*cross(ainc(:,ind), vinc(:,ind));
vr_a=cross(((-1/30)*gyro(:,i-2)+(11/15)*gyro(:,i-1)), acc(:,i));
vr_b=cross(((-1/30)*acc(:,i-2)+(11/15)*acc(:,i-1)), gyro(:,i));
corr(:,ind)=rot+vr_a+vr_b;
ind=ind+1;
end
case (1)
%ignagni(1998):algorithm 2 -> (1996):algo3
outlen=floor((inlen-3)/3)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=3:3:inlen
vinc(:,ind)=sum(acc(:,i-2:i),2);
ainc(:,ind)=sum(gyro(:,i-2:i),2);

rot=0.5*cross(ainc(:,ind), vinc(:,ind));
vr_a=cross(((9/20)*gyro(:,i-2)+(27/20)*gyro(:,i-1)), acc(:,i));
vr_b=cross(((9/20)*acc(:,i-2)+(27/20)*acc(:,i-1)), gyro(:,i));
corr(:,ind)=rot+vr_a+vr_b;
ind=ind+1;
end
case(2)
%ignagni(1998):algorithm 3 -> (1996):algo5
outlen=floor((inlen-4)/3)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=4:3:inlen
vinc(:,ind)=sum(acc(:,i-2:i),2);
ainc(:,ind)=sum(gyro(:,i-2:i),2);

rot=0.5*cross(ainc(:,ind), vinc(:,ind));
vr_a=cross(((3/280)*gyro(:,i-3)+(57/140)*gyro(:,i-2)+(393/280)*gyro(:,i-1)), acc(:,i));
vr_b=cross(((3/280)*acc(:,i-3)+(57/140)*acc(:,i-2)+(393/280)*acc(:,i-1)), gyro(:,i));
corr(:,ind)=rot+vr_a+vr_b;
ind=ind+1;
end
case(3)
%ignagni(1998):algorithm 4 -> (1996):algo6
outlen=floor((inlen-5)/3)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=5:3:inlen
vinc(:,ind)=sum(acc(:,i-2:i),2);
ainc(:,ind)=sum(gyro(:,i-2:i),2);

rot=0.5*cross(ainc(:,ind), vinc(:,ind));
vr_a=cross(((-1/420)*gyro(:,i-4)+(1/40)*gyro(:,i-3)+(157/420)*gyro(:,i-2)+(1207/840)*gyro(:,i-1)), acc(:,i));
vr_b=cross(((-1/420)*acc(:,i-4)+(1/40)*acc(:,i-3)+(157/420)*acc(:,i-2)+(1207/840)*acc(:,i-1)), gyro(:,i));
corr(:,ind)=rot+vr_a+vr_b;
ind=ind+1;
end
case(4)
%ignagni(1998):algorithm 5 -> (1996):algo7
outlen=floor((inlen-4)/4)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=4:4:inlen
vinc(:,ind)=sum(acc(:,i-3:i),2);
ainc(:,ind)=sum(gyro(:,i-3:i),2);

rot=0.5*cross(ainc(:,ind), vinc(:,ind));
vr_a=cross(((54/105)*gyro(:,i-3)+(92/105)*gyro(:,i-2)+(214/105)*gyro(:,i-1)), acc(:,i));
vr_b=cross(((54/105)*acc(:,i-3)+(92/105)*acc(:,i-2)+(214/105)*acc(:,i-1)), gyro(:,i));
corr(:,ind)=rot+vr_a+vr_b;
ind=ind+1;
end
case(5) %%quadratic fit to both gyro and accel outputs (similar to the coning algorithm in ignagni(1990):algoD
outlen=floor((inlen-3)/3)+1;
vinc=zeros(3,outlen);
ainc=zeros(3,outlen);
corr=zeros(3,outlen);

ind=1;
for i=3:3:inlen
vinc(:,ind)=sum(acc(:,i-2:i),2);
ainc(:,ind)=sum(gyro(:,i-2:i),2);

%compute the polynomial coefficients
b=(11/6)*gyro(:,i-2)+(-7/6)*gyro(:,i-1)+(1/3)*gyro(:,i);
c=(-2)*gyro(:,i-2)+(3)*gyro(:,i-1)+(-1)*gyro(:,i);
d=(1/2)*gyro(:,i-2)+(-1)*gyro(:,i-1)+(1/2)*gyro(:,i);

e=(11/6)*acc(:,i-2)+(-7/6)*acc(:,i-1)+(1/3)*acc(:,i);
f=(-2)*acc(:,i-2)+(3)*acc(:,i-1)+(-1)*acc(:,i);
g=(1/2)*acc(:,i-2)+(-1)*acc(:,i-1)+(1/2)*acc(:,i);

%Compute sculling(+rotation)
corr(:,ind)=(3^2/2)*cross(b,e)+(3^3/3)*(cross(b,f)+cross(c,e)/2)+(3^4/4)*(cross(b,g)+cross(c,f)/2+cross(d,e)/3)+...
(3^5/5)*(cross(c,g)/2+cross(d,f)/3)+(3^6/6)*cross(d,g)/3;

ind=ind+1;
end

otherwise
disp('undefined algorithm');
end
```