//private int CalibrationCount = 0; //locally stored count on the number of times calibrator has been called. public Merge(IDataProducer <VirtualSensorData> idealsensor, IDataProducer <InertialSensorData> actualsensor, int count) { //CalibrationCount = count; this.ActualSensor = actualsensor; IdealSensor = idealsensor; ActualSensor.NewTData += OnActualSensorNewTData; IdealSensor.NewTData += OnIdealSensorNewTData; virtualAcc = new CircularBuffer <AccelerationTime>((int)(KinectFPS * CalibrationLookBackTimeInSec)); //kinect actualAcc = new CircularBuffer <AccelerationTime>((int)(InertialMPS * (CalibrationLookBackTimeInSec + 1))); //sensor calibrators = new CircularBuffer <SensorCalibratorData>((int)(KinectFPS * CalibrationLookBackTimeInSec - 4)); //calibrators finalCalibrator = new SensorCalibratorData(); //DataTracker.CurrentSection = 999; }
/// <summary> /// Look back at X amount of seconds of acceleration data and generate appropriate corrections matrices and constants /// </summary> public void mathCalibrate() { int actualAccIndex = 0; double[] avgActualAcc = new double[] { 0, 0, 0 }; double[] avgAngularAcc = new double[] { 0, 0, 0 }; double[] idealAcc = new double[] { 0, 0, 0 }; int a = 0; finalCalibrator = new SensorCalibratorData(); CreateFile(); //NEED TO CONVERT FROM VIRTUAL TO IDEAL for (int i = 0; i < calibrators.Capacity && i + 2 < virtualAcc.size; i++) { int virtualAccIndex = i + 2; try { actualAccIndex = searchClosestTime(virtualAcc[i].Time, ref a); //Searches for the closest time in the actualAcc buffer to begin the calibration } catch { finalCalibrator.CalibrationFailed = true; //return; } avgActualAcc = averageActualAcc(actualAccIndex); //average 6 sensor accelertion vectors avgAngularAcc = averageAngularAcc(actualAccIndex); calibrators[i] = new SensorCalibratorData(); calibrators[i].setData(avgActualAcc, avgAngularAcc, virtualAcc[virtualAccIndex + 1].linearAcceleration, virtualAcc[virtualAccIndex + 1].IsGood); // Virtual is set back a frame to compensate for lag calibrators[i].generateCorrections(); Write(avgActualAcc, virtualAcc[virtualAccIndex + 1].linearAcceleration); } Close(); finalCalibrator.correctionConst = MathFunctions.correctionConstAvg(calibrators); finalCalibrator.Displacement = MathFunctions.linearDisplacementAvg(calibrators); //finalCalibrator.rollCorrection = MathFunctions.matrixAvg(calibrators, MathFunctions.MatrixType.Roll); //finalCalibrator.yawCorrection = MathFunctions.matrixAvg(calibrators, MathFunctions.MatrixType.Yaw); finalCalibrator.eulerAngles = MathFunctions.eulerAngleAvg(calibrators); finalCalibrator.rotquat = MathFunctions.rotQuatAvg(calibrators); }
public CorrectedInertialSensor(IDataProducer <InertialSensorData> imu, SensorCalibratorData calibrator) { accelerometer = imu; finalCalibrator = calibrator; accelerometer.NewTData += OnAccelNewTData; }