-
Notifications
You must be signed in to change notification settings - Fork 0
/
RotorIy.cpp
67 lines (55 loc) · 1.64 KB
/
RotorIy.cpp
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
// Copyright (C) 2018 kaz Kojima
//
// This file is part of RotorIy program. This program is free
// software; you can redistribute it and/or modify it under the
// terms of the GNU General Public License as published by the
// Free Software Foundation; either version 3, or (at your option)
// any later version.
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
// You should have received a copy of the GNU General Public License
// along with this program;
// see the file COPYING.
#include "c3ga.h"
using namespace c3ga;
#include "RotorIy.h"
RotorIy::RotorIy (float dt, float gain, float epsilon):
m_dt(dt), m_gain(gain), m_epsilon(epsilon)
{
m_S = 1.0;
m_I = 0.0;
}
RotorIy::~RotorIy ()
{
}
mv
RotorIy::UpdateIMU (float ax, float ay, float az, float gx, float gy, float gz)
{
// Gyro. bivector and accel. directional vector.
mv X = m_dt*(gx*(e2^e3) + gy*(e3^e1) + gz*(e1^e2));
mv y = ax*e1 + ay*e2 + az*e3;
// mv v = m_S*(-e3)/m_S;
mv v = applyVersor (reverse(m_S), -e3);
mv dS;
float nm = norm (y+v);
// Don't fuse if y+v is too short.
if (m_gain > 0 && nm > m_norm_threshold)
{
mv P = ((1.0/nm)*(y+v))*v;
mv Y = -2.0*log (P);
m_I = (1 - m_epsilon)*m_I + m_epsilon*X;
X -= m_I;
dS = exp (-0.5*(-m_gain*m_dt*Y + X));
}
else
{
// Fall back to the dead reckoning.
m_I = (1 - m_epsilon)*m_I + m_epsilon*X;
X -= m_I;
dS = exp (-0.5*X);
}
m_S = dS*m_S;
return m_S;
}