1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
import unittest
import math
import vs
class TestIMUStabilization(unittest.TestCase):
def setUp(self):
pass
def tearDown(self):
pass
def checkAlmostEqualQuaternions(self, q1, q2):
self.assertAlmostEqual(q1.getQ0(), q2.getQ0(), 5)
self.assertAlmostEqual(q1.getQ1(), q2.getQ1(), 5)
self.assertAlmostEqual(q1.getQ2(), q2.getQ2(), 5)
self.assertAlmostEqual(q1.getQ3(), q2.getQ3(), 5)
def testFusionIMU(self):
acc = vs.Vector(0, 0, 1)
gyr = vs.Vector(0, 0, 0)
fusionIMU = vs.FusionIMU()
q = fusionIMU.init(acc, 0)
qTarget = vs.Quat(1., 0, 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
angle = math.pi / 3
acc = vs.Vector(0, math.sin(angle), math.cos(angle))
q = fusionIMU.init(acc, 0)
qTarget = vs.Quat(math.cos(angle/2.), math.sin(angle/2.), 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
acc = vs.Vector(0, -math.sin(angle), math.cos(angle))
q = fusionIMU.init(acc, 0)
qTarget = vs.Quat(math.cos(angle/2.), -math.sin(angle/2.), 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
acc = vs.Vector(math.sin(angle), 0, math.cos(angle))
q = fusionIMU.init(acc, 0)
qTarget = vs.Quat(math.cos(angle/2.), 0, -math.sin(angle/2.), 0)
self.checkAlmostEqualQuaternions(q, qTarget)
acc = vs.Vector(-math.sin(angle), 0, math.cos(angle))
q = fusionIMU.init(acc, 0)
qTarget = vs.Quat(math.cos(angle/2.), 0, math.sin(angle/2.), 0)
self.checkAlmostEqualQuaternions(q, qTarget)
# use accelerometer only
fusionIMU.setFusionFactor(1) # discard gyroscope during fusion
acc = vs.Vector(0, 0, 1)
q = fusionIMU.init(acc, 1000000)
qTarget = vs.Quat(1, 0, 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
acc = vs.Vector(0, math.sin(angle), math.cos(angle))
gyr = vs.Vector(1, 1, 1)
q = fusionIMU.update(gyr, acc, 2000000)
qTarget = vs.Quat(math.cos(angle/2.), math.sin(angle/2.), 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
# use gyroscope only
fusionIMU.setFusionFactor(0) # discard accelerometer during fusion
acc = vs.Vector(0, 0, 1)
q = fusionIMU.init(acc, 1000000)
qTarget = vs.Quat(1, 0, 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)
acc = vs.Vector(0, 0, 1)
gyr = vs.Vector(angle, 0, 0)
q = fusionIMU.update(gyr, acc, 1033333) # integrate only during 1/30 of a second
qTarget = vs.Quat(math.cos(angle/(2.*30.)), math.sin(angle/(2.*30.)), 0, 0)
self.checkAlmostEqualQuaternions(q, qTarget)