Subtraction of gravity from accerelometer data

10 Ansichten (letzte 30 Tage)
Bruce Rogers
Bruce Rogers am 31 Jan. 2021
Beantwortet: Bruce Rogers am 8 Feb. 2021
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 Kommentare
James Tursa
James Tursa am 3 Feb. 2021
Can you post that csv file, or some of it?
Bruce Rogers
Bruce Rogers am 4 Feb. 2021
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

Melden Sie sich an, um zu kommentieren.

Akzeptierte Antwort

Bruce Rogers
Bruce Rogers am 8 Feb. 2021
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.

Weitere Antworten (0)

Kategorien

Mehr zu Antennas, Microphones, and Sonar Transducers finden Sie in Help Center und File Exchange

Produkte


Version

R2020b

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!

Translated by