void Awake()
{
if (instance != null)
{
UnityEngine.Debug.LogError("More than an instance of SensorFusion detected!");
}
instance = this;
if (SystemInfo.supportsGyroscope)
{
filter = new ComplementaryFilter(.98f);
Input.gyro.enabled = true;
Input.gyro.updateInterval = 0.0167f; // Set the update interval to it's highest value (60 Hz)
}
else
{
Debug.LogWarning("This device doesn't have a gyroscope! Using accelerometer only (if available)");
filter = new ComplementaryFilter(.5f);
}
posePredictor = new PosePredictor();
filterToWorldQ = Quaternion.AngleAxis((-Mathf.PI / 2) * Mathf.Rad2Deg, new Vector3(1, 0, 0));
originalPoseAdjustQ = Quaternion.AngleAxis((-windowOrientation * Mathf.PI / 18) * Mathf.Rad2Deg, new Vector3(0, 0, 1));
SetScreenTransform();
if (isLandscape)
{
filterToWorldQ *= inverseWorldToScreenQ;
}
Recenter();
}
public LSM9DS1ImuSensor(
byte accelGyroI2CAddress,
byte magI2CAddress,
LSM9DS1Config config,
SensorFusion fusion)
: base(fusion)
{
_accelGyroI2CAddress = accelGyroI2CAddress;
_magI2CAddress = magI2CAddress;
_config = config;
SampleRate = 100;
}
public LSM9DS1ImuSensor(
byte accelGyroI2CAddress,
byte magI2CAddress,
LSM9DS1Config config,
SensorFusion fusion)
: base(fusion)
{
_accelGyroI2CAddress = accelGyroI2CAddress;
_magI2CAddress = magI2CAddress;
_config = config;
SampleRate = 100;
}
protected long SampleInterval; // interval between samples in microseonds
protected ImuSensor(SensorFusion fusion)
{
Fusion = fusion;
AxisRotation = new AxisRotation();
for (int i = 0; i < 3; i++)
{
_magMax[i] = -1000.0;
_magMin[i] = 1000.0;
}
}
private async void Ready(object sender, RoutedEventArgs e)
{
//brick = new Brick(new BluetoothCommunication("COM10"), true); // Jeppe
//brick = new Brick(new BluetoothCommunication("COM3"), true); // Kim1
brick = new Brick(new BluetoothCommunication("COM5"), true); // Kim2
await brick.ConnectAsync();
await brick.DirectCommand.PlayToneAsync(100, 440, 500);
Sensors = new SensorFusion(brick);
motionControle = new Motion(brick);
}
private void DataProcessing(object sender, EventArgs e)
{
#region Calculate dt
if (stopwatch2.IsRunning)
{
stopwatch2.Stop();
dt = stopwatch2.Elapsed.TotalSeconds;
//tempo.Text = dt.ToString();
}
stopwatch2.Restart();
#endregion
#region EKF imu
Q = new double[4, 4] {
{ 0.007, 0, 0, 0 },
{ 0, 0.007, 0, 0 },
{ 0, 0, 0.007, 0 },
{ 0, 0, 0, 0.007 }
};
R1 = new double[3, 3] {
{ 1.0f, 0, 0 },
{ 0, 1.0f, 0 },
{ 0, 0, 1.0f }
};
SensorFusion.EKFimu(qEKFIMU, pEKFIMU, Q, R1, dt, accx, accy, accz, gyrx, gyry, gyrz, out qEKFIMU, out pEKFIMU);
if (EKFIMU)
{
qFinal = qEKFIMU;
}
#endregion
#region SebMadgwick
beta = Math.Sqrt(3.0f / 4.0f) * (Math.PI * (20.0f / 180.0f));
zeta = Math.Sqrt(3.0f / 4.0f) * (Math.PI * (00.0f / 180.0f));
SensorFusion.Madgwick(qMadgwick, beta, zeta, dt, accx, accy, accz, gyrx, gyry, gyrz, magx, magy, magz, out qMadgwick);
EulerMadgwick = SensorFusion.QuaternionToEuler(qMadgwick);
rollMadgwick = SensorFusion.ConvertToDegrees(EulerMadgwick[0, 0]);
pitchMadgwick = SensorFusion.ConvertToDegrees(EulerMadgwick[1, 0]);
yawMadgwick = SensorFusion.ConvertToDegrees(EulerMadgwick[2, 0]);
if (Madgwick)
{
qFinal = qMadgwick;
}
#endregion
#region Mahony
Kp = 2.0f * 1.0f;
Ki = 0.0f;
SensorFusion.MahonyQuaternionAHRS(qMahony, Kp, Ki, dt, accx, accy, accz, gyrx, gyry, gyrz, magx, magy, magz, out qMahony);
EulerMahony = SensorFusion.QuaternionToEuler(qMahony);
rollMahony = SensorFusion.ConvertToDegrees(EulerMahony[0, 0]);
pitchMahony = SensorFusion.ConvertToDegrees(EulerMahony[1, 0]);
yawMahony = SensorFusion.ConvertToDegrees(EulerMahony[2, 0]);
if (Mahony)
{
qFinal = qMahony;
}
Kp = 2.0f * 1.0f;
Ki = 0.0f;
SensorFusion.MahonyQuaternionUpdateIMU(qMahonyIMU, Kp, Ki, dt, accx, accy, accz, gyrx, gyry, gyrz, out qMahonyIMU);
if (MahonyIMU)
{
qFinal = qMahonyIMU;
}
#endregion
#region DoubleStageEKF
Q = new double[4, 4] {
{ 0.007, 0, 0, 0 },
{ 0, 0.007, 0, 0 },
{ 0, 0, 0.007, 0 },
{ 0, 0, 0, 0.007 }
};
R1 = new double[3, 3] {
{ 1.0f, 0, 0 },
{ 0, 1.0f, 0 },
{ 0, 0, 1.0f }
};
R2 = new double[3, 3] {
{ 2.0f, 0, 0 },
{ 0, 2.0f, 0 },
{ 0, 0, 2.0f }
};
SensorFusion.DoubleStageEKF(pEKF, qEKF, Q, R1, R2, dt, gyrx, gyry, gyrz, accx, accy, accz, magx, magy, magz, out qEKF, out pEKF);
EulerEKF = SensorFusion.QuaternionToEuler(qEKF);
rollEKF = SensorFusion.ConvertToDegrees(EulerEKF[0, 0]);
pitchEKF = SensorFusion.ConvertToDegrees(EulerEKF[1, 0]);
yawEKF = SensorFusion.ConvertToDegrees(EulerEKF[2, 0]);
if (EKFDS)
{
qFinal = qEKF;
}
#endregion
#region Gauss-Newton
gain = 0.75;
SensorFusion.GaussNewtonComplementar(qComplementarGN, qOsservGN, gain, dt, accx, accy, accz, magx, magy, magz, gyrx, gyry, gyrz, out qComplementarGN, out qOsservGN);
EulerGNCF = SensorFusion.QuaternionToEuler(qComplementarGN);
rollGNCF = SensorFusion.ConvertToDegrees(EulerGNCF[0, 0]);
pitchGNCF = SensorFusion.ConvertToDegrees(EulerGNCF[1, 0]);
yawGNCF = SensorFusion.ConvertToDegrees(EulerGNCF[2, 0]);
if (GNCF)
{
qFinal = qComplementarGN;
}
Q = new double[4, 4] {
{ 0.007, 0, 0, 0 },
{ 0, 0.007, 0, 0 },
{ 0, 0, 0.007, 0 },
{ 0, 0, 0, 0.007 }
};
R = new double[4, 4] {
{ 0.5f, 0, 0, 0 },
{ 0, 0.5f, 0, 0 },
{ 0, 0, 0.5f, 0 },
{ 0, 0, 0, 0.5f }
};
SensorFusion.GaussNewtonKalmanFilter(qGNKF, pGNKF, Q, R, dt, accx, accy, accz, magx, magy, magz, gyrx, gyry, gyrz, out qGNKF, out pGNKF);
EulerGNKF = SensorFusion.QuaternionToEuler(qGNKF);
rollGNKF = SensorFusion.ConvertToDegrees(EulerGNKF[0, 0]);
pitchGNKF = SensorFusion.ConvertToDegrees(EulerGNKF[1, 0]);
yawGNKF = SensorFusion.ConvertToDegrees(EulerGNKF[2, 0]);
if (GNKF)
{
qFinal = qGNKF;
}
#endregion
#region Get Euler
//datax[count] = gyrx;
//datay[count] = gyry;
//dataz[count] = gyrz;
//count++;
//if (count == N)
//{
// SensorFusion.GaussianEval(datax, datay, dataz, N, th, out motionx, out motiony, out motionz);
// count = 0;
//}
//if (motionx == 1 || motiony == 1 || motionz == 1)
//{
Euler = SensorFusion.QuaternionToEuler(qFinal);
roll = SensorFusion.ConvertToDegrees(Euler[0, 0]);
pitch = SensorFusion.ConvertToDegrees(Euler[1, 0]);
yaw = SensorFusion.ConvertToDegrees(Euler[2, 0]);
//}
#endregion
#region Remove Gravity
if (WithoutGravity.Checked == true)
{
SensorFusion.RemoveGravityQuatEarthFrame(accx, accy, accz, qFinal, out RealAccx, out RealAccy, out RealAccz);
}
#endregion
#region Display Data on Graphs
if (Acc.Series["Acc X"].Points.Count == 100)
{
Acc.Series["Acc X"].Points.RemoveAt(0);
Acc.Series["Acc Y"].Points.RemoveAt(0);
Acc.Series["Acc Z"].Points.RemoveAt(0);
}
if (WithoutGravity.Checked == true)
{
Acc.Series["Acc X"].Points.AddY(RealAccx);
Acc.Series["Acc Y"].Points.AddY(RealAccy);
Acc.Series["Acc Z"].Points.AddY(RealAccz);
Acc.ChartAreas["ChartArea1"].RecalculateAxesScale();
}
else
{
Acc.Series["Acc X"].Points.AddY(accx);
Acc.Series["Acc Y"].Points.AddY(accy);
Acc.Series["Acc Z"].Points.AddY(accz);
Acc.ChartAreas["ChartArea1"].RecalculateAxesScale();
}
if (Gyr.Series["Gyr X"].Points.Count == 100)
{
Gyr.Series["Gyr X"].Points.RemoveAt(0);
Gyr.Series["Gyr Y"].Points.RemoveAt(0);
Gyr.Series["Gyr Z"].Points.RemoveAt(0);
}
Gyr.Series["Gyr X"].Points.AddY(gyrx);
Gyr.Series["Gyr Y"].Points.AddY(gyry);
Gyr.Series["Gyr Z"].Points.AddY(gyrz);
Gyr.ChartAreas["ChartArea1"].RecalculateAxesScale();
if (Mag.Series["Mag X"].Points.Count == 100)
{
Mag.Series["Mag X"].Points.RemoveAt(0);
Mag.Series["Mag Y"].Points.RemoveAt(0);
Mag.Series["Mag Z"].Points.RemoveAt(0);
}
Mag.Series["Mag X"].Points.AddY(magx);
Mag.Series["Mag Y"].Points.AddY(magy);
Mag.Series["Mag Z"].Points.AddY(magz);
Mag.ChartAreas["ChartArea1"].RecalculateAxesScale();
if (EulerGraph.Series["Roll"].Points.Count == 100)
{
EulerGraph.Series["Roll"].Points.RemoveAt(0);
EulerGraph.Series["Pitch"].Points.RemoveAt(0);
EulerGraph.Series["Yaw"].Points.RemoveAt(0);
}
EulerGraph.Series["Roll"].Points.AddY(roll);
EulerGraph.Series["Pitch"].Points.AddY(pitch);
EulerGraph.Series["Yaw"].Points.AddY(yaw);
EulerGraph.ChartAreas["ChartArea1"].RecalculateAxesScale();
#endregion
#region Write to File
if (start_log.Checked == true)
{
string gnuplotDataString = "";
gnuplotDataString += accx.ToString().Replace(",", ".") + " ";
gnuplotDataString += accy.ToString().Replace(",", ".") + " ";
gnuplotDataString += accz.ToString().Replace(",", ".") + " ";
gnuplotDataString += gyrx.ToString().Replace(",", ".") + " ";
gnuplotDataString += gyry.ToString().Replace(",", ".") + " ";
gnuplotDataString += gyrz.ToString().Replace(",", ".") + " ";
gnuplotDataString += magx.ToString().Replace(",", ".") + " ";
gnuplotDataString += magy.ToString().Replace(",", ".") + " ";
gnuplotDataString += magz.ToString().Replace(",", ".") + " ";
gnuplotDataString += qMadgwick[0, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMadgwick[1, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMadgwick[2, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMadgwick[3, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += rollMadgwick.ToString().Replace(",", ".") + " ";
gnuplotDataString += pitchMadgwick.ToString().Replace(",", ".") + " ";
gnuplotDataString += yawMadgwick.ToString().Replace(",", ".") + " ";
gnuplotDataString += qMahony[0, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMahony[1, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMahony[2, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qMahony[3, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += rollMahony.ToString().Replace(",", ".") + " ";
gnuplotDataString += pitchMahony.ToString().Replace(",", ".") + " ";
gnuplotDataString += yawMahony.ToString().Replace(",", ".") + " ";
gnuplotDataString += qEKF[0, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qEKF[1, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qEKF[2, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qEKF[3, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += rollEKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += pitchEKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += yawEKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += qComplementarGN[0, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qComplementarGN[1, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qComplementarGN[2, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qComplementarGN[3, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += rollGNCF.ToString().Replace(",", ".") + " ";
gnuplotDataString += pitchGNCF.ToString().Replace(",", ".") + " ";
gnuplotDataString += yawGNCF.ToString().Replace(",", ".") + " ";
gnuplotDataString += qGNKF[0, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qGNKF[1, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qGNKF[2, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += qGNKF[3, 0].ToString().Replace(",", ".") + " ";
gnuplotDataString += rollGNKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += pitchGNKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += yawGNKF.ToString().Replace(",", ".") + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += RealAccx.ToString().Replace(",", ".") + " ";
gnuplotDataString += RealAccy.ToString().Replace(",", ".") + " ";
gnuplotDataString += RealAccz.ToString().Replace(",", ".") + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += "1" + " ";
gnuplotDataString += dt.ToString().Replace(",", ".") + " ";
WriteGnuplotFile(gnuplotDataString, path);
}
#endregion
#region Display Quaternions TextBox
FusedQuatWText.Text = qFinal[0, 0].ToString();
FusedQuatXText.Text = qFinal[1, 0].ToString();
FusedQuatYText.Text = qFinal[2, 0].ToString();
FusedQuatZText.Text = qFinal[3, 0].ToString();
FusedRollText.Text = roll.ToString();
FusedPitchText.Text = pitch.ToString();
FusedYawText.Text = yaw.ToString();
#endregion
#region Update 3D Cube
glControl1.Invalidate();
#endregion
}
// Use this for initialization
void Start()
{
SensorFusion.Recenter();
}
// Update is called once per frame
void Update()
{
transform.rotation = SensorFusion.GetOrientation();
}