Relative rotation between two IMU's

I'm trying to obtain the relative rotation between two IMU's.

In order to verify the quality of the orientation delivered by the IMU (quaternions), I attached both IMUs to a rigid body (bar) but placed in a different orientation.

I recording of 1 min data for a rotation of the system (2 IMUs attached to the bar) in slow motion.

Both IMUs should give the same orientations after aligning their frames or in other words the relative rotation between both should be constant.

So, to align the frames, i first take q_start of both (imu1 and 2) in the first 5s when the system was at rest on the table:

q_start1 = mean(Q1(t0->5s));

q_start2 = mean(Q2(t0->5s));

Now in order to express their frames in world frame:

Q1_w = Q1 * Inverse(q_start1);

Q2_w = Q2 * Inverse(q_start2);

And to obtain the relative rotation between them:

Q_relative_w= Inverse(Q1_w) * Q2_w;

But Q_relative was not constant but if I switch the 3rd and – 2nd component of Q2_w that’s mean:

Q2_w = [w,x,y,z] -> Q2_w_new = [w,-y,x,z]

Then i apply: Q_ relative_w_new = Inverse(Q1_w) * Q2_w_new;

I obtain a relative constant Q_ relative_w_new.

My Question:

Where is my error in the calculation or if this calibration makes sense at all?

If not how can I otherwise align the frames?

[Image Analyst]

Hard to visualize without some diagram to explain things.

[James Tursa]

The very first thing you need to do is verify the convention of the quaternions you are using. Are you sure they are scalar-vector order as you seem to be assuming? And are you sure they represent coordinate system transformation quaternions? Are these quaternions an output of the IMU or are you constructing them manually somehow?

Side note: You should normalize your mean quaternions before using them downstream. E.g.,

 q_start1 = mean(Q1(t0->5s));  q_start1 = q_start1 / norm(q_start1);
 q_start2 = mean(Q2(t0->5s));  q_start2 = q_start2 / norm(q_start2);

Answer 1

[James Tursa]

Assuming the coordinate frames are as follows:

ECI, the world frame

BODY1, the IMU1 body frame

BODY2, the IMU2 body frame

Examining your quaternion arithmetic:

q_start1 = mean(Q1(t0->5s)); % ECI->BODY1_t0

q_start2 = mean(Q2(t0->5s)); % ECI->BODY2_t0

% (NOTE: The above should be normalized)

Now in order to express their frames in world frame:

Q1_w = Q1 * Inverse(q_start1); % ECI->BODY1_t * inverse(ECI->BODY1_t0) = ECI->BODY1_t * BODY1_t0->ECI

Q2_w = Q2 * Inverse(q_start2); % ECI->BODY2_t * inverse(ECI->BODY2_t0) = ECI->BODY2_t * BODY2_t0->ECI

And to obtain the relative rotation between them:

Q_relative_w= Inverse(Q1_w) * Q2_w; % inverse(ECI->BODY1_t * BODY1_t0->ECI) * (ECI->BODY2_t * BODY2_t0->ECI)

Expanding that last one:

(ECI->BODY1_t0 * BODY1_t->ECI) * (ECI->BODY2_t * BODY2_t0->ECI) =

ECI->BODY1_t0 * (BODY1_t->ECI * ECI->BODY2_t) * BODY2_t0->ECI =

ECI->BODY1_t0 * (BODY1_t->BODY2_t) * BODY2_t0->ECI

So, you have what I think you are after, BODY1_t->BODY2_t, wrapped inside of other ECI to BODY stuff. I think what you should be after is just the BODY1_t->BODY2_t stuff. E.g.,

q_t0 = inverse(q_start1) * q_start2; % inverse(ECI->BODY1_t0) * ECI->BODY2_t0 = BODY1_t0->BODY2_t0

Then calculate this downstream:

q_t = inverse(Q1) * Q2; % inverse(ECI->BODY1_t) * ECI->BODY2_t = BODY1_t->BODY2_t

And see if all of the q_t calculations seems to be fairly constant.

For a discussion of MATLAB quaternion conventions (you need to know this in order to do the quaternion arithmetic correctly), see these links:

https://www.mathworks.com/matlabcentral/answers/465053-rotation-order-of-quatrotate

https://www.mathworks.com/matlabcentral/answers/352465-what-is-the-aerospace-blockset-quaternion-convention

https://www.mathworks.com/matlabcentral/answers/95432-why-is-the-euler-output-of-the-quaternion-block-different-than-the-euler-angles-block-in-the-aerospa

https://www.mathworks.com/matlabcentral/answers/524471-what-is-the-convention-of-the-new-quaternion-class-introduced-in-r2018b

[hichem mestiri]

Thank you very much for your answer and sorry for the late reply i was very busy.

So for the first point, ''convention'', i didn't even know that there is many type of quaternion. I take a look at the data sheet of the sensor, it does not mention which convention it is used but I suppose that scalar-vector because I tried with matlab imufilter and they gave the same quaternion. So i know now that the quaternion from sensor are calculated with game rotation vector (without magnetometer).

Second point, i normalize the quaternion every time like you sayed.

And third point, you are right i dont need to rotate the sensor frames, I just need to apply directly q_t = inverse(Q1) * Q2; and i should see q_t constant. I tried this but q_t is not constant. I tried also the same thing but with the quaternion calculated with matlab and it's also not constant.

you can find attached the .zip file containing the matlab code, the sensor data and an image of the system.

Thaks a lot.

[James Tursa]

In the image, does the screw in the middle stay tight and fixed? I.e., does the middle section remain at a fixed angle the entire time while you are collecting data?

What are the units of timestamp? Seconds?

What type of motion did you do with this board?

[hichem mestiri]

Yes absolutely, the screw in the middle remains tight and fixed for this data and the central section remains at a fixed angle all the time. I actually wanted to attach a video instead of the image but it was too large for an attachment

[James Tursa]

What are a1, a2, m1, m2, w1, w2 ... did you calculate them or are they coming from the IMU? The w1 and w2 look like angular rate vectors, or perhaps delta angle vectors.

Is there any description in the IMU data sheet that would help with this?

[hichem mestiri]

a1 and a2 are the accelerations comming from the imus 1 and 2 in g. w1 and w2 angular velocity in deg/s and m1, m2 are the magnetometer readings in µT.

[James Tursa]

You can see that the magnitudes of w1 and w2 are the same, so they are seeing the same magnitude of rotation. Also, I can back out the w1 and w2 vectors numerically from the quaternion sets and they match. But the IMU's were not aligned to begin with, so they apparently are not using the same inertial frame, which means my analyses above isn't quite correct. You should be able to compare the w1 and w2 to get that alignment, but I haven't been able to get that to work yet. I will continue to look at this. Do you have any more data sets, or just the one you posted?

[James Tursa]

So I have some results for you. The reason the alignment calculations were not working out for me was because of bad data. Once I replaced the bad data and normalized all the quaternions I seem to be getting somewhat consistent results now. I also had to assume the Left Chain convention for your quaternions to get things to work out. The basic calculations go as follows:

Q1 = Inertial1 -> Body1

Q2 = Inertial2 -> Body2

Since Q1 and Q2 are starting from arbitrary orientations we need to find that alignment between Inertial1 and Inertial2. I did that by using Matt J's absor function from the FEX with the derived rate vectors as inputs (bad data removed):

https://www.mathworks.com/matlabcentral/fileexchange/26186-absolute-orientation-horn-s-method

Then you can get things to work out and the relative delta quaternion calculations as follows seems to be fairly constant (subject to the noisy data and I assume the looseness of the tape in your setup) using the Left Chain convention:

Q2 * Q * Q1^-1 = (Inertial2 -> Body2) * (Inertial1 -> Inertial2) * (Body1 -> Inertial1)

= Body1 -> Body2

where Q is the relative alignment estimated by the absor function above.

The code is listed below. The plots are:

figure 1: The derived w1 rate from Q1 differencing (matches figure 2)

figure 2: The reported w1 rate from IMU1

figure 3: The derived w2 rate from Q2 differencing (matches figure 4)

figure 4: The reported w2 rate from IMU2

figure 5: Norm of figure1 plotted on top of norm of figure 3 (derived rate magnitudes match)

figure 6: Norm of figure 2 plotted on top of norm of figure 4 (reported rate magnitudes match)

figure 7: Figure 1 rates rotated into Figure 3 frame including alignment (matches)

figure 8: The differences in Figure 7 (reasonable but noisy)

figure 9: Raw Q1 norm

figure 10: Q1

figure 11: Raw Q2 norm

figure 12: Q2

figure 13: Relative Q1 vs Q2 differences accounting for alignment (reasonably constant I suppose)

The Figure 13 relative quaternion difference is

The code:

% Assuming SVL+ convention
% Scalar first, Vector last, Left Chain, Hamilton Convention
% Quaternions and vectors are arranged in rows
function alignment = qcomparel(Q1,Q2,timestamp,w1,w2)

% Bad data replace
Q1norm = sqrt(sum(Q1 .* Q1, 2));
Q2norm = sqrt(sum(Q2 .* Q2, 2));

Q1bad = Q1norm < 0.9 | Q1norm > 1.1;
f = find(Q1bad);
for k=1:numel(f)
    Q1(f(k),:) = Q1(f(k)-1,:) + (Q1(f(k)-1,:) - Q1(f(k)-2,:));
    Q1(f(k),:) = Q1(f(k),:) / norm(Q1(f(k),:));
end

Q2bad = Q2norm < 0.9 | Q2norm > 1.1;
f = find(Q2bad);
for k=1:numel(f)
    Q2(f(k),:) = Q2(f(k)-1,:) + (Q2(f(k)-1,:) - Q2(f(k)-2,:));
    Q2(f(k),:) = Q2(f(k),:) / norm(Q2(f(k),:));
end

% Renormalize results
Q1norm = sqrt(sum(Q1 .* Q1, 2));
Q2norm = sqrt(sum(Q2 .* Q2, 2));

Q1 = bsxfun(@rdivide,Q1,Q1norm);
Q2 = bsxfun(@rdivide,Q2,Q2norm);

% Enforce Sign consistency
n = size(Q1,1);
for k=2:n
    [~,m] = max(abs(Q1(k,:)));
    if( Q1(k-1,m) * Q1(k,m) < 0 )
        Q1(k,:) = -Q1(k,:);
    end
end

n = size(Q2,1);
for k=2:n
    [~,m] = max(abs(Q2(k,:)));
    if( Q2(k-1,m) * Q2(k,m) < 0 )
        Q2(k,:) = -Q2(k,:);
    end
end

% Numerical derivative calculation (deltaq/deltat)
Q1dot = bsxfun(@rdivide,diff(Q1),diff(timestamp));
Q2dot = bsxfun(@rdivide,diff(Q2),diff(timestamp));

% Numerical body rate calculation
% For Left Chain convention, qdot = -0.5 * q * weci
% Solve this for weci = -2 * q^-1 * qdot
Q1w = -2 * qmult( qconj(Q1(1:end-1,:)), Q1dot ) * (180/pi);
Q1wnorm = sqrt(sum(Q1w .* Q1w,2));
Q2w = -2 * qmult( qconj(Q2(1:end-1,:)), Q2dot ) * (180/pi);
Q2wnorm = sqrt(sum(Q2w .* Q2w,2));

% Alignment estimation (Matt J's FEX code)
% https://www.mathworks.com/matlabcentral/fileexchange/26186-absolute-orientation-horn-s-method
alignment = absor(Q1w(:,2:4)',Q2w(:,2:4)');
DC = alignment.R;

% IMU reported rotation rates
w1norm = sqrt(sum(w1 .* w1,2));
w2norm = sqrt(sum(w2 .* w2,2));

% Rotate Q1 derived rates into Q2 derived rates frame
RW1 = Q1w(:,2:4) * DC';

% Relative alignment quaternion
Q = alignment.q';

% Quaternion difference accounting for relative alignment Q
Qdiff = qmult(Q2,qmult(Q,qconj(Q1)));

% Plots
figure;
plot(timestamp(1:end-1),Q1w(:,[2 3 4 1]));
grid on
title('Q1w');
legend('w1x','w1y','w1z','s');

figure;
plot(timestamp,w1);
grid on
title('w1');
legend('w1x','w1y','w1z');

figure;
plot(timestamp(1:end-1),Q2w(:,[2 3 4 1]));
grid on
title('Q2w');
legend('w2x','w2y','w2z','s');

figure;
plot(timestamp,w2);
grid on
title('w2');
legend('w2x','w2y','w2z');

figure;
plot(timestamp(1:end-1),[Q1wnorm,Q2wnorm]);
grid on
title('QWnorm');
legend('w1norm','w2norm');

figure;
plot(timestamp,[w1norm,w2norm]);
grid on
title('wnorm');
legend('w1norm','w2norm');

figure;
plot(timestamp(1:end-1),[RW1,Q2w(:,2:4)]);
grid on
title('DC*Qw1 and Qw2');
legend('(DC*Qw1)x','(DC*Qw1)y','(DC*Qw1)z','Qw2x','Qw2y','Qw2z');

figure;
plot(timestamp(1:end-1),RW1-Q2w(:,2:4));
grid on
title('DC*Qw1 - Qw2');
legend('(R*Qw1)x - Qw2x','(R*Qw1)y - Qw2y','(R*Qw1)z - Qw2z');

figure;
plot(timestamp,Q1norm);
grid on
title('Q1 norm');

figure;
plot(timestamp,Q1);
grid on
title('Q1');
legend('q1','q2','q3','q4');

figure;
plot(timestamp,Q2norm);
grid on
title('Q2 norm');

figure;
plot(timestamp,Q2);
grid on
title('Q2');
legend('q1','q2','q3','q4');

figure;
plot(timestamp,Qdiff)
grid on
title('Q diff');
ylabel('Qdiff elements');

end

with

function r = qmult(q,p)
s = 1;
v = 2:4;
%s = 4;
%v = 1:3;
qs = q(:,s);
qv = q(:,v);
ps = p(:,s);
pv = p(:,v);
rs = qs .* ps - DOT(qv,pv);
rv = bsxfun(@times,qs,pv) + bsxfun(@times,ps,qv) + CROSS(qv,pv);
if( s == 1 )
    r = [rs,rv];
else
    r = [rv,rs];
end
end

function d = DOT(a,b)
    d = sum(bsxfun(@times,a,b), 2);
end

function c = CROSS(a,b)
if( size(a,1) == size(b,1) )
    c = cross(a,b,2);
elseif( size(a,1) == 1 )
    c = b;
    for k=1:size(b,1)
        c(k,:) = cross(a,b(k,:));
    end
elseif( size(b,1) == 1 )
    c = a;
    for k=1:size(a,1)
        c(k,:) = cross(a(k,:),b);
    end
else
    error('Size mismatch');
end
end

and

function qc = qconj(q)
s = 1;
v = 2:4;
%s = 4;
%v = 1:3;
if( s == 1 )
    qc = [-q(:,s) q(:,v)];
else
    qc = [q(:,v) -q(:,s)];
end
end

[James Tursa]

Which IMU is this? The gyro data noise is much larger than earth rate, otherwise we could have used that along with the accelrometer data to align to a local level frame (assuming the gyro and accelerometer data are using the same frame).

[hichem mestiri]

Thank you very much. I am using invensens icm20948 as sensor.

I'm very interested in how to do it with acceleration but i don't know if gyro and accelerometer data are using the same frame. how can i verify this?

[James Tursa]

Updated the code above for better result (Qdiff needed to be done in Left Chain convention).

Note: There is a coordinate frame ambiguity, and you might have to switch "Body" and "Inertial" in the above. I haven't been able to locate any information about the quaternion convention of this sensor package online.

[hichem mestiri]

Do you know how can i verify if the gyro and accelerometer data are using the same frame. I think that there is a little misallignement.

[James Tursa]

If there is no information in the spec sheets, then this is going to be hard to figure out. Maybe impossible given the noise present in the sensor data. E.g., the gyro noise is so great you can't even detect Earth Rate in the data.

[宇 梁]

it did has Effect on my work with ICM20948 also.but how can I use this work on real time calculating?I noticed that I have to use all the data to calculate the alignment.q which mean that I have to sample all the data before I calculate the Q_diff