mirror of
https://github.com/ryujinx-mirror/ryujinx.git
synced 2024-10-21 10:41:41 +00:00
167 lines
5.2 KiB
C#
167 lines
5.2 KiB
C#
|
using System.Numerics;
|
|||
|
|
|||
|
namespace Ryujinx.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
|
|||
|
public 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;
|
|||
|
}
|
|||
|
}
|
|||
|
}
|