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; }
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(); }