Thursday, April 30, 2015
Haar Wavelet example
%uses haar functions to project functions in L2(R) into V0
k = 5;
t = -k:0.001:k;
a = 1; %j-1 in Vj so that it is phi(t-k)
l = 1;
v = zeros(1, length(t));
y = exp(-abs(t));
for i = 1000:1000:5000
kp = ((i/1000)-6);
v(i-999:i) = exp(kp)*(exp(1)-1);
end
for i = 5001:1000:9001
kp = (((i-1)/1000)-5);
v(i:i+999) = exp(-(1+kp))*(exp(1)-1);
end
plot(t, y)
hold on
plot(t, v)
hold off
k = 5;
t = -k:0.001:k;
a = 1; %j-1 in Vj so that it is phi(t-k)
l = 1;
v = zeros(1, length(t));
y = exp(-abs(t));
for i = 1000:1000:5000
kp = ((i/1000)-6);
v(i-999:i) = exp(kp)*(exp(1)-1);
end
for i = 5001:1000:9001
kp = (((i-1)/1000)-5);
v(i:i+999) = exp(-(1+kp))*(exp(1)-1);
end
plot(t, y)
hold on
plot(t, v)
hold off
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
%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
implementation/code for analysis and optimization of algorithms
%Analysis, and Results from Data
%Animation for the position of the Foot/IMU
%Plot the initial positions from the VICON
% figure(1)
% stem3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
% hold on
% stem3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
% hold on
% stem3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
% hold off
% grid on
figure(1)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
hold on
plot3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
hold off
grid on
%Turn Accelerations in volts into m/s
g = 9.80665;
Accelerometer = (Accelerometer - 2.5).*g; %in m/s/s
Gyroscope = (Gyroscope - 2.5).*500; %in deg/s
%Sample Autocorrelation functions
%Ax
datAx = Accelerometer(:,1);
N = length(datAx);
acfx = zeros(1,N);
meanx = (1/N)*sum(datAx);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAx(j)-meanx)*(datAx(j+l)-meanx);
end
acfx(l+1) = (1/(N-l))*temp;
end
figure(2)
plot(acfx(1:1600))
%Ay
datAy = Accelerometer(:,2);
N = length(datAy);
acfy = zeros(1,N);
meany = (1/N)*sum(datAy);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAy(j)-meany)*(datAy(j+l)-meany);
end
acfy(l+1) = (1/(N-l))*temp;
end
figure(3)
plot(acfy(1:1600))
%Az
datAz = Accelerometer(:,3);
N = length(datAz);
acfz = zeros(1,N);
meanz = (1/N)*sum(datAz);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAz(j)-meanz)*(datAz(j+l)-meanz);
end
acfz(l+1) = (1/(N-l))*temp;
end
figure(4)
plot(acfz(1:1600))
%empirical derivation of the autocovariance function
n = (1:1015)/120;
x1 = 30.7;
x2 = 50*sin(3.4*n);
x3 = (50/9)*sin(3.4*3*n);
x4 = (50/25)*sin(3.4*5*n);
x5 = (50/49)*sin(3.4*7*n);
x6 = (50/81)*sin(3.4*9*n);
x7 = (50/(11*11))*sin(3.4*11*n);
y = x1+x2+x3+x5+x6+x7;
figure(2)
hold on
plot(y, 'r')
hold off
%Simple Integration of the accelerometer data / No orientation correction
%or removal of gravity, No Kalman gain
Ts = 1/120;
po = [RANK(1,1) RANK(1,2) RANK(1,3)];
xk = [RANK(1,1) 0 0];
A = [1 Ts (Ts^2)/2;
0 1 Ts;
0 0 1];
% xkp = zeros(length(RANK(:,1)), 3);
% xkp(1,:) = xk;
% for i = 2:length(RANK(:,1))+1
% xkp(i-1, 3) = Accelerometer(i-1,1);
% temp = xkp(i-1, :)';
% xkp(i,:) = A*temp;
% xkp(i,:) = xkp(i,:)';
% end
% x=xkp(:,1);
%
% yk = [RANK(1,2) 0 0];
% A = [1 Ts (Ts^2)/2;
% 0 1 Ts;
% 0 0 1];
% ykp = zeros(length(RANK(:,1)), 3);
% ykp(1,:) = yk;
% for i = 2:length(RANK(:,1))+1
% ykp(i-1, 3) = Accelerometer(i-1,1);
% temp = ykp(i-1, :)';
% ykp(i,:) = A*temp;
% ykp(i,:) = ykp(i,:)';
% end
% y=ykp(:,1);
%
% zk = [RANK(1,3) 0 0];
% A = [1 Ts (Ts^2)/2;
% 0 1 Ts;
% 0 0 1];
% zkp = zeros(length(RANK(:,1)), 3);
% zkp(1,:) = zk;
% for i = 2:length(RANK(:,1))+1
% zkp(i-1, 3) = Accelerometer(i-1,1);
% temp = zkp(i-1, :)';
% zkp(i,:) = A*temp;
% zkp(i,:) = zkp(i,:)';
% end
% z=zkp(:,1);
%Same as above
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*Accelerometer(d-1,1)*Ts^2) (xn(d-1,2)+1000*Accelerometer(d-1,1)*Ts) 1000*Accelerometer(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*Accelerometer(d-1,2)*Ts^2) (yn(d-1,2)+1000*Accelerometer(d-1,2)*Ts) 1000*Accelerometer(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*Accelerometer(d-1,3)*Ts^2) (zn(d-1,2)+1000*Accelerometer(d-1,3)*Ts) 1000*Accelerometer(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(5)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
Ts = 1/120;
lRANK = length(RANK(:,1));
time = 1:lRANK;
%Complete Madgwick filtering to remove the effect of gravity
figure(6);
axis(1) = subplot(2,1,1);
hold on;
plot(time, Gyroscope(:,1), 'r');
plot(time, Gyroscope(:,2), 'g');
plot(time, Gyroscope(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
axis(2) = subplot(2,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Acceleration (g)');
title('Accelerometer');
hold off;
% axis(3) = subplot(3,1,3);
% hold on;
% plot(time, Magnetometer(:,1), 'r');
% plot(time, Magnetometer(:,2), 'g');
% plot(time, Magnetometer(:,3), 'b');
% legend('X', 'Y', 'Z');
% xlabel('Time (s)');
% ylabel('Flux (G)');
% title('Magnetometer');
% hold off;
linkaxes(axis, 'x');
%% Process sensor data through algorithm
AHRS = MadgwickAHRSsimpleBiasCOMP();%('SamplePeriod', 1/fs, 'Beta', 0.1, 'lenavg', 10);
% AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
quaternion = zeros(length(time), 4); quaternion(1, :) = [1 0 0 0];
newGyro = zeros(length(time),3);
for t = 1:length(time)
if t == 1
AHRS.UpdateIMU(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), quaternion(t,:)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
else
AHRS.UpdateIMU(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
end
end
%% Plot algorithm output as Euler angles
% The first and third Euler angles in the sequence (phi and psi) become
% unreliable when the middle angles of the sequence (theta) approaches ±90
% degrees. This problem commonly referred to as Gimbal Lock.
% See: http://en.wikipedia.org/wiki/Gimbal_lock
euler = quatern2euler(quaternConj(quaternion)) * (180/pi); % use conjugate for sensor frame relative to Earth and convert to degrees.
figure(7);
hold on;
plot(time, euler(:,1), 'r');
plot(time, euler(:,2), 'g');
plot(time, euler(:,3), 'b');
title('Euler angles');
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('\phi', '\theta', '\psi');
hold off;
%Using quaternion to transform from the lab frame to the aligned with
%gravity reference frame (so you can subtract out the gravity term later)
AccelerometerRef = zeros(size(Accelerometer));
AccelerometerRef = [AccelerometerRef zeros(lRANK, 1)];
newA = [zeros(lRANK, 1) Accelerometer];
for k = 1:lRANK
AccelerometerRef(k,:) = quaternProd(quaternProd(quaternion(k,:), newA(k,:)), quaternConj(quaternion(k,:)));
end
AccelRef = AccelerometerRef(:,(2:4));
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(8)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Subtracting out the gravity term as [0 0 1]*g from the above section
AccelRef(:,3) = AccelRef(:,3)-9.80665;
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(9)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%Complete Wavelet denoising using Haar wavelet and 5 'averages' and leaving
%out the 5 details
%Plot and show only the x acceleration
Ax = [AccelRef(:,1); zeros(19,1)];
[swa,swd] = swt(Ax,5,'db1');
[thr,sorh] = ddencmp('den','wv',Ax);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
figure(10)
subplot(2,1,1), plot(Ax); title('Original signal'); grid on
subplot(2,1,2), plot(clean); title('denoised signal'); grid on
%Now complete the wavelet filtering on the accelerations
Ax = clean(1:lRANK);
%Acc. in Y
Ay = [AccelRef(:,2); zeros(19,1)];
[swa,swd] = swt(Ay,5,'db1');
[thr,sorh] = ddencmp('den','wv',Ay);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
Ay = clean(1:lRANK);
%Acc. in Z
Az = [AccelRef(:,3); zeros(19,1)];
[swa,swd] = swt(Az,5,'db1');
[thr,sorh] = ddencmp('den','wv',Az);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
Az = clean(1:lRANK);
AccelRef = [Ax' Ay' Az'];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Redo the state updates with the new values of acceleration
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(11)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%Determine the Autocovariance for each of the gyroscope axes
%Establish the number of samples where there is significant correlation
%Determine the threshold from the data for very slow walking by iterating
%until the error length (final point vicon vs final point IMU) is minimized
%Gx, Gy, Gz
datGx = newGyro(:,1);
N = length(datGx);
acfGx = zeros(1,N);
meanGx = (1/N)*sum(datGx);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGx(j)-meanGx)*(datGx(j+l)-meanGx);
end
acfGx(l+1) = (1/(N-l))*temp;
end
datGy = newGyro(:,2);
N = length(datGy);
acfGy = zeros(1,N);
meanGy = (1/N)*sum(datGy);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGy(j)-meanGy)*(datGy(j+l)-meanGy);
end
acfGy(l+1) = (1/(N-l))*temp;
end
datGz = newGyro(:,3);
N = length(datGz);
acfGz = zeros(1,N);
meanGz = (1/N)*sum(datGz);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGz(j)-meanGz)*(datGz(j+l)-meanGz);
end
acfGz(l+1) = (1/(N-l))*temp;
end
figure(12)
plot(acfGx(1:1600))
grid on
hold on
plot(acfGy(1:1600), 'ro')
plot(acfGz(1:1600), 'g*')
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%After looking at the amount of time (or number of samples it takes to
%accurately determine a zero velocity - 9 samples - Apply the average
%rotational energy algorithm to zero out the bias, and optimize for the
%slowest speed walk
%For loop for optimizing the threshold detection on the angular rate energy
%algorithm
% jake = 5e5:10000:1e6;
% compare2 = zeros(5, length(jake));
%Threshold parameter
temp =[];
W = 1; %window size
gamma=990000; %threshold statistic for determining ZV
[rows, cols] = size(Gyroscope);
new_Gyro = zeros(size(Gyroscope));
checkZero = zeros(1,length(time));
T = AREdetector(Gyroscope, W);
for i = 1:length(T) %for loop for implementing the ZUPT detection
if T(i) < gamma
new_Gyro(i, 1:3) = [0 0 0];
checkZero(i) = 1; %1 means reset the accel
else
new_Gyro(i, 1:3) = Gyroscope(i, 1:3);
checkZero(i) = 0;
end
end
for i = length(T)+1:length(time)
new_Gyro(i, 1:3) = Gyroscope(i, 1:3);
checkZero(i) = 0;
end
figure(15)
plot(Gyroscope(:,3))
hold on
stem(new_Gyro(:,3),'g')
hold off
%Determine bias during each of the zero velocity periods and
%subtract from future points
%I just used the first 290 points to do this, then subtract from
%future points between ZUPT periods although it can be done for
%each period
% meanAx = mean(Accelerometer(1:290,1));
% meanAz = mean(Accelerometer(1:290,2));
% meanAy = mean(Accelerometer(1:290,3));
meanGx = mean(Accelerometer(1:290,1));
meanGz = mean(Accelerometer(1:290,2));
meanGy = mean(Accelerometer(1:290,3));
% Accelerometer(:,1)=Accelerometer(:,1)-meanAx;
% Accelerometer(:,2)=Accelerometer(:,2)-meanAz;
% Accelerometer(:,3)=Accelerometer(:,3)-meanAy;
Gyroscope(:,1)=Accelerometer(:,1)-meanGx;
Gyroscope(:,2)=Accelerometer(:,2)-meanGz;
Gyroscope(:,3)=Accelerometer(:,3)-meanGy;
AHRS = MadgwickAHRSsimpleBiasCOMP();%('SamplePeriod', 1/fs, 'Beta', 0.1, 'lenavg', 10);
% AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
quaternion = zeros(length(time), 4); quaternion(1, :) = [1 0 0 0];
newGyro = zeros(length(time),3);
for t = 1:length(time)
if t == 1
AHRS.UpdateIMU(new_Gyro(t,:) * (pi/180), Accelerometer(t,:), quaternion(t,:)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
else
AHRS.UpdateIMU(new_Gyro(t,:) * (pi/180), Accelerometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
end
end
AccelerometerRef = [zeros(size(Accelerometer)) zeros(lRANK, 1)];
newA = [zeros(lRANK, 1) Accelerometer];
for k = 1:lRANK
AccelerometerRef(k,:) = quaternProd(quaternProd(quaternion(k,:), newA(k,:)), quaternConj(quaternion(k,:)));
end
figure(18)
plot(Accelerometer(:,1))
hold on
stem(AccelerometerRef(:,2), 'r')
hold off
AccelRef = AccelerometerRef(:,(2:4));
% AccelRef = Accelerometer;
figure(16)
plot(Accelerometer(:,2))
% AccelRef(:,3) = AccelRef(:,3)-9.80665;
hold on
stem(AccelerometerRef(:,3), 'g')
%Applying ZUPT to the accelerations
for jade = 1:length(checkZero)
if checkZero(jade) == 1;
AccelRef(jade, :) = 0;
end
end
hold on
stem(AccelRef(:,2), 'r')
hold off
% AccelRef(:,2) = AccelRef(:,2)-9.80665;
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Applying a minimum mean square error filter using the average
%length of a person's step, and assuming a linear trajectory and
%same average step size
x2 = zeros(1, floor(1645/220));
y2 = zeros(1, floor(1645/220));
z2 = zeros(1, floor(1645/220));
xvec = [0 1 0 0];
yvec = [0 0 1 0];
zvec = [0 0 0 1];
%translate into the new orientation for each of the points
for to = 1:floor(1645/220)
x2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), xvec), quaternConj(quaternion(220*to-219,:)));
y2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), yvec), quaternConj(quaternion(220*to-219,:)));
z2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), zvec), quaternConj(quaternion(220*to-219,:)));
end
for to = 1:floor(1645/220)
x2n(to,:) = x2new(to, 2:4);
y2n(to,:) = y2new(to, 2:4);
z2n(to,:) = z2new(to, 2:4);
end
x2pos(1) = RANK(1,1);
y2pos(1) = RANK(1,3);
z2pos(1) = RANK(1,2);
for hat = 2:floor(1645/220)
x2pos(hat) = x2pos(hat - 1)+x2n(hat-1,1)*812;
y2pos(hat) = x2pos(hat - 1)+x2n(hat-1,3)*812;
z2pos(hat) = x2pos(hat - 1)+x2n(hat-1,2)*812;
end
% compare2(W, jat) = sqrt((x(1645)-RANK(1645,1))^2 + (y(1645)-RANK(1645,2))^2 + (z(1645)-RANK(1645,3))^2);
figure(13)
plot3(RANK(:,1),RANK(:,3),RANK(:,2), 'b');
hold on
plot3(x,y,z, 'r');
hold on
plot3(x2pos, y2pos, z2pos, 'g')
grid on
hold off
%Animation for the position of the Foot/IMU
%Plot the initial positions from the VICON
% figure(1)
% stem3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
% hold on
% stem3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
% hold on
% stem3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
% hold off
% grid on
figure(1)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
hold on
plot3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
hold off
grid on
%Turn Accelerations in volts into m/s
g = 9.80665;
Accelerometer = (Accelerometer - 2.5).*g; %in m/s/s
Gyroscope = (Gyroscope - 2.5).*500; %in deg/s
%Sample Autocorrelation functions
%Ax
datAx = Accelerometer(:,1);
N = length(datAx);
acfx = zeros(1,N);
meanx = (1/N)*sum(datAx);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAx(j)-meanx)*(datAx(j+l)-meanx);
end
acfx(l+1) = (1/(N-l))*temp;
end
figure(2)
plot(acfx(1:1600))
%Ay
datAy = Accelerometer(:,2);
N = length(datAy);
acfy = zeros(1,N);
meany = (1/N)*sum(datAy);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAy(j)-meany)*(datAy(j+l)-meany);
end
acfy(l+1) = (1/(N-l))*temp;
end
figure(3)
plot(acfy(1:1600))
%Az
datAz = Accelerometer(:,3);
N = length(datAz);
acfz = zeros(1,N);
meanz = (1/N)*sum(datAz);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datAz(j)-meanz)*(datAz(j+l)-meanz);
end
acfz(l+1) = (1/(N-l))*temp;
end
figure(4)
plot(acfz(1:1600))
%empirical derivation of the autocovariance function
n = (1:1015)/120;
x1 = 30.7;
x2 = 50*sin(3.4*n);
x3 = (50/9)*sin(3.4*3*n);
x4 = (50/25)*sin(3.4*5*n);
x5 = (50/49)*sin(3.4*7*n);
x6 = (50/81)*sin(3.4*9*n);
x7 = (50/(11*11))*sin(3.4*11*n);
y = x1+x2+x3+x5+x6+x7;
figure(2)
hold on
plot(y, 'r')
hold off
%Simple Integration of the accelerometer data / No orientation correction
%or removal of gravity, No Kalman gain
Ts = 1/120;
po = [RANK(1,1) RANK(1,2) RANK(1,3)];
xk = [RANK(1,1) 0 0];
A = [1 Ts (Ts^2)/2;
0 1 Ts;
0 0 1];
% xkp = zeros(length(RANK(:,1)), 3);
% xkp(1,:) = xk;
% for i = 2:length(RANK(:,1))+1
% xkp(i-1, 3) = Accelerometer(i-1,1);
% temp = xkp(i-1, :)';
% xkp(i,:) = A*temp;
% xkp(i,:) = xkp(i,:)';
% end
% x=xkp(:,1);
%
% yk = [RANK(1,2) 0 0];
% A = [1 Ts (Ts^2)/2;
% 0 1 Ts;
% 0 0 1];
% ykp = zeros(length(RANK(:,1)), 3);
% ykp(1,:) = yk;
% for i = 2:length(RANK(:,1))+1
% ykp(i-1, 3) = Accelerometer(i-1,1);
% temp = ykp(i-1, :)';
% ykp(i,:) = A*temp;
% ykp(i,:) = ykp(i,:)';
% end
% y=ykp(:,1);
%
% zk = [RANK(1,3) 0 0];
% A = [1 Ts (Ts^2)/2;
% 0 1 Ts;
% 0 0 1];
% zkp = zeros(length(RANK(:,1)), 3);
% zkp(1,:) = zk;
% for i = 2:length(RANK(:,1))+1
% zkp(i-1, 3) = Accelerometer(i-1,1);
% temp = zkp(i-1, :)';
% zkp(i,:) = A*temp;
% zkp(i,:) = zkp(i,:)';
% end
% z=zkp(:,1);
%Same as above
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*Accelerometer(d-1,1)*Ts^2) (xn(d-1,2)+1000*Accelerometer(d-1,1)*Ts) 1000*Accelerometer(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*Accelerometer(d-1,2)*Ts^2) (yn(d-1,2)+1000*Accelerometer(d-1,2)*Ts) 1000*Accelerometer(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*Accelerometer(d-1,3)*Ts^2) (zn(d-1,2)+1000*Accelerometer(d-1,3)*Ts) 1000*Accelerometer(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(5)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
Ts = 1/120;
lRANK = length(RANK(:,1));
time = 1:lRANK;
%Complete Madgwick filtering to remove the effect of gravity
figure(6);
axis(1) = subplot(2,1,1);
hold on;
plot(time, Gyroscope(:,1), 'r');
plot(time, Gyroscope(:,2), 'g');
plot(time, Gyroscope(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
axis(2) = subplot(2,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Acceleration (g)');
title('Accelerometer');
hold off;
% axis(3) = subplot(3,1,3);
% hold on;
% plot(time, Magnetometer(:,1), 'r');
% plot(time, Magnetometer(:,2), 'g');
% plot(time, Magnetometer(:,3), 'b');
% legend('X', 'Y', 'Z');
% xlabel('Time (s)');
% ylabel('Flux (G)');
% title('Magnetometer');
% hold off;
linkaxes(axis, 'x');
%% Process sensor data through algorithm
AHRS = MadgwickAHRSsimpleBiasCOMP();%('SamplePeriod', 1/fs, 'Beta', 0.1, 'lenavg', 10);
% AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
quaternion = zeros(length(time), 4); quaternion(1, :) = [1 0 0 0];
newGyro = zeros(length(time),3);
for t = 1:length(time)
if t == 1
AHRS.UpdateIMU(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), quaternion(t,:)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
else
AHRS.UpdateIMU(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
end
end
%% Plot algorithm output as Euler angles
% The first and third Euler angles in the sequence (phi and psi) become
% unreliable when the middle angles of the sequence (theta) approaches ±90
% degrees. This problem commonly referred to as Gimbal Lock.
% See: http://en.wikipedia.org/wiki/Gimbal_lock
euler = quatern2euler(quaternConj(quaternion)) * (180/pi); % use conjugate for sensor frame relative to Earth and convert to degrees.
figure(7);
hold on;
plot(time, euler(:,1), 'r');
plot(time, euler(:,2), 'g');
plot(time, euler(:,3), 'b');
title('Euler angles');
xlabel('Time (s)');
ylabel('Angle (deg)');
legend('\phi', '\theta', '\psi');
hold off;
%Using quaternion to transform from the lab frame to the aligned with
%gravity reference frame (so you can subtract out the gravity term later)
AccelerometerRef = zeros(size(Accelerometer));
AccelerometerRef = [AccelerometerRef zeros(lRANK, 1)];
newA = [zeros(lRANK, 1) Accelerometer];
for k = 1:lRANK
AccelerometerRef(k,:) = quaternProd(quaternProd(quaternion(k,:), newA(k,:)), quaternConj(quaternion(k,:)));
end
AccelRef = AccelerometerRef(:,(2:4));
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(8)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Subtracting out the gravity term as [0 0 1]*g from the above section
AccelRef(:,3) = AccelRef(:,3)-9.80665;
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(9)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%Complete Wavelet denoising using Haar wavelet and 5 'averages' and leaving
%out the 5 details
%Plot and show only the x acceleration
Ax = [AccelRef(:,1); zeros(19,1)];
[swa,swd] = swt(Ax,5,'db1');
[thr,sorh] = ddencmp('den','wv',Ax);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
figure(10)
subplot(2,1,1), plot(Ax); title('Original signal'); grid on
subplot(2,1,2), plot(clean); title('denoised signal'); grid on
%Now complete the wavelet filtering on the accelerations
Ax = clean(1:lRANK);
%Acc. in Y
Ay = [AccelRef(:,2); zeros(19,1)];
[swa,swd] = swt(Ay,5,'db1');
[thr,sorh] = ddencmp('den','wv',Ay);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
Ay = clean(1:lRANK);
%Acc. in Z
Az = [AccelRef(:,3); zeros(19,1)];
[swa,swd] = swt(Az,5,'db1');
[thr,sorh] = ddencmp('den','wv',Az);
dswd = wthresh(swd,sorh,thr);
clean = iswt(swa,dswd,'db1');
Az = clean(1:lRANK);
AccelRef = [Ax' Ay' Az'];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Redo the state updates with the new values of acceleration
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Plot RANK and this crude approximation
figure(11)
plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
hold on
plot3(x,y,z, 'r');
grid on
hold off
%Determine the Autocovariance for each of the gyroscope axes
%Establish the number of samples where there is significant correlation
%Determine the threshold from the data for very slow walking by iterating
%until the error length (final point vicon vs final point IMU) is minimized
%Gx, Gy, Gz
datGx = newGyro(:,1);
N = length(datGx);
acfGx = zeros(1,N);
meanGx = (1/N)*sum(datGx);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGx(j)-meanGx)*(datGx(j+l)-meanGx);
end
acfGx(l+1) = (1/(N-l))*temp;
end
datGy = newGyro(:,2);
N = length(datGy);
acfGy = zeros(1,N);
meanGy = (1/N)*sum(datGy);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGy(j)-meanGy)*(datGy(j+l)-meanGy);
end
acfGy(l+1) = (1/(N-l))*temp;
end
datGz = newGyro(:,3);
N = length(datGz);
acfGz = zeros(1,N);
meanGz = (1/N)*sum(datGz);
temp = 0;
for l = 0:(N-1)
for j = 1:(N-l)
temp = temp + (datGz(j)-meanGz)*(datGz(j+l)-meanGz);
end
acfGz(l+1) = (1/(N-l))*temp;
end
figure(12)
plot(acfGx(1:1600))
grid on
hold on
plot(acfGy(1:1600), 'ro')
plot(acfGz(1:1600), 'g*')
hold off
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%After looking at the amount of time (or number of samples it takes to
%accurately determine a zero velocity - 9 samples - Apply the average
%rotational energy algorithm to zero out the bias, and optimize for the
%slowest speed walk
%For loop for optimizing the threshold detection on the angular rate energy
%algorithm
% jake = 5e5:10000:1e6;
% compare2 = zeros(5, length(jake));
%Threshold parameter
temp =[];
W = 1; %window size
gamma=990000; %threshold statistic for determining ZV
[rows, cols] = size(Gyroscope);
new_Gyro = zeros(size(Gyroscope));
checkZero = zeros(1,length(time));
T = AREdetector(Gyroscope, W);
for i = 1:length(T) %for loop for implementing the ZUPT detection
if T(i) < gamma
new_Gyro(i, 1:3) = [0 0 0];
checkZero(i) = 1; %1 means reset the accel
else
new_Gyro(i, 1:3) = Gyroscope(i, 1:3);
checkZero(i) = 0;
end
end
for i = length(T)+1:length(time)
new_Gyro(i, 1:3) = Gyroscope(i, 1:3);
checkZero(i) = 0;
end
figure(15)
plot(Gyroscope(:,3))
hold on
stem(new_Gyro(:,3),'g')
hold off
%Determine bias during each of the zero velocity periods and
%subtract from future points
%I just used the first 290 points to do this, then subtract from
%future points between ZUPT periods although it can be done for
%each period
% meanAx = mean(Accelerometer(1:290,1));
% meanAz = mean(Accelerometer(1:290,2));
% meanAy = mean(Accelerometer(1:290,3));
meanGx = mean(Accelerometer(1:290,1));
meanGz = mean(Accelerometer(1:290,2));
meanGy = mean(Accelerometer(1:290,3));
% Accelerometer(:,1)=Accelerometer(:,1)-meanAx;
% Accelerometer(:,2)=Accelerometer(:,2)-meanAz;
% Accelerometer(:,3)=Accelerometer(:,3)-meanAy;
Gyroscope(:,1)=Accelerometer(:,1)-meanGx;
Gyroscope(:,2)=Accelerometer(:,2)-meanGz;
Gyroscope(:,3)=Accelerometer(:,3)-meanGy;
AHRS = MadgwickAHRSsimpleBiasCOMP();%('SamplePeriod', 1/fs, 'Beta', 0.1, 'lenavg', 10);
% AHRS = MahonyAHRS('SamplePeriod', 1/256, 'Kp', 0.5);
quaternion = zeros(length(time), 4); quaternion(1, :) = [1 0 0 0];
newGyro = zeros(length(time),3);
for t = 1:length(time)
if t == 1
AHRS.UpdateIMU(new_Gyro(t,:) * (pi/180), Accelerometer(t,:), quaternion(t,:)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
else
AHRS.UpdateIMU(new_Gyro(t,:) * (pi/180), Accelerometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
%AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:), quaternion((t-1), :)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
newGyro(t, :) = AHRS.Gyr;
end
end
AccelerometerRef = [zeros(size(Accelerometer)) zeros(lRANK, 1)];
newA = [zeros(lRANK, 1) Accelerometer];
for k = 1:lRANK
AccelerometerRef(k,:) = quaternProd(quaternProd(quaternion(k,:), newA(k,:)), quaternConj(quaternion(k,:)));
end
figure(18)
plot(Accelerometer(:,1))
hold on
stem(AccelerometerRef(:,2), 'r')
hold off
AccelRef = AccelerometerRef(:,(2:4));
% AccelRef = Accelerometer;
figure(16)
plot(Accelerometer(:,2))
% AccelRef(:,3) = AccelRef(:,3)-9.80665;
hold on
stem(AccelerometerRef(:,3), 'g')
%Applying ZUPT to the accelerations
for jade = 1:length(checkZero)
if checkZero(jade) == 1;
AccelRef(jade, :) = 0;
end
end
hold on
stem(AccelRef(:,2), 'r')
hold off
% AccelRef(:,2) = AccelRef(:,2)-9.80665;
xn(1,:) = [RANK(1,1) 0 0];
for d = 2:length(RANK(:,1))
xn(d,:) = [(xn(d-1,1)+Ts*xn(d-1,2)+(1/2)*1000*AccelRef(d-1,1)*Ts^2) (xn(d-1,2)+1000*AccelRef(d-1,1)*Ts) 1000*AccelRef(d,1)];
end
x = xn(:,1);
yn(1,:) = [RANK(1,3) 0 0];
for d = 2:length(RANK(:,1))
yn(d,:) = [(yn(d-1,1)+Ts*yn(d-1,2)+(1/2)*1000*AccelRef(d-1,2)*Ts^2) (yn(d-1,2)+1000*AccelRef(d-1,2)*Ts) 1000*AccelRef(d,2)];
end
y = yn(:,1);
zn(1,:) = [RANK(1,2) 0 0];
for d = 2:length(RANK(:,1))
zn(d,:) = [(zn(d-1,1)+Ts*zn(d-1,2)+(1/2)*1000*AccelRef(d-1,3)*Ts^2) (zn(d-1,2)+1000*AccelRef(d-1,3)*Ts) 1000*AccelRef(d,3)];
end
z = zn(:,1);
%Applying a minimum mean square error filter using the average
%length of a person's step, and assuming a linear trajectory and
%same average step size
x2 = zeros(1, floor(1645/220));
y2 = zeros(1, floor(1645/220));
z2 = zeros(1, floor(1645/220));
xvec = [0 1 0 0];
yvec = [0 0 1 0];
zvec = [0 0 0 1];
%translate into the new orientation for each of the points
for to = 1:floor(1645/220)
x2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), xvec), quaternConj(quaternion(220*to-219,:)));
y2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), yvec), quaternConj(quaternion(220*to-219,:)));
z2new(to,:) = quaternProd(quaternProd(quaternion(220*to-219,:), zvec), quaternConj(quaternion(220*to-219,:)));
end
for to = 1:floor(1645/220)
x2n(to,:) = x2new(to, 2:4);
y2n(to,:) = y2new(to, 2:4);
z2n(to,:) = z2new(to, 2:4);
end
x2pos(1) = RANK(1,1);
y2pos(1) = RANK(1,3);
z2pos(1) = RANK(1,2);
for hat = 2:floor(1645/220)
x2pos(hat) = x2pos(hat - 1)+x2n(hat-1,1)*812;
y2pos(hat) = x2pos(hat - 1)+x2n(hat-1,3)*812;
z2pos(hat) = x2pos(hat - 1)+x2n(hat-1,2)*812;
end
% compare2(W, jat) = sqrt((x(1645)-RANK(1645,1))^2 + (y(1645)-RANK(1645,2))^2 + (z(1645)-RANK(1645,3))^2);
figure(13)
plot3(RANK(:,1),RANK(:,3),RANK(:,2), 'b');
hold on
plot3(x,y,z, 'r');
hold on
plot3(x2pos, y2pos, z2pos, 'g')
grid on
hold off
Code for reading in/ plotting IMU/VICON data
Copy paste into vector - script for VICON:
%Trial 8 - Slow Speed with Obstacle
RANK = [767.438599 -2699.683594 93.373024
767.387817 -2699.699219 93.380745
767.429565 -2699.683838 93.36335
767.417419 -2699.652344 93.360855
767.422363 -2699.650391 93.357536
767.412659 -2699.652832 93.361465
767.356201 -2699.651367 93.399445
767.333435 -2699.671143 93.371094
Code for plotting:
%Plotting the IMU data
%Turn Accelerations in volts into m/s
g = 9.80665;
Accelerometer = (Accelerometer - 2.5).*g; %in m/s/s
Gyroscope = (Gyroscope - 2.5).*500; %in deg/s
time = (1:length(Gyroscope(:,1)))*(1/120);
figure('Name', 'Sensor Data');
axis(1) = subplot(2,1,1);
hold on;
plot(time, Gyroscope(:,1), 'r');
plot(time, Gyroscope(:,2), 'g');
plot(time, Gyroscope(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
axis(2) = subplot(2,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Acceleration (m/s)');
title('Accelerometer');
hold off;
% axis(3) = subplot(3,1,3);
% hold on;
% plot(time, Magnetometer(:,1), 'r');
% plot(time, Magnetometer(:,2), 'g');
% plot(time, Magnetometer(:,3), 'b');
% legend('X', 'Y', 'Z');
% xlabel('Time (s)');
% ylabel('Flux (G)');
% title('Magnetometer');
% hold off;
linkaxes(axis, 'x');
% figure(2)
% plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
% hold on
% plot3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
% hold on
% plot3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
% hold off
% grid on
%Trial 8 - Slow Speed with Obstacle
RANK = [767.438599 -2699.683594 93.373024
767.387817 -2699.699219 93.380745
767.429565 -2699.683838 93.36335
767.417419 -2699.652344 93.360855
767.422363 -2699.650391 93.357536
767.412659 -2699.652832 93.361465
767.356201 -2699.651367 93.399445
767.333435 -2699.671143 93.371094
Code for plotting:
%Plotting the IMU data
%Turn Accelerations in volts into m/s
g = 9.80665;
Accelerometer = (Accelerometer - 2.5).*g; %in m/s/s
Gyroscope = (Gyroscope - 2.5).*500; %in deg/s
time = (1:length(Gyroscope(:,1)))*(1/120);
figure('Name', 'Sensor Data');
axis(1) = subplot(2,1,1);
hold on;
plot(time, Gyroscope(:,1), 'r');
plot(time, Gyroscope(:,2), 'g');
plot(time, Gyroscope(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Angular rate (deg/s)');
title('Gyroscope');
hold off;
axis(2) = subplot(2,1,2);
hold on;
plot(time, Accelerometer(:,1), 'r');
plot(time, Accelerometer(:,2), 'g');
plot(time, Accelerometer(:,3), 'b');
legend('X', 'Y', 'Z');
xlabel('Time (s)');
ylabel('Acceleration (m/s)');
title('Accelerometer');
hold off;
% axis(3) = subplot(3,1,3);
% hold on;
% plot(time, Magnetometer(:,1), 'r');
% plot(time, Magnetometer(:,2), 'g');
% plot(time, Magnetometer(:,3), 'b');
% legend('X', 'Y', 'Z');
% xlabel('Time (s)');
% ylabel('Flux (G)');
% title('Magnetometer');
% hold off;
linkaxes(axis, 'x');
% figure(2)
% plot3(RANK(:,1),RANK(:,2),RANK(:,3), 'b');
% hold on
% plot3(RHEEL(:,1),RHEEL(:,2),RHEEL(:,3), 'r');
% hold on
% plot3(RTOE(:,1),RTOE(:,2),RTOE(:,3), 'g');
% hold off
% grid on
Subscribe to:
Posts (Atom)