Subtraction of gravity from accerelometer data

9 次查看（过去 30 天）
Bruce Rogers
Bruce Rogers 2021-1-31
评论： Simon 2024-7-12
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更早的评论
Bruce Rogers
Bruce Rogers 2021-2-4
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

请先登录，再进行评论。

请先登录，再回答此问题。

采纳的回答

Bruce Rogers
Bruce Rogers 2021-2-8
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.
  1 个评论
Simon
Simon 2024-7-12
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

请先登录，再进行评论。

更多回答（0 个）

类别

在 Help Center 和 File Exchange 中查找有关 Satellite Mission Analysis 的更多信息

标签

产品


版本

R2020b

Community Treasure Hunt

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

Start Hunting!

Translated by