Rotating Accel Data

[vitorsotero]

Hi,

I am trying to rotate accelerometer data using quaternion.

But when I use your Madgick script, I get somo confusing results. My figures: "raw" are the Accel, Gyr and Mag datas (all calibrated). "rotated1" and "rotated2" are the accel data rotate, with the difference on the line: rotM(:,:,i) = quatern2rotMat(quaternion(i,:)); and rotM(:,:,i) = quatern2rotMat(quaternion(i,:))'; (a quote).

Can you help me to understand what I am doing wrong?

Thanks,

Vitor Sotero

`function flora_Accel()
addpath('quaternion_library'); % include quaternion library

Freq = 200; %frequência de aquisição
%beta = 0.5;
GyroMeasError = pi * (40.0 / 180.0);
beta = sqrt(3.0 / 4.0) * GyroMeasError;
%MinPeakHeight=0; % higher noise
MinPeakHeight=0.05; % higher noise
limT=0.1; % minimal duration limit
%limA=0; % minimal accel limit ('mean noise')
limA=0.01; % minimal accel limit ('mean noise')
limD=0.02; % minimal displacement limit
limV=0.01; % minimal velocity limit
cutI = 1;

filename = 'dataFlora.txt';
fileID = fopen(filename);

axis=textscan(fileID,'%s %n %n %n %s %n %n %n %s %n %n %n','delimiter',',','headerlines',5);
fclose(fileID);

xAccel = axis{1,2};
yAccel = axis{1,3};
zAccel = axis{1,4};

xMag = axis{1,6};
yMag = axis{1,7};
zMag = axis{1,8};

xGyr = axis{1,10};
yGyr = axis{1,11};
zGyr = axis{1,12};

xAccel = xAccel(cutI:length(zGyr)-1); %Cortar início e final.
yAccel = yAccel(cutI:length(zGyr)-1);
zAccel = zAccel(cutI:length(zGyr)-1);

xMag = xMag(cutI:length(zGyr)-1);
yMag = yMag(cutI:length(zGyr)-1);
zMag = zMag(cutI:length(zGyr)-1);

xGyr = xGyr(cutI:length(xAccel));
yGyr = yGyr(cutI:length(xAccel));
zGyr = zGyr(cutI:length(xAccel));

t=length(xAccel)/Freq;
t=linspace(0,t,length(xAccel));
time=t';

Accelerometer =zeros(length(xAccel),3);
Accelerometer(:,1) = xAccel;
Accelerometer(:,2) = yAccel;
Accelerometer(:,3) = zAccel;

Magnetometer = zeros(length(xAccel),3);
Magnetometer(:,1) = xMag;
Magnetometer(:,2) = yMag;
Magnetometer(:,3) = zMag;

Gyroscope = zeros(length(xAccel),3);
Gyroscope(:,1) = xGyr;
Gyroscope(:,2) = yGyr;
Gyroscope(:,3) = zGyr;

% %% Plot sensor data
%
% figure('Name', 'Sensor Data');
% axes(1) = subplot(3,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;
% axes(2) = subplot(3,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;
% axes(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(axes, 'x');

AHRS = MadgwickAHRS('SamplePeriod', 1/Freq, 'Beta', beta);

quaternion = zeros(length(time), 4);
for t = 1:length(time)
AHRS.Update(Gyroscope(t,:) * (pi/180), Accelerometer(t,:), Magnetometer(t,:)); % gyroscope units must be radians
quaternion(t, :) = AHRS.Quaternion;
end

for i=1:length(xAccel)
rotM(:,:,i) = quatern2rotMat(quaternion(i,:)); % create rotation matrix
end

% quatrotate(quaternion,Accelerometer);
% rotM = quatern2rotMat(quaternion); % create rotation matrix

for r=1:length(xAccel) %rotate Accelerometer Data
xAccel_R(r) = rotM(1,1,r)*xAccel(r) + rotM(1,2,r)*yAccel(r) + rotM(1,3,r)*zAccel(r);
yAccel_R(r) = rotM(2,1,r)*xAccel(r) + rotM(2,2,r)*yAccel(r) + rotM(2,3,r)*zAccel(r);
zAccel_R(r) = rotM(3,1,r)*xAccel(r) + rotM(3,2,r)*yAccel(r) + rotM(3,3,r)*zAccel(r);
end

[b_lp,a_lp] = butter(4, 10 / Freq/2, 'low'); %low pass filter
[b_hp,a_hp] = butter(1, 0.1/(Freq/2), 'high'); %high pass filter

xAccel_R = filtfilt(b_lp,a_lp, xAccel_R);
xAccel_R = xAccel_R - mean(xAccel_R);
%xAccel = filtfilt(b_hp,a_hp, xAccel);
xAccel_R=xAccel_R-xAccel_R(1);
yAccel_R = filtfilt(b_lp,a_lp, yAccel_R);
yAccel_R = yAccel_R - mean(yAccel_R);
%yAccel = filtfilt(b_hp,a_hp, yAccel);
yAccel_R=yAccel_R-yAccel_R(1);
zAccel_R = filtfilt(b_lp,a_lp, zAccel_R);
zAccel_R = zAccel_R - mean(zAccel_R);
%zAccel = filtfilt(b_hp,a_hp, zAccel);
zAccel_R=zAccel_R-zAccel_R(1);

[sIx,sFx,xVel,xCoord]=segmentAccel_teste(time,xAccel,limT,limA,limV,MinPeakHeight,b_hp,a_hp,1);
[sIy,sFy,yVel,yCoord]=segmentAccel_teste(time,yAccel,limT,limA,limV,MinPeakHeight,b_hp,a_hp,2);
[sIz,sFz,zVel,zCoord]=segmentAccel_teste(time,zAccel,limT,limA,limV,MinPeakHeight,b_hp,a_hp,3);
end`

[xioTechnologies]

You do not seem to be reporting an issue. Please contact x-io via the website if you have a specific question.