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
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
|
using System.Numerics;
namespace Ryujinx.Input.Motion
{
// MahonyAHRS class. Madgwick's implementation of Mayhony's AHRS algorithm.
// See: https://x-io.co.uk/open-source-imu-and-ahrs-algorithms/
// Based on: https://github.com/xioTechnologies/Open-Source-AHRS-With-x-IMU/blob/master/x-IMU%20IMU%20and%20AHRS%20Algorithms/x-IMU%20IMU%20and%20AHRS%20Algorithms/AHRS/MahonyAHRS.cs
class MotionSensorFilter
{
/// <summary>
/// Sample rate coefficient.
/// </summary>
public const float SampleRateCoefficient = 0.45f;
/// <summary>
/// Gets or sets the sample period.
/// </summary>
public float SamplePeriod { get; set; }
/// <summary>
/// Gets or sets the algorithm proportional gain.
/// </summary>
public float Kp { get; set; }
/// <summary>
/// Gets or sets the algorithm integral gain.
/// </summary>
public float Ki { get; set; }
/// <summary>
/// Gets the Quaternion output.
/// </summary>
public Quaternion Quaternion { get; private set; }
/// <summary>
/// Integral error.
/// </summary>
private Vector3 _intergralError;
/// <summary>
/// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
public MotionSensorFilter(float samplePeriod) : this(samplePeriod, 1f, 0f) { }
/// <summary>
/// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
/// <param name="kp">
/// Algorithm proportional gain.
/// </param>
public MotionSensorFilter(float samplePeriod, float kp) : this(samplePeriod, kp, 0f) { }
/// <summary>
/// Initializes a new instance of the <see cref="MotionSensorFilter"/> class.
/// </summary>
/// <param name="samplePeriod">
/// Sample period.
/// </param>
/// <param name="kp">
/// Algorithm proportional gain.
/// </param>
/// <param name="ki">
/// Algorithm integral gain.
/// </param>
public MotionSensorFilter(float samplePeriod, float kp, float ki)
{
SamplePeriod = samplePeriod;
Kp = kp;
Ki = ki;
Reset();
_intergralError = new Vector3();
}
/// <summary>
/// Algorithm IMU update method. Requires only gyroscope and accelerometer data.
/// </summary>
/// <param name="accel">
/// Accelerometer measurement in any calibrated units.
/// </param>
/// <param name="gyro">
/// Gyroscope measurement in radians.
/// </param>
public void Update(Vector3 accel, Vector3 gyro)
{
// Normalise accelerometer measurement.
float norm = 1f / accel.Length();
if (!float.IsFinite(norm))
{
return;
}
accel *= norm;
float q2 = Quaternion.X;
float q3 = Quaternion.Y;
float q4 = Quaternion.Z;
float q1 = Quaternion.W;
// Estimated direction of gravity.
Vector3 gravity = new Vector3()
{
X = 2f * (q2 * q4 - q1 * q3),
Y = 2f * (q1 * q2 + q3 * q4),
Z = q1 * q1 - q2 * q2 - q3 * q3 + q4 * q4
};
// Error is cross product between estimated direction and measured direction of gravity.
Vector3 error = new Vector3()
{
X = accel.Y * gravity.Z - accel.Z * gravity.Y,
Y = accel.Z * gravity.X - accel.X * gravity.Z,
Z = accel.X * gravity.Y - accel.Y * gravity.X
};
if (Ki > 0f)
{
_intergralError += error; // Accumulate integral error.
}
else
{
_intergralError = Vector3.Zero; // Prevent integral wind up.
}
// Apply feedback terms.
gyro += (Kp * error) + (Ki * _intergralError);
// Integrate rate of change of quaternion.
Vector3 delta = new Vector3(q2, q3, q4);
q1 += (-q2 * gyro.X - q3 * gyro.Y - q4 * gyro.Z) * (SampleRateCoefficient * SamplePeriod);
q2 += (q1 * gyro.X + delta.Y * gyro.Z - delta.Z * gyro.Y) * (SampleRateCoefficient * SamplePeriod);
q3 += (q1 * gyro.Y - delta.X * gyro.Z + delta.Z * gyro.X) * (SampleRateCoefficient * SamplePeriod);
q4 += (q1 * gyro.Z + delta.X * gyro.Y - delta.Y * gyro.X) * (SampleRateCoefficient * SamplePeriod);
// Normalise quaternion.
Quaternion quaternion = new Quaternion(q2, q3, q4, q1);
norm = 1f / quaternion.Length();
if (!float.IsFinite(norm))
{
return;
}
Quaternion = quaternion * norm;
}
public void Reset()
{
Quaternion = Quaternion.Identity;
}
}
}
|
