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
Sunday, April 26, 2015
Rosbagging into Text Files
http://answers.ros.org/question/11330/how-can-i-use-bag-file-data-in-matlab/
Saturday, April 11, 2015
Final IMU Debugging Useful Links
More useful links that helped me eventually get the IMU working with ROS on the BBB.
-Ted
Awesome introduction to using the BBB for I2C among many other things. Con is that he is using a Beagleboard or not the Black so the instructions and code are actually dated.
http://derekmolloy.ie/beaglebone/beaglebone-an-i2c-tutorial-interfacing-to-a-bma180-accelerometer/
Guy who had similar problems as I did, and helped stimulate thoughts. Still wasn't the solution.
http://electronics.stackexchange.com/questions/144674/if-an-i2c-device-does-not-appear-in-i2cdetect-does-it-mean-the-device-is-potenti
Another great resource that helped stimulate thoughts, but did not fix my problem. Has a more updated walk through as compared to Molloy.
http://beaglebone.cameon.net/home/i2c-devices
-Ted
Awesome introduction to using the BBB for I2C among many other things. Con is that he is using a Beagleboard or not the Black so the instructions and code are actually dated.
http://derekmolloy.ie/beaglebone/beaglebone-an-i2c-tutorial-interfacing-to-a-bma180-accelerometer/
Guy who had similar problems as I did, and helped stimulate thoughts. Still wasn't the solution.
http://electronics.stackexchange.com/questions/144674/if-an-i2c-device-does-not-appear-in-i2cdetect-does-it-mean-the-device-is-potenti
Another great resource that helped stimulate thoughts, but did not fix my problem. Has a more updated walk through as compared to Molloy.
http://beaglebone.cameon.net/home/i2c-devices
Friday, April 10, 2015
VICON Testing 1
Today we tested a synchronized IMU with the VICON and there are 8 trials with the data on the link below:
https://drive.google.com/a/case.edu/folderview?id=0B3ZJVS4arjDOfkcySzFrNEhzQlFqQ044eThFQ1BadWZBMHNkLVVZU0xRSzEwMkZDcW4yZ2c&usp=sharing
https://drive.google.com/a/case.edu/folderview?id=0B3ZJVS4arjDOfkcySzFrNEhzQlFqQ044eThFQ1BadWZBMHNkLVVZU0xRSzEwMkZDcW4yZ2c&usp=sharing
Friday, March 6, 2015
Gantt Chart Updated in Smartsheets
This is the link to the smartsheet:
https://app.smartsheet.com/b/home
https://app.smartsheet.com/b/home
Position Tracking with Inertial Measurement Units Midterm Report
Position
Tracking with Inertial Measurement Units
By:
Theodore Nowak and Alex Haufler
Abstract
There are many factors
that differentiate commercially available IMUs such as absolute
accuracy, drift, bias stability, inherent
noise, and vibration rejection. The IMUs currently
deployed in sensitive environments can
cost tens of thousands of dollars, while the IMUs in
phones and tablets cost fractions of a
dollar. In order to compensate for the different factors
affecting the accuracy of the
measurements, various algorithms and measurement schemes have
been developed. Also, once the measurements
have been made and adverse factors
compensated for, there are various
algorithms for filtering the data so that it better represents the
actual acceleration, angular rate, and
magnetic orientation.
That
being said, the end goal of our project is to create a system that will track
multiple IMUs simultaneously, test multiple correctional algorithms on this
data, and then compare the results with the VICON system at the Veteran’s
Affairs Hospital in their gait analysis lab. Last semester we worked to test
the differences between IMUs (cost, DOF, etc.) and also between many of the
leading filtering techniques used in the field. This semester we will begin to
integrate this knowledge into further investigation and the beginnings of a
working product.
Distribution
of Labor
Ted is responsible for creating
an interface from which to record from the IMUs using ROS and a Beaglebone
Black. This will involve installing Linux, ROS, and any necessary packages on
the Beaglebone Black, and also creating executable code in C++ to record from
the IMU. This code must be able to record from multiple IMUs at once at equivalent,
programmable frequencies. He has been assigned this task because he has the
most experience both with ROS and C++. In addition to any work involving ROS,
Ted will also be responsible for understanding Kalman-Bucy filtering in full.
Last semester our previous partner Emeline investigated this topic, but was
unable to flush out some aspects of the filter. Ted will aim to start where she
left off and work with Professor Loparo to fully understand the filter.
Alex Haufler is a senior
electrical engineering major with ample experience in signal processing and
analysis, and in embedded systems design. His main focus is on understanding
the theory and implementation associated with each of the separate filtering
techniques relevant to denoising, determining orientation, rejecting bias, and
estimating position. Then, with a complete understanding of the separate
pieces, Alex will develop custom algorithms involving a combination of the
various filtering methods.
Both Alex and Ted have made sufficient progress thus far.
Ted has already installed Linux and ROS on the Beaglebone Black and will soon
begin drafting code to record from the MPU6050’s. Similarly, Alex has already
read through five chapters in a wavelets textbook and skimmed chapters in a few
others. As of now, the project is progressing on time.
Technical
Challenges
Creating the interface
from which to record from the IMUs is riddled with challenges. Firstly, there
are limitations within the version of Linux on the Beaglebone Black. Because of
this, there are some differences that might necessitate all communication with
the Beaglebone Black to be done through SSH. Similarly, all code written will
need to be transferred through Github. Other hurdles include properly altering
existing code to work on ROS, and finding how to instantiate three IMUs to
function simultaneously at similar frequencies. These problems will be overcome
through careful setup of file structures and through adequate research. Ideally
other software engineers will have had similar experience from which we may
learn.
Position estimation from
inertial measurements has a number of obstacles to achieving a high degree of
accuracy. Since inertial measurements are accelerations or angular velocities,
double integration and integration respectively is necessary for obtaining an
estimate of position and orientation. First, since the measurements are in
discrete time, an approximation has to be used for the integrations. A higher
sampling rate could aid in the approximation. However, if there is any
deviation from the true values of acceleration and angular velocity, the effect
of integrating twice will mean something of a quadratic growth in error with
time. Deviations from the true values include bias in the measurements from the
MEMS devices, drift of this bias with temperature and time, noise that includes
vibrations from external sources, and incorrect orientation estimation. The
incorrect orientation estimation has to do with gravity, which is registered as
an acceleration, and if the direction is not known exactly there will be an
incorrect acceleration measurement along the different axes. As far as
technical challenges associated with the algorithms are concerned, wavelets and
Hilbert-Huang transforms to be used in denoising the data are fairly
complicated and will take more time to fully understand and implement.
There are many pertinent
numerical methods and filtering techniques to investigate and amalgamate such
as least-squares spline interpolation, wavelets, Hilbert-Huang transforms,
zero-velocity update aided filtering, Madgwick’s filter, Mahoney’s filter, and
Kalman-Bucy filtering. There are more, but these are the most interesting due
to their current development and relevance to the project. In order to learn more
about the current state of the art associated with wavelets, Alex will continue
to work through the books he has checked out of the library, in addition to
consulting Professor Loparo and Professor Buchner, and reading papers,
articles, etc. Once a thorough understanding of the subject has been obtained,
he will implement different methods using wavelets in Matlab that have shown
success, perhaps in other applications such as ECG analysis, and test them
against the benchmark data from the VICON system. Wavelets are to be used in
denoising, but Alex will take the same approach with different algorithms and
ideas applied to bias/drift rejection, orientation estimation, and integration
approximations.
Specifications
Due to the nature of
this project, it is much more reasonable to think in terms of goals rather than
in terms of specifications. They are as follows: firstly, a working platform
using ROS and the Beaglebone Black on which one can record from three MPU6050’s
simultaneously and at preordained frequencies; secondly, to study various
different filters for denoising, determining orientation, rejecting bias, and
estimating position; thirdly, to combine these techniques into one optimized
methodology for filtering the MPU6050 data; and, lastly, to compare these
results with the “golden standard,” the VICON system. These results will be
verified and confirmed throughout the course of the project. The Beaglebone
Black test-base, will be physical in nature, and therefore validated through
visualization and through its effectiveness in yielding results. Then, throughout
the course of testing each filtering algorithm, we will generate many plots with
the data from the Beaglebone Black either validating or disproving the efficacy
of the filter. These plots can be included in the final write up to prove the
satisfaction of aforementioned goals. Lastly, the VICON system will yield data from
which we will compare the mean squared error of our filters and VICON against
each other. Through these comparisons we will determine which of our filters
was most effective. While the nature of this project makes it difficult to
quantify results, our end results should blend a healthy mix of visual
confirmation and algorithmic comparison to satisfy the prudent analyst.
Gantt
Chart
Monday, February 16, 2015
Tuesday, February 10, 2015
Kalman-Bucy filtering
A great introduction to the derivation and application:
http://www.academia.edu/1512888/Introduction_to_the_Kalman_Filter_and_its_Derivation
Related Math:
http://math.stackexchange.com/questions/622195/taking-a-derivative-with-respect-to-a-matrix
http://www.academia.edu/1512888/Introduction_to_the_Kalman_Filter_and_its_Derivation
Related Math:
http://math.stackexchange.com/questions/622195/taking-a-derivative-with-respect-to-a-matrix
EMD and Hilbert-Huang Transform
Original Paper:
http://tec.earth.sinica.edu.tw/research/report/paper/20070711HHT.pdf
A related gait study using emd applied to gyroscope data:
http://tec.earth.sinica.edu.tw/research/report/paper/20070711HHT.pdf
A related gait study using emd applied to gyroscope data:
"Integration of Human Walking Gyroscopic Data Using Empirical Mode Decomposition" http://www.mdpi.com/1424-8220/14/1/370
A comparison of EMD and Wavelets (and the fusion of both) in data analysis:
http://www.arpapress.com/volumes/vol11issue3/ijrras_11_3_16.pdf
Madgwick and Mahoney Filter
Madgwick Filter
http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
Mahoney Filter in the context of modern filtering approaches
http://arxiv.org/pdf/1110.0274.pdf
Mahoney's complete filter:
https://hal-unice.archives-ouvertes.fr/hal-00488376/document
http://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf
Mahoney Filter in the context of modern filtering approaches
http://arxiv.org/pdf/1110.0274.pdf
Mahoney's complete filter:
https://hal-unice.archives-ouvertes.fr/hal-00488376/document
Quaternion (Background) and interpolation
This is a good source on the basics of quaternion math and interpolation:
http://www.geometrictools.com/Documentation/Quaternions.pdf
An interesting paper on "Spherical Averages and Applications to Spherical Splines and Interpolation"
http://www.math.ucsd.edu/~sbuss/ResearchWeb/spheremean/paper.pdf
http://www.geometrictools.com/Documentation/Quaternions.pdf
An interesting paper on "Spherical Averages and Applications to Spherical Splines and Interpolation"
http://www.math.ucsd.edu/~sbuss/ResearchWeb/spheremean/paper.pdf
Wavelets (Background)
Overview and general approach:
http://www.eecis.udel.edu/~amer/CISC651/IEEEwavelet.pdf
For in-depth treatment of wavelets, I have checked out the following book from ksl and have made significant progress:
http://www.eecis.udel.edu/~amer/CISC651/IEEEwavelet.pdf
For in-depth treatment of wavelets, I have checked out the following book from ksl and have made significant progress:
Computational signal processing with wavelets
ROS and UbuntuARM links
In order to install UbuntuARM on the Beaglebone Black we hope to use for recording, I have found several helpful links to decide: which version of UbuntuARM to use, which versions UbuntuARM are compatible with which versions of ROS, and lastly some already made nodes to record from the MPU 6050.
http://beagleboard.org/getting-started
https://github.com/ggallin08/crwucutter_core_bbb
http://wiki.ros.org/indigo/Installation/UbuntuARM
http://wiki.ros.org/hydro/Installation/UbuntuARM
http://beagleboard.org/getting-started
https://github.com/ggallin08/crwucutter_core_bbb
http://wiki.ros.org/indigo/Installation/UbuntuARM
http://wiki.ros.org/hydro/Installation/UbuntuARM
Subscribe to:
Posts (Atom)