Subtraction of gravity from accerelometer data

Hello,

I'm having some trouble subtracting the gravitation from my accelerometer data. I'm following the implementation from this site http://www.owen-lu.com/program-accel-grav , but Matlab gives me this error code

"Error using quatmultiply (line 36)

First input elements are not real."

even when I'm using

quatmultiply(quat2(i+1,:),conj(quat2(i,:))

The section in my coding is the following:

%%

e_1 = [1 0 0];                          
e_2 = [0 1 0];
e_3 = [0 0 1];

e_1rot = zeros(height(T),3);
e_2rot = zeros(height(T),3);
e_3rot = zeros(height(T),3);

i = 1;

while i < height(T) 
    
    %I also used quatmultiply(quat2(i+1,:),conj(quat2(i,:)), but there was the error message
    
    e_1rot(i,:) = quat2(i+1,:)*conj(quat2(i,:))*e_1*quat2(i,:)*conj(quat2(i+1,:));     
    e_2rot(i,:) = quat2(i+1,:)*conj(quat2(i,:))*e_2*quat2(i,:)*conj(quat2(i+1,:));
    e_3rot(i,:) = quat2(i+1,:)*(conjquat2(i,:))*e_3*quat2(i,:)*conj(quat2(i+1,:));
    
    i = i + 1;
    
end

where quat2 is the pure quaternion (without the real part) and conj(quat2) is the conjugation. Could it be that the basis vectors should be 4 dimensional, just like the quaternion? How do I solve then my 3 dimensional gravity problem?

I hope you guys can help me, if needed, I'll post the whole code for better understanding.

[James Tursa]

We need to see more code. How are you building your quaternions? Are they standard double arrays, or OOP class objects?

[Bruce Rogers]

clear;

T = readtable('First1000.csv');

t = 0.05;

phi = zeros(height(T),1);   %Getting angular velocity from csv 
theta = zeros(height(T),1); %
psi = zeros(height(T),1);   

for i = 1:height(T)
        psi(i) = table2array(T(i,6)) * t;   %table2array 1x1 Matrix in double 
end

for i = 1:height(T)
        theta(i) = table2array(T(i,5)) * t;
end

for i = 1:height(T)
        phi(i) = table2array(T(i,4)) * t;
end

%%
a=zeros(height(T),1);
b=zeros(height(T),1);
c=zeros(height(T),1);
d=zeros(height(T),1);

for i = 1:height(T)
    a(i) = cosd(phi(i)/2)*cosd(theta(i)/2)*cosd(psi(i)/2)+sind(psi(i)/2)*sind(theta(i)/2)*sind(psi(i)/2);
end

for i = 1:height(T)
    b(i) = sind(phi(i)/2)*cosd(theta(i)/2)*cosd(psi(i)/2)-cosd(phi(i)/2)*sind(theta(i)/2)*sind(psi(i)/2);
end

for i = 1:height(T)
    c(i) = cosd(phi(i)/2)*sind(theta(i)/2)*cosd(psi(i)/2)+sind(phi(i)/2)*cosd(theta(i)/2)*sind(psi(i)/2);
end

for i = 1:height(T)
    d(i) = cosd(phi(i)/2)*cosd(theta(i)/2)*sind(psi(i)/2)-sind(phi(i)/2)*sind(theta(i)/2)*cosd(psi(i)/2);
end

quat = quaternion(a, b, c, d);              %From Matlab Robot Toolbox


quat2 = quaternion(zeros(height(T),1),b, c, d);

%%

e_1 = [1 0 0];                           %Basis Vector
e_2 = [0 1 0];
e_3 = [0 0 1];


% % 

e_1rot = zeros(height(T),3);
e_2rot = zeros(height(T),3);
e_3rot = zeros(height(T),3);

i = 1;

while i < height(T) 
    
    %I also used quatmultiply(quat2(i+1,:),conj(quat2(i,:)), but there was the error message
    
    e_1rot(i,:) = quat2(i+1,:)*conj(quat2(i,:))*e_1*quat2(i,:)*conj(quat2(i+1,:));     
    e_2rot(i,:) = quat2(i+1,:)*conj(quat2(i,:))*e_2*quat2(i,:)*conj(quat2(i+1,:));
    e_3rot(i,:) = quat2(i+1,:)*(conjquat2(i,:))*e_3*quat2(i,:)*conj(quat2(i+1,:));
    
    i = i + 1;
    
end

%%

a_xrot = T(:,1); 

a_yrot = T(:,2);

a_zrot = T(:,3);

a_m = zeros(height(T),3);


for i = 1:height(T)
    
    for j = 1:3
    
        a_m(i,j) = table2array(a_xrot(i,1)) * e_1rot(i,j) + table2array(a_yrot(i,1)) * e_2rot(i,j)...
            + table2array(a_zrot(i,1)) * e_3rot(i,j);

    end
        
end


%%
g = [0; 0; 981];

g_rot = zeros(3, height(T));



for i = 1:height(T)
    
    g_rot(:,i) = [cosd(theta(i))*cosd(psi(i)) cosd(theta(i))*sind(psi(i)) -sind(theta(i));...
    sind(phi(i))*sind(theta(i))*cosd(psi(i))-cosd(theta(i))*sind(psi(i)) ...
    sind(phi(i))*sind(theta(i))*sind(psi(i))+cosd(phi(i))*cosd(psi(i)) sind(phi(i))*cosd(theta(i));...
    cosd(phi(i))*sind(theta(i))*cosd(psi(i))+sind(phi(i))*sind(psi(i))...
    cosd(phi(i))*sind(theta(i))*sind(psi(i))-sind(phi(i))*cosd(psi(i)) cosd(phi(i))*cosd(theta(i))] *g;

    %rotating gravitation vector
end

%%

acc = zeros(3,size(T,1));

for i = 1:size(T,1)
    
    acc(:,i) = a_m(:,i) - g_rot(:,i);
    
end

%%

Hello James,

thanks for your answer.

This is my whole code. I'm building them by getting every quaternion Part (a,b,c,d) with a formula and putting them together to a quaternion by quat function. Quat is a function from the robot toolbox. With quat2 I'm putting every real part to 0 to get real quaternion. These quaternions are standart double arrays.

The csv is just a txt file with acceleration, angular velocity and magnetometer data.

[James Tursa]

Can you post that csv file, or some of it?

[Bruce Rogers]

Sure, this is a part of that csv:

% in columns: x,y,z acceleration in mm/s, x,y,z angular velocity in deg/s, 
% magnetometer data in µT. Every line is one data packet in an interval of 0.05s

19.41	-15.93	991.03	-0.02	0.11	-0.02	-812.00	-114.00	-1133.00
17.58	-16.97	986.45	-0.03	0.21	0.02	-812.00	-114.00	-1133.00
18.37	-20.69	986.27	-0.04	0.24	0.01	-812.00	-114.00	-1133.00
17.33	-14.53	988.77	0.03	0.14	0.02	-823.00	-121.00	-1153.00
17.03	-15.50	989.07	-0.11	0.22	0.10	-823.00	-121.00	-1153.00
21.00	-23.32	989.99	0.03	0.18	0.08	-816.00	-89.00	-1143.00
19.53	-14.71	985.90	0.05	0.15	0.10	-816.00	-89.00	-1143.00
16.60	-16.17	986.15	-0.02	0.15	0.12	-816.00	-89.00	-1143.00
18.92	-24.05	985.66	-0.07	0.14	0.01	-811.00	-116.00	-1145.00
17.33	-18.19	986.88	-0.05	0.19	0.02	-811.00	-116.00	-1145.00
19.59	-16.24	985.53	0.95	1.20	0.09	-819.00	-125.00	-1153.00
22.40	-24.17	991.27	-0.45	0.02	-0.05	-819.00	-125.00	-1153.00
-26.92	41.50	975.04	1.14	0.41	1.82	-819.00	-125.00	-1153.00
58.96	-0.49	955.87	17.84	3.18	39.73	-823.00	-125.00	-1146.00
9.70	5.74	949.40	-16.33	0.18	20.60	-823.00	-125.00	-1146.00
11.54	55.30	1070.43	-18.39	-10.90	13.71	-803.00	-209.00	-1179.00
1.71	-2.56	978.45	-2.90	0.65	40.90	-803.00	-209.00	-1179.00
44.68	-54.08	986.69	-1.90	2.06	14.56	-803.00	-209.00	-1179.00
29.36	12.63	985.41	0.23	-1.45	11.65	-784.00	-282.00	-1219.00
116.52	280.76	944.15	-0.18	-2.61	6.03	-784.00	-282.00	-1219.00
391.60	581.12	1185.73	20.98	-75.58	-13.74	-610.00	-267.00	-1501.00
189.33	487.55	1298.22	-1.75	-5.80	-21.78	-610.00	-267.00	-1501.00
526.73	410.22	1157.35	8.26	-7.71	-30.20	-610.00	-267.00	-1501.00
383.42	405.94	1074.65	10.72	23.86	-5.15	1235.00	-14.00	-699.00
549.62	257.20	920.72	18.56	13.31	-6.36	1235.00	-14.00	-699.00
429.69	160.77	863.28	23.43	6.33	0.40	866.00	-21.00	99.00
473.75	0.12	788.76	18.44	-9.96	26.30	866.00	-21.00	99.00
398.74	22.16	734.86	4.78	-14.70	49.36	866.00	-21.00	99.00
374.15	127.99	865.23	16.76	-32.34	76.29	598.00	-23.00	253.00
59.88	250.24	1031.37	20.90	12.33	108.57	598.00	-23.00	253.00
9.95	208.31	979.37	52.19	4.12	105.09	541.00	-12.00	263.00
-157.59	8.61	1156.98	51.40	6.84	118.52	541.00	-12.00	263.00
-127.20	106.02	940.86	48.36	15.14	114.05	541.00	-12.00	263.00
-61.65	50.78	1092.65	83.04	30.95	157.47	466.00	58.00	284.00
-177.67	134.89	1026.00	24.73	19.45	145.93	466.00	58.00	284.00
-256.10	68.18	922.49	21.22	16.83	98.37	353.00	137.00	304.00
-80.81	-96.44	890.93	13.30	52.33	95.09	353.00	137.00	304.00
-204.04	159.73	901.06	-24.09	5.42	40.78	353.00	137.00	304.00
150.39	44.86	905.21	10.19	10.80	39.18	333.00	189.00	289.00
-42.18	20.87	866.64	-4.33	19.98	9.75	333.00	189.00	289.00
308.11	185.97	852.66	15.82	34.62	21.03	312.00	192.00	278.00
181.52	63.42	966.61	-18.10	-29.00	-3.81	312.00	192.00	278.00
244.57	149.35	1022.09	15.47	2.20	34.81	312.00	192.00	278.00
358.89	193.05	1323.73	1.29	-27.40	1.88	285.00	187.00	263.00
70.50	369.57	1111.39	47.44	-7.39	42.02	285.00	187.00	263.00
277.10	55.24	1213.44	-26.31	13.47	23.73	225.00	162.00	235.00
184.75	274.78	894.96	-40.60	24.88	-13.35	225.00	162.00	235.00
131.23	246.77	747.01	99.78	43.51	-5.39	225.00	162.00	235.00
103.39	178.71	956.85	196.68	43.27	-35.93	225.00	169.00	249.00
198.43	332.89	986.88	48.97	-47.67	-73.62	225.00	169.00	249.00
91.31	643.25	572.88	-58.61	-49.13	13.55	111.00	151.00	211.00
66.10	345.76	675.54	-82.87	1.75	35.93	111.00	151.00	211.00
129.15	420.72	947.33	-84.56	-13.61	-46.11	200.00	137.00	222.00
44.92	742.00	937.81	-34.07	-31.88	-31.66	200.00	137.00	222.00
97.47	169.49	789.00	-15.00	25.42	-15.90	200.00	137.00	222.00
51.70	382.63	1239.44	-50.24	45.46	-113.66	216.00	117.00	206.00
41.75	375.06	811.58	32.06	-37.01	-77.07	216.00	117.00	206.00
122.74	109.62	634.64	3.49	5.78	-77.22	216.00	117.00	206.00
160.28	267.40	892.27	25.54	-7.66	-92.16	234.00	92.00	216.00
151.98	408.69	696.53	28.54	-44.69	-57.60	234.00	92.00	216.00
190.49	405.40	787.35	-59.49	-29.17	-0.21	255.00	75.00	192.00
154.05	381.35	1002.26	-34.61	-38.73	17.88	255.00	75.00	192.00
49.13	385.68	865.84	-30.36	-46.67	28.39	296.00	26.00	201.00
127.44	263.92	964.90	13.46	-19.03	19.23	296.00	26.00	201.00
-83.25	272.95	1441.53	-56.18	-2.74	0.07	296.00	26.00	201.00
39.73	448.12	1017.46	-12.83	13.81	-17.73	298.00	32.00	192.00
127.44	511.05	1172.55	17.64	33.65	-19.60	298.00	32.00	192.00
122.25	143.43	909.79	6.45	-16.56	-43.98	298.00	32.00	192.00
81.12	-117.61	664.31	-67.61	-49.48	-114.51	305.00	57.00	182.00
253.30	87.04	307.25	-38.61	-2.75	-39.99	305.00	57.00	182.00
-26.49	-52.19	537.23	-38.25	-10.76	-43.32	360.00	-5.00	180.00
118.77	32.23	577.33	-53.58	8.80	-35.00	360.00	-5.00	180.00
-2.93	207.15	762.94	-25.32	4.09	1.68	431.00	-30.00	156.00
144.17	80.75	617.68	23.62	20.50	18.72	431.00	-30.00	156.00
-98.08	53.83	682.37	7.22	-4.08	53.72	431.00	-30.00	156.00
146.12	22.89	442.20	-11.01	16.11	28.38	443.00	-17.00	172.00
40.89	56.76	683.35	-36.25	-5.95	56.25	443.00	-17.00	172.00
39.61	175.11	911.93	-30.98	63.20	-7.74	443.00	-17.00	172.00
314.58	136.60	1119.45	-28.29	31.60	35.94	493.00	17.00	158.00
274.96	374.15	1481.20	2.04	-16.07	29.22	493.00	17.00	158.00
174.07	267.40	1765.14	-32.81	-66.25	23.15	498.00	23.00	153.00
159.61	67.26	1676.09	-28.67	-65.58	-32.92	498.00	23.00	153.00
192.87	-140.44	1667.18	-4.09	-34.87	-18.67	530.00	-44.00	132.00
106.32	-99.91	1588.07	10.86	-23.38	-1.11	530.00	-44.00	132.00
200.74	-101.87	1212.22	11.63	23.30	-54.08	530.00	-44.00	132.00
192.38	-6.84	1021.42	-6.17	-22.97	-16.83	514.00	-75.00	137.00
221.50	-72.88	1004.70	-35.96	18.07	-54.71	514.00	-75.00	137.00
292.97	-112.55	904.79	-33.19	-10.06	-40.34	514.00	-75.00	137.00
258.54	-35.58	962.95	-56.20	-72.49	-40.64	534.00	-98.00	122.00
396.61	-291.87	904.05	61.71	53.36	-52.82	534.00	-98.00	122.00
364.56	-135.62	956.48	54.36	45.25	34.38	551.00	-123.00	87.00
164.86	74.83	887.33	39.65	65.95	23.88	551.00	-123.00	87.00
-35.52	-627.62	1535.46	-16.20	105.61	-13.40	512.00	-62.00	129.00
166.93	-56.64	959.47	-1.94	6.53	-20.82	512.00	-62.00	129.00
164.31	-34.91	963.26	-0.11	-5.52	0.26	512.00	-62.00	129.00
-134.52	-212.89	728.21	14.70	0.50	49.95	505.00	-73.00	129.00
-68.73	33.57	733.89	0.08	170.89	-4.16	505.00	-73.00	129.00
-696.17	-324.46	812.93	-26.29	-38.58	-26.15	505.00	-73.00	129.00
6.59	-24.29	992.25	0.98	1.62	0.05	507.00	0.00	130.00
15.14	-28.08	986.21	0.35	0.70	0.03	507.00	0.00	130.00
16.97	-27.04	986.45	-0.16	0.11	0.02	514.00	-7.00	137.00
17.70	-31.49	986.21	0.03	0.27	-0.04	514.00	-7.00	137.00
18.68	-29.42	987.98	-0.04	0.05	0.14	500.00	7.00	141.00
15.50	-29.91	985.53	-0.05	0.23	0.12	500.00	7.00	141.00
17.33	-29.24	988.22	-0.01	0.24	-0.01	500.00	7.00	141.00
17.40	-30.94	988.89	-0.05	0.15	0.08	507.00	7.00	123.00
19.59	-31.01	986.27	-0.03	0.21	-0.01	507.00	7.00	123.00
15.87	-28.32	987.30	-0.05	0.21	0.02	512.00	1.00	142.00
15.26	-29.17	987.49	0.01	0.12	0.02	512.00	1.00	142.00
16.30	-28.69	985.47	-0.02	0.18	0.02	512.00	1.00	142.00
16.97	-31.01	986.63	-0.03	0.12	-0.01	518.00	-3.00	127.00
18.01	-28.02	989.87	-0.02	0.20	0.02	518.00	-3.00	127.00
15.56	-28.50	989.07	-0.09	0.11	-0.02	523.00	16.00	132.00
18.55	-28.87	986.51	0.02	0.17	0.05	523.00	16.00	132.00
14.95	-30.40	987.85	0.03	0.24	0.09	523.00	16.00	132.00
17.64	-30.03	987.12	-0.08	0.13	-0.02	525.00	-3.00	130.00
16.11	-29.72	985.35	-0.01	0.17	-0.02	525.00	-3.00	130.00
18.62	-30.27	984.13	0.02	0.26	-0.02	512.00	-1.00	136.00
18.07	-30.27	983.95	0.05	0.11	0.01	512.00	-1.00	136.00
17.03	-30.76	989.32	-0.01	0.15	0.05	512.00	-1.00	136.00
15.50	-28.93	985.29	0.05	0.17	-0.04	521.00	7.00	127.00
16.30	-30.52	988.65	0.03	0.22	0.11	521.00	7.00	127.00
18.62	-28.38	988.95	0.01	0.16	-0.07	516.00	8.00	136.00
18.31	-30.70	985.11	0.05	0.18	0.02	516.00	8.00	136.00
17.09	-27.83	985.41	-0.11	0.17	0.11	516.00	8.00	136.00
19.10	-31.25	983.70	-0.02	0.14	-0.01	512.00	-1.00	139.00
17.82	-31.07	987.06	-0.05	0.31	0.09	512.00	-1.00	139.00
18.55	-28.44	988.59	-0.03	0.11	0.02	518.00	3.00	137.00
16.17	-29.91	986.08	0.03	0.24	-0.05	518.00	3.00	137.00
16.91	-27.34	986.21	0.07	0.21	0.02	518.00	3.00	137.00
17.70	-29.79	986.51	0.06	0.24	0.06	516.00	5.00	136.00
18.98	-29.48	987.61	0.05	0.18	-0.05	516.00	5.00	136.00
17.94	-30.46	987.30	0.05	0.17	-0.05	509.00	12.00	142.00
16.91	-29.48	985.60	-0.05	0.14	-0.06	509.00	12.00	142.00
15.44	-29.97	985.78	-0.08	0.15	0.02	509.00	12.00	142.00
19.65	-30.27	986.76	0.01	0.24	-0.06	507.00	7.00	137.00
21.24	-30.52	986.02	0.00	0.02	-0.02	507.00	7.00	137.00
-76.66	-3.97	974.67	2.08	0.84	7.90	512.00	-1.00	139.00
16.85	-27.53	978.76	22.82	19.06	1.43	512.00	-1.00	139.00
-124.69	-94.30	1512.45	105.34	36.12	-1.48	512.00	-1.00	139.00
-353.52	-364.56	1712.52	30.63	74.89	-0.36	472.00	28.00	158.00
-481.57	263.92	1446.66	14.03	72.32	-89.69	472.00	28.00	158.00

Answer 1

[Bruce Rogers]

I got the solution for my problem. Owens Website ( http://www.owen-lu.com/program-accel-grav ) uses two different ways to subtract the gravity. One way is to rotate the gravitation according to the new coordinate system, the other one is rotating the whole coordinate system and substract the gravity from the x,y,z acceleration.

I used the first way, because then you only need one loop for rotating the gravity, which looks like this:

g = [0; 0; 981];

g_rot = zeros(3,height(T));         %height(T) means the height of the csv

for i = 1:height(T)
    
     g_rot(:,i) = [1-(2*quatParts(i,3)^2)*(2*quatParts(i,4)^2) 2*((quatParts(i,2)*quatParts(i,3))+(quatParts(i,1)*quatParts(i,4)))...
        2*((quatParts(i,2)*quatParts(i,4))-(quatParts(i,1)*quatParts(i,3)))
        2*((quatParts(i,2)*quatParts(i,3))-(quatParts(i,1)*quatParts(i,4))) (1-(2*quatParts(i,2))^2-(2*quatParts(i,4)^2))...
        2*((quatParts(i,3)*quatParts(i,4))+(quatParts(i,1)*quatParts(i,2)))
        2*((quatParts(i,2)*quatParts(i,4))+(quatParts(i,1)*quatParts(i,3))) 2*((quatParts(i,3)*quatParts(i,4))-(quatParts(i,1)*quatParts(i,2)))...
        1-(2*quatParts(i,2)^2)-(2*quatParts(i,3)^2)]*g
    
end

But thank you for your interest @James Tursa.

[Simon]

Hey @Bruce Rogers, awesome that it worked out for you. I also want to get rid of the gravity component in my data. Unfortunately "http://www.owen-lu.com/program-accel-grav" doesnt exist anymore. Is there any chance you could share your complete code?

Many thanks!

Simon