public void CalcFilterCoeffs(MEMSReadingsSet3f readings, out double gyroCoeff, out double accMagnetCoeff, double initAccMagnet, double initGyro) { var gyroVect = readings.GyroVector3f.Values; var gyroMagn = Math.Sqrt(gyroVect[0] * gyroVect[0] + gyroVect[1] * gyroVect[1] + gyroVect[2] * gyroVect[2]); double gs = 2; double gm = 1; double gmin = 0.5; double gmax = 0.8; var gCoeff = gmin + (1 - 1 / (1 + gyroMagn * gm)) * (gmax - gmin); gyroCoeff = gCoeff; accMagnetCoeff = initAccMagnet; Console.WriteLine("gyroCoeff: " + gyroCoeff.ToString() + " ; magnetCoeff: " + accMagnetCoeff.ToString()); }
// protected void Init() { this._isStarted = false; this._isPaused = false; //this._is_videoSourceCap_ImageGrabbed_inUse = false; //this._is_StereoMEMSDataProviderCVCapFromFile__nextAccValEvent_inUse = false; //this._is_StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent_inUse = false; //this._is_StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent_inUse = false; //this._is_StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent_inUse = false; this._isNewAccVal = false; this._isNewMagnetVal = false; this._isNewGyroVal = false; _isVideoStreamExpired = false; _isAccStreamExpired = false; _isMagnetStreamExpired = false; _isGyroStreamExpired = false; this._currentMEMSSet = new MEMSReadingsSet3f(); if (this._useMEMS) { this._currentMEMSSet.AccVector3f = this.GetNextAccVector3f(); this._currentMEMSSet.MagnetVector3f = this.GetNextMagnetVector3f(); this._currentMEMSSet.GyroVector3f = this.GetNextGyroVector3f(); this._startMEMSTimeStampIOrig = this._currentMEMSSet.AccVector3f.TimeStampI; this._nextAccValEvent += StereoMEMSDataProviderCVCapFromFile__nextAccValEvent; this._nextMagnetValEvent += StereoMEMSDataProviderCVCapFromFile__nextMagnetValEvent; this._nextGyroValEvent += StereoMEMSDataProviderCVCapFromFile__nextGyroValEvent; } if (this._useVideo) { var rawFrame = this._videoSourceCap.QueryFrame(); this._currentStereoFrame = this.ElementFromRawFrame(rawFrame); rawFrame = null; this._nextVideoFrameEvent += StereoMEMSDataProviderCVCapFromFile__nextVideoFrameEvent; this._isFirstVideoFrame = true; } }
protected MEMSReadingsSet3f GetResultMEMSSet() { var res = new MEMSReadingsSet3f() { AccVector3f = new ReadingsVector3f(this._currentMEMSSet.AccVector3f), MagnetVector3f = new ReadingsVector3f(this._currentMEMSSet.MagnetVector3f), GyroVector3f = new ReadingsVector3f(this._currentMEMSSet.GyroVector3f) }; res.AccVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.AccVector3f.TimeStampI); res.MagnetVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.MagnetVector3f.TimeStampI); res.GyroVector3f.TimeStampI = this.RecalcMEMSTimeStamp(res.GyroVector3f.TimeStampI); return res; }
public override MEMSReadingsSet3f GetNextReadingsSet() { MEMSReadingsSet3f res = new MEMSReadingsSet3f(); res.AccVector3f = this.GetNextAccVector3f(); res.MagnetVector3f = this.GetNextMagnetVector3f(); res.GyroVector3f = this.GetNextGyroVector3f(); this.CurrentReadingsSet3f = res; return res; }
protected MEMSReadingsSet3f ConvertToMEMSReadingsSet3f(byte[] buffer) { if (buffer.Length != RecMessageSize) { return null; } MEMSReadingsSet3f res = new MEMSReadingsSet3f(); //res.AccVector3f = this.GetNextAccVector3f(); //res.MagnetVector3f = this.GetNextMagnetVector3f(); //res.GyroVector3f = this.GetNextGyroVector3f(); return res; }
public double[][] GetAccMagnetOrientationMatrix(MEMSReadingsSet3f newReadings, bool useAccMagnet, bool useGyroscope, bool useLowpassFilter, bool useAdaptiveFiltering, double accMagnetFilterCoeff, double gyroFilterCoeff) { if (useAdaptiveFiltering) { this.CalcFilterCoeffs(newReadings, out gyroFilterCoeff, out accMagnetFilterCoeff, accMagnetFilterCoeff, gyroFilterCoeff); } double[][] res = new double[3][]; for (int i = 0; i < 3; ++i) { res[i] = new double[3]; } MCvPoint3D64f rawAccPoint = new MCvPoint3D64f(newReadings.AccVector3f.Values[0], newReadings.AccVector3f.Values[1], newReadings.AccVector3f.Values[2]); MCvPoint3D64f rawMagnetPoint = new MCvPoint3D64f(newReadings.MagnetVector3f.Values[0], newReadings.MagnetVector3f.Values[1], newReadings.MagnetVector3f.Values[2]); double gyroSpeedMul = 1; if (useAdaptiveFiltering) { if(useAccMagnet) { gyroSpeedMul = this.GyroSpeedMul; } } var gyroDiffMatr = this.GetGyroRotationMatrix(newReadings.GyroVector3f, gyroSpeedMul); MCvPoint3D64f accFilteredPoint; MCvPoint3D64f magnetFilteredPoint; if (useLowpassFilter && this.OldAMGFusedAcc != null && this.OldAMGFusedMagnet != null) { if (useAccMagnet) { //this.FilterAccMagnet(ref resAccPoint, ref resMagnetPoint, accMagnetFilterCoeff); accFilteredPoint = new MCvPoint3D64f( rawAccPoint.x * accMagnetFilterCoeff + OldAMGFusedAcc.Value.x * (1 - accMagnetFilterCoeff), rawAccPoint.y * accMagnetFilterCoeff + OldAMGFusedAcc.Value.y * (1 - accMagnetFilterCoeff), rawAccPoint.z * accMagnetFilterCoeff + OldAMGFusedAcc.Value.z * (1 - accMagnetFilterCoeff) ); magnetFilteredPoint = new MCvPoint3D64f( rawMagnetPoint.x * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.x * (1 - accMagnetFilterCoeff), rawMagnetPoint.y * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.y * (1 - accMagnetFilterCoeff), rawMagnetPoint.z * accMagnetFilterCoeff + OldAMGFusedMagnet.Value.z * (1 - accMagnetFilterCoeff) ); } else { accFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedAcc.Value.x, this.OldAMGFusedAcc.Value.y, this.OldAMGFusedAcc.Value.z); magnetFilteredPoint = new MCvPoint3D64f(this.OldAMGFusedMagnet.Value.x, this.OldAMGFusedMagnet.Value.y, this.OldAMGFusedMagnet.Value.z); } } else { accFilteredPoint = new MCvPoint3D64f(rawAccPoint.x, rawAccPoint.y, rawAccPoint.z); magnetFilteredPoint = new MCvPoint3D64f(rawMagnetPoint.x, rawMagnetPoint.y, rawMagnetPoint.z); } MCvPoint3D64f resAccPoint = new MCvPoint3D64f(0, 9, 0); MCvPoint3D64f resMagnetPoint = new MCvPoint3D64f(48, 2, 0); if(this.OldAMGFusedAcc == null || this.OldAMGFusedMagnet == null) { resAccPoint = accFilteredPoint; resMagnetPoint = magnetFilteredPoint; } else { if (useGyroscope && useAccMagnet) { resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(accFilteredPoint).Mul(1 - gyroFilterCoeff))); resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr).Mul(gyroFilterCoeff).Add(MCvPoint3D64fToMatrix(magnetFilteredPoint).Mul(1 - gyroFilterCoeff))); } else if (useAccMagnet) { resAccPoint = accFilteredPoint; resMagnetPoint = magnetFilteredPoint; } else if(useGyroscope) { resAccPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedAcc.Value).Mul(gyroDiffMatr)); resMagnetPoint = MatrixToMCvPoint3D64f(MCvPoint3D64fToMatrix(OldAMGFusedMagnet.Value).Mul(gyroDiffMatr)); } } this.OldAMGFusedAcc = resAccPoint; this.OldAMGFusedMagnet = resMagnetPoint; this.OldRawAcc = rawAccPoint; this.OldRawMagnet = rawMagnetPoint; this.OldFilteredAcc = accFilteredPoint; this.OldFilteredMagnet = magnetFilteredPoint; this.OldGyroReadings = newReadings.GyroVector3f; MCvPoint3D64f x; MCvPoint3D64f y; MCvPoint3D64f z; this.GetOrthoNormalAccMagnetBasis(resAccPoint, resMagnetPoint, out x, out y, out z); Matrix<double> accMagnetMatrix = new Matrix<double>(3, 3); accMagnetMatrix[0, 0] = x.x; accMagnetMatrix[0, 1] = x.y; accMagnetMatrix[0, 2] = x.z; accMagnetMatrix[1, 0] = y.x; accMagnetMatrix[1, 1] = y.y; accMagnetMatrix[1, 2] = y.z; accMagnetMatrix[2, 0] = z.x; accMagnetMatrix[2, 1] = z.y; accMagnetMatrix[2, 2] = z.z; //AccMagnetGyro fuse Matrix<double> newGAMFusedMatrix = null; //old amg fusion //if (this.OldGAMFusedMatrix == null) //{ // newGAMFusedMatrix = accMagnetMatrix; //} //else //{ // var newGyroMatrix = this.OldGAMFusedMatrix.Mul(gyroDiffMatr); // newGAMFusedMatrix = this.FuseAccMagnetGyro(accMagnetMatrix, newGyroMatrix, gyroFilterCoeff); //} newGAMFusedMatrix = accMagnetMatrix; OldGAMFusedMatrix = newGAMFusedMatrix; //// var resMatrix = newGAMFusedMatrix.Transpose(); //res = this.ConvertMatrix(m); //gyro test //m = this.OldGyroMatrix; //m = m.Transpose(); // //auto res set up //for (int i = 0; i < 3; ++i) //{ // for (int j = 0; j < 3; ++j) // { // res[i][j] = resMatrix[i, j]; // } //} //// //manual res set up res[0][0] = -resMatrix[0, 0]; res[0][1] = -resMatrix[0, 1]; res[0][2] = resMatrix[0, 2]; res[1][0] = -resMatrix[1, 0]; res[1][1] = -resMatrix[1, 1]; res[1][2] = resMatrix[1, 2]; res[2][0] = -resMatrix[2, 0]; res[2][1] = -resMatrix[2, 1]; res[2][2] = resMatrix[2, 2]; //// return res; }
private void ProcessMEMSReadings(MEMSReadingsSet3f nextReadings) { if (this.isOrientCalcInUse) { return; } this.isOrientCalcInUse = true; if (this.IsActualReadingsSet(nextReadings)) { double gyroCoeff = 0.8; double accMagnetCoeff = (double)this.accMagnetFilterTrackBar.Value / this.accMagnetFilterTrackBar.Maximum; bool useAccMagnet = this.accMagnetCheckBox.Checked; bool useGyro = this.gyroCheckBox.Checked; bool useAdoptiveFilter = this.adoptiveFilterCheckBox.Checked; this.ReadingsTestOuptut(nextReadings); var orientMatr3f = this.OrientationCalc.GetOrientationMatrix( newReadings: nextReadings, useAccMagnet: useAccMagnet, useGyroscope: useGyro, useLowpassFilter: true, useAdaptiveFiltering: useAdoptiveFilter, accMagnetFilterCoeff: accMagnetCoeff, gyroFilterCoeff: gyroCoeff); lock (this.memsOrientationMatrLock) { this.memsRotMatr = orientMatr3f; } //var res = this.MulReadingsVect(orientMatr3f, nextReadings.AccVector3f, true); //log readings check //this.MEMSRTBLogger.WriteLn("readings check"); //this.PrintVector(this.MEMSRTBLogger, res); //this.MEMSRTBLogger.WriteLn(""); //// ////log matrix //this.MEMSRTBLogger.WriteLn("orientationMatrix:"); //this.PrintMatrix(this.MEMSRTBLogger, orientMatr3f); //this.MEMSRTBLogger.WriteLn(""); //// } this.isOrientCalcInUse = false; }
private bool IsActualReadingsSet(MEMSReadingsSet3f set) { if (set == null) { return false; } if (!set.IsNotEmpty()) { return false; } if (set.TimeStampOrigI == 0) { return false; } return true; }
private void ReadingsTestOuptut(MEMSReadingsSet3f nextReadings) { //nextReadings = this.MemsProvider.GetNextReadingsSet(); //MEMS READINGS to enable - comment return; // Console.WriteLine("timestamp: " + nextReadings.TimeStampOrigI.ToString()); Console.WriteLine("acc: " + " Xa=" + nextReadings.AccVector3f.Values[0] + " Ya=" + nextReadings.AccVector3f.Values[1] + " Za=" + nextReadings.AccVector3f.Values[2] + "\nmagnet: " + " Xm=" + nextReadings.MagnetVector3f.Values[0] + " Ym=" + nextReadings.MagnetVector3f.Values[1] + " Zm=" + nextReadings.MagnetVector3f.Values[2] + "\ngyro: " + " Xg=" + nextReadings.GyroVector3f.Values[0] + " Yg=" + nextReadings.GyroVector3f.Values[1] + " Zg=" + nextReadings.GyroVector3f.Values[2] + "\n\n"); }
public MEMSReadingsSet3f(MEMSReadingsSet3f original) { this.AccVector3f = new ReadingsVector3f(original.AccVector3f); this.MagnetVector3f = new ReadingsVector3f(original.MagnetVector3f); this.GyroVector3f = new ReadingsVector3f(original.GyroVector3f); }