//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;
}
