Subtraction of gravity from accerelometer data

Question

Bruce Rogers 2021-1-31

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/731898-subtraction-of-gravity-from-accerelometer-data

回答： Bruce Rogers 2021-2-8

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.

4 个评论
显示 2更早的评论隐藏 2更早的评论

Bruce Rogers 2021-2-3

在 MATLAB Online 中打开

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 2021-2-3

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

Bruce Rogers 2021-2-4

在 MATLAB Online 中打开

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
41	-15.93	991.03	-0.02	0.11	-0.02	-812.00	-114.00	-1133.00
58	-16.97	986.45	-0.03	0.21	0.02	-812.00	-114.00	-1133.00
37	-20.69	986.27	-0.04	0.24	0.01	-812.00	-114.00	-1133.00
33	-14.53	988.77	0.03	0.14	0.02	-823.00	-121.00	-1153.00
03	-15.50	989.07	-0.11	0.22	0.10	-823.00	-121.00	-1153.00
00	-23.32	989.99	0.03	0.18	0.08	-816.00	-89.00	-1143.00
53	-14.71	985.90	0.05	0.15	0.10	-816.00	-89.00	-1143.00
60	-16.17	986.15	-0.02	0.15	0.12	-816.00	-89.00	-1143.00
92	-24.05	985.66	-0.07	0.14	0.01	-811.00	-116.00	-1145.00
33	-18.19	986.88	-0.05	0.19	0.02	-811.00	-116.00	-1145.00
59	-16.24	985.53	0.95	1.20	0.09	-819.00	-125.00	-1153.00
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
96	-0.49	955.87	17.84	3.18	39.73	-823.00	-125.00	-1146.00
70	5.74	949.40	-16.33	0.18	20.60	-823.00	-125.00	-1146.00
54	55.30	1070.43	-18.39	-10.90	13.71	-803.00	-209.00	-1179.00
71	-2.56	978.45	-2.90	0.65	40.90	-803.00	-209.00	-1179.00
68	-54.08	986.69	-1.90	2.06	14.56	-803.00	-209.00	-1179.00
36	12.63	985.41	0.23	-1.45	11.65	-784.00	-282.00	-1219.00
52	280.76	944.15	-0.18	-2.61	6.03	-784.00	-282.00	-1219.00
60	581.12	1185.73	20.98	-75.58	-13.74	-610.00	-267.00	-1501.00
33	487.55	1298.22	-1.75	-5.80	-21.78	-610.00	-267.00	-1501.00
73	410.22	1157.35	8.26	-7.71	-30.20	-610.00	-267.00	-1501.00
42	405.94	1074.65	10.72	23.86	-5.15	1235.00	-14.00	-699.00
62	257.20	920.72	18.56	13.31	-6.36	1235.00	-14.00	-699.00
69	160.77	863.28	23.43	6.33	0.40	866.00	-21.00	99.00
75	0.12	788.76	18.44	-9.96	26.30	866.00	-21.00	99.00
74	22.16	734.86	4.78	-14.70	49.36	866.00	-21.00	99.00
15	127.99	865.23	16.76	-32.34	76.29	598.00	-23.00	253.00
88	250.24	1031.37	20.90	12.33	108.57	598.00	-23.00	253.00
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
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
11	185.97	852.66	15.82	34.62	21.03	312.00	192.00	278.00
52	63.42	966.61	-18.10	-29.00	-3.81	312.00	192.00	278.00
57	149.35	1022.09	15.47	2.20	34.81	312.00	192.00	278.00
89	193.05	1323.73	1.29	-27.40	1.88	285.00	187.00	263.00
50	369.57	1111.39	47.44	-7.39	42.02	285.00	187.00	263.00
10	55.24	1213.44	-26.31	13.47	23.73	225.00	162.00	235.00
75	274.78	894.96	-40.60	24.88	-13.35	225.00	162.00	235.00
23	246.77	747.01	99.78	43.51	-5.39	225.00	162.00	235.00
39	178.71	956.85	196.68	43.27	-35.93	225.00	169.00	249.00
43	332.89	986.88	48.97	-47.67	-73.62	225.00	169.00	249.00
31	643.25	572.88	-58.61	-49.13	13.55	111.00	151.00	211.00
10	345.76	675.54	-82.87	1.75	35.93	111.00	151.00	211.00
15	420.72	947.33	-84.56	-13.61	-46.11	200.00	137.00	222.00
92	742.00	937.81	-34.07	-31.88	-31.66	200.00	137.00	222.00
47	169.49	789.00	-15.00	25.42	-15.90	200.00	137.00	222.00
70	382.63	1239.44	-50.24	45.46	-113.66	216.00	117.00	206.00
75	375.06	811.58	32.06	-37.01	-77.07	216.00	117.00	206.00
74	109.62	634.64	3.49	5.78	-77.22	216.00	117.00	206.00
28	267.40	892.27	25.54	-7.66	-92.16	234.00	92.00	216.00
98	408.69	696.53	28.54	-44.69	-57.60	234.00	92.00	216.00
49	405.40	787.35	-59.49	-29.17	-0.21	255.00	75.00	192.00
05	381.35	1002.26	-34.61	-38.73	17.88	255.00	75.00	192.00
13	385.68	865.84	-30.36	-46.67	28.39	296.00	26.00	201.00
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
73	448.12	1017.46	-12.83	13.81	-17.73	298.00	32.00	192.00
44	511.05	1172.55	17.64	33.65	-19.60	298.00	32.00	192.00
25	143.43	909.79	6.45	-16.56	-43.98	298.00	32.00	192.00
12	-117.61	664.31	-67.61	-49.48	-114.51	305.00	57.00	182.00
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
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
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
12	22.89	442.20	-11.01	16.11	28.38	443.00	-17.00	172.00
89	56.76	683.35	-36.25	-5.95	56.25	443.00	-17.00	172.00
61	175.11	911.93	-30.98	63.20	-7.74	443.00	-17.00	172.00
58	136.60	1119.45	-28.29	31.60	35.94	493.00	17.00	158.00
96	374.15	1481.20	2.04	-16.07	29.22	493.00	17.00	158.00
07	267.40	1765.14	-32.81	-66.25	23.15	498.00	23.00	153.00
61	67.26	1676.09	-28.67	-65.58	-32.92	498.00	23.00	153.00
87	-140.44	1667.18	-4.09	-34.87	-18.67	530.00	-44.00	132.00
32	-99.91	1588.07	10.86	-23.38	-1.11	530.00	-44.00	132.00
74	-101.87	1212.22	11.63	23.30	-54.08	530.00	-44.00	132.00
38	-6.84	1021.42	-6.17	-22.97	-16.83	514.00	-75.00	137.00
50	-72.88	1004.70	-35.96	18.07	-54.71	514.00	-75.00	137.00
97	-112.55	904.79	-33.19	-10.06	-40.34	514.00	-75.00	137.00
54	-35.58	962.95	-56.20	-72.49	-40.64	534.00	-98.00	122.00
61	-291.87	904.05	61.71	53.36	-52.82	534.00	-98.00	122.00
56	-135.62	956.48	54.36	45.25	34.38	551.00	-123.00	87.00
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
93	-56.64	959.47	-1.94	6.53	-20.82	512.00	-62.00	129.00
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
59	-24.29	992.25	0.98	1.62	0.05	507.00	0.00	130.00
14	-28.08	986.21	0.35	0.70	0.03	507.00	0.00	130.00
97	-27.04	986.45	-0.16	0.11	0.02	514.00	-7.00	137.00
70	-31.49	986.21	0.03	0.27	-0.04	514.00	-7.00	137.00
68	-29.42	987.98	-0.04	0.05	0.14	500.00	7.00	141.00
50	-29.91	985.53	-0.05	0.23	0.12	500.00	7.00	141.00
33	-29.24	988.22	-0.01	0.24	-0.01	500.00	7.00	141.00
40	-30.94	988.89	-0.05	0.15	0.08	507.00	7.00	123.00
59	-31.01	986.27	-0.03	0.21	-0.01	507.00	7.00	123.00
87	-28.32	987.30	-0.05	0.21	0.02	512.00	1.00	142.00
26	-29.17	987.49	0.01	0.12	0.02	512.00	1.00	142.00
30	-28.69	985.47	-0.02	0.18	0.02	512.00	1.00	142.00
97	-31.01	986.63	-0.03	0.12	-0.01	518.00	-3.00	127.00
01	-28.02	989.87	-0.02	0.20	0.02	518.00	-3.00	127.00
56	-28.50	989.07	-0.09	0.11	-0.02	523.00	16.00	132.00
55	-28.87	986.51	0.02	0.17	0.05	523.00	16.00	132.00
95	-30.40	987.85	0.03	0.24	0.09	523.00	16.00	132.00
64	-30.03	987.12	-0.08	0.13	-0.02	525.00	-3.00	130.00
11	-29.72	985.35	-0.01	0.17	-0.02	525.00	-3.00	130.00
62	-30.27	984.13	0.02	0.26	-0.02	512.00	-1.00	136.00
07	-30.27	983.95	0.05	0.11	0.01	512.00	-1.00	136.00
03	-30.76	989.32	-0.01	0.15	0.05	512.00	-1.00	136.00
50	-28.93	985.29	0.05	0.17	-0.04	521.00	7.00	127.00
30	-30.52	988.65	0.03	0.22	0.11	521.00	7.00	127.00
62	-28.38	988.95	0.01	0.16	-0.07	516.00	8.00	136.00
31	-30.70	985.11	0.05	0.18	0.02	516.00	8.00	136.00
09	-27.83	985.41	-0.11	0.17	0.11	516.00	8.00	136.00
10	-31.25	983.70	-0.02	0.14	-0.01	512.00	-1.00	139.00
82	-31.07	987.06	-0.05	0.31	0.09	512.00	-1.00	139.00
55	-28.44	988.59	-0.03	0.11	0.02	518.00	3.00	137.00
17	-29.91	986.08	0.03	0.24	-0.05	518.00	3.00	137.00
91	-27.34	986.21	0.07	0.21	0.02	518.00	3.00	137.00
70	-29.79	986.51	0.06	0.24	0.06	516.00	5.00	136.00
98	-29.48	987.61	0.05	0.18	-0.05	516.00	5.00	136.00
94	-30.46	987.30	0.05	0.17	-0.05	509.00	12.00	142.00
91	-29.48	985.60	-0.05	0.14	-0.06	509.00	12.00	142.00
44	-29.97	985.78	-0.08	0.15	0.02	509.00	12.00	142.00
65	-30.27	986.76	0.01	0.24	-0.06	507.00	7.00	137.00
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
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 2021-2-8

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/731898-subtraction-of-gravity-from-accerelometer-data#answer_617947

在 MATLAB Online 中打开

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