public static Quaternion MultiplyQuaternion(Quaternion a, Quaternion b)
{
ThreeDimensionalVector vp = new ThreeDimensionalVector(a.Y * b.Z - a.Z * b.Y, a.Z * b.X - a.X * b.Z, a.X * b.Y - a.Y * b.X);
Quaternion ab = new Quaternion(a.T * b.T, a.T * b.X + b.T * a.X + vp.X, a.T * b.Y + b.T * a.Y + vp.Y, a.T * b.Z + b.T * a.Z + vp.Z);
return(ab);
}
public void OperatorPlus_WithTwoPosiniveVector_GetPositiveVector()
{
var threeDimVector1 = new ThreeDimensionalVector(5, 10, 6);
var threeDimVector2 = new ThreeDimensionalVector(8, 14, 3);
ThreeDimensionalVector result = threeDimVector1 + threeDimVector2;
var actualResult = new ThreeDimensionalVector(13, 24, 9);
Assert.AreEqual(result, actualResult);
}
public void OperatorMultiply_WhenNumberIsZero_GetZeroVector()
{
var threeDimVector1 = new ThreeDimensionalVector(-10, -14, -25);
int number = 0;
var result = threeDimVector1 * number;
var actualResult = new ThreeDimensionalVector(0, 0, 0);
Assert.AreEqual(result, actualResult);
}
public void OperatorMultiply_WhenVectorIsNegativeNumberIsPositive_GetNegativeVector()
{
var threeDimVector1 = new ThreeDimensionalVector(-10, -14, -25);
int number = 4;
ThreeDimensionalVector result = threeDimVector1 * number;
var actualResult = new ThreeDimensionalVector(-40, -56, -100);
Assert.AreEqual(result, actualResult);
}
public void OperatorMinus_WhenFirstVectorIsNegativeSecondVectorIsPositive_GetNegativeVector()
{
var threeDimVector1 = new ThreeDimensionalVector(-9, -4, -2);
var threeDimVector2 = new ThreeDimensionalVector(9, 4, 2);
ThreeDimensionalVector result = threeDimVector1 - threeDimVector2;
var actualResult = new ThreeDimensionalVector(-18, -8, -4);
Assert.AreEqual(result, actualResult);
}
public void OperatorDivision_WhenFirstVectorIsNegativeNumberIsPositive_GetNegativeVector()
{
var threeDimVector1 = new ThreeDimensionalVector(10, 14, 6);
int number = -2;
ThreeDimensionalVector result = threeDimVector1 / number;
var actualResult = new ThreeDimensionalVector(-5, -7, -3);
Assert.AreEqual(result, actualResult);
}
public void CrossVectors_WithTwoPositiveVectors_GetPositiveVector()
{
var threeDimVector1 = new ThreeDimensionalVector(7, 1, 5);
var threeDimVector2 = new ThreeDimensionalVector(9, 4, 2);
ThreeDimensionalVector result = ThreeDimensionalVector.CrossVectors(threeDimVector1, threeDimVector2);
var actualResult = new ThreeDimensionalVector(-18, 31, 19);
Assert.AreEqual(result, actualResult);
}
public void OperatorPlus_WithTwoNegativeVector_GetNegativeVector()
{
var threeDimVector1 = new ThreeDimensionalVector(-12, -10, -8);
var threeDimVector2 = new ThreeDimensionalVector(-18, -15, -16);
ThreeDimensionalVector result = threeDimVector1 + threeDimVector2;
var actualResult = new ThreeDimensionalVector(-30, -25, -24);
Assert.AreEqual(result, actualResult);
}
public void OperatorMinus_WhenFirstVectorEqualSecondVector_GetZeroVector()
{
var threeDimVector1 = new ThreeDimensionalVector(9, 4, 2);
var threeDimVector2 = new ThreeDimensionalVector(9, 4, 2);
ThreeDimensionalVector result = threeDimVector1 - threeDimVector2;
var actualResult = new ThreeDimensionalVector(0, 0, 0);
Assert.AreEqual(result, actualResult);
}
public void OperatorMinus_WhenFirstVectorMoreSecondVector_GetPositiveVector()
{
var threeDimVector1 = new ThreeDimensionalVector(14, 10, 18);
var threeDimVector2 = new ThreeDimensionalVector(12, 7, 8);
ThreeDimensionalVector result = threeDimVector1 - threeDimVector2;
var actualResult = new ThreeDimensionalVector(2, 3, 10);
Assert.AreEqual(result, actualResult);
}
public void OperatorMinus_WhenFirstVectorLessSecondVector_GetNegativeVector()
{
var threeDimVector1 = new ThreeDimensionalVector(8, 4, 2);
var threeDimVector2 = new ThreeDimensionalVector(15, 7, 4);
ThreeDimensionalVector result = threeDimVector1 - threeDimVector2;
var actualResult = new ThreeDimensionalVector(-7, -3, -2);
Assert.AreEqual(result, actualResult);
}
public void OperatorPlus_WhenFirstVectorIsOppositeSecondVector_GetZeroVector()
{
var threeDimVector1 = new ThreeDimensionalVector(-12, -10, -8);
var threeDimVector2 = new ThreeDimensionalVector(12, 10, 8);
ThreeDimensionalVector result = threeDimVector1 + threeDimVector2;
var actualResult = new ThreeDimensionalVector(0, 0, 0);
Assert.AreEqual(result, actualResult);
}
public void OperatorMultiply_WithTwoPositiveVectors_GetPositiveVector()
{
var threeDimVector1 = new ThreeDimensionalVector(7, 1, 5);
var threeDimVector2 = new ThreeDimensionalVector(9, 4, 2);
int result = threeDimVector1 * threeDimVector2;
int actualResult = 77;
Assert.AreEqual(result, actualResult);
}
bool Calibrate(CalibrationAxes calibrationAxes)
{
bool result = false;
lock (sampleBuffer)
{
if (CanCalibrate(calibrationAxes))
{
CalibrationOffset = new ThreeDimensionalVector(
calibrationAxes == CalibrationAxes.XAxis ? -averageAcceleration.X : CalibrationOffset.X,
calibrationAxes == CalibrationAxes.YAxis ? -averageAcceleration.Y : CalibrationOffset.Y,
0);
/* Persist data. */
SetCalibrationSetting(CalibrationOffset);
result = true;
}
}
return(result);
}
void ProcessReading(AccelerometerReading accelerometerReading)
{
ThreeDimensionalVector lowPassFilteredAcceleration;
ThreeDimensionalVector optimalFilteredAcceleration;
ThreeDimensionalVector averagedAcceleration;
Vector3 acceleration = accelerometerReading.Acceleration;
ThreeDimensionalVector rawAcceleration = new ThreeDimensionalVector(
acceleration.X, acceleration.Y, acceleration.Z);
lock (sampleBuffer)
{
if (!initialized)
{
/* Initialize buffer with first value. */
sampleSumVector = rawAcceleration * SamplesCount;
averageAcceleration = rawAcceleration;
for (int i = 0; i < SamplesCount; i++)
{
sampleBuffer[i] = averageAcceleration;
}
previousLowPassOutput = averageAcceleration;
previousOptimalFilterOutput = averageAcceleration;
initialized = true;
}
lowPassFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilter(rawAcceleration.X, previousLowPassOutput.X),
ApplyLowPassFilter(rawAcceleration.Y, previousLowPassOutput.Y),
ApplyLowPassFilter(rawAcceleration.Z, previousLowPassOutput.Z));
previousLowPassOutput = lowPassFilteredAcceleration;
optimalFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.X, previousOptimalFilterOutput.X),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Y, previousOptimalFilterOutput.Y),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Z, previousOptimalFilterOutput.Z));
previousOptimalFilterOutput = optimalFilteredAcceleration;
/* Increment circular buffer insertion index. */
if (++sampleIndex >= SamplesCount)
{
/* If at max length then wrap samples back
* to the beginning position in the list. */
sampleIndex = 0;
}
/* Add new and remove old at sampleIndex. */
ThreeDimensionalVector vector = optimalFilteredAcceleration;
sampleSumVector += vector;
sampleSumVector -= sampleBuffer[sampleIndex];
sampleBuffer[sampleIndex] = vector;
averagedAcceleration = sampleSumVector / SamplesCount;
averageAcceleration = averagedAcceleration;
/* Stablity check
* If current low-pass filtered sample is deviating
* for more than 1/100 g from average
* (max of 0.5 deg inclination noise if device steady),
* then reset the stability counter.
* The calibration will be prevented until the counter is reaching
* the sample count size.
* Calibration enabled only if entire sampling buffer is "stable" */
ThreeDimensionalVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset)
|| (Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset)
|| (Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
/* Unstable */
deviceStableCount = 0;
}
else
{
if (deviceStableCount < SamplesCount)
{
++deviceStableCount;
}
}
/* Adjust with calibration value. */
rawAcceleration += CalibrationOffset;
lowPassFilteredAcceleration += CalibrationOffset;
optimalFilteredAcceleration += CalibrationOffset;
averagedAcceleration += CalibrationOffset;
}
Reading = new EnhancedAccelerometerReading(accelerometerReading.Timestamp,
rawAcceleration,
optimalFilteredAcceleration,
lowPassFilteredAcceleration,
averagedAcceleration);
DetectShake(acceleration);
previousAcceleration = acceleration;
}
bool Calibrate(CalibrationAxes calibrationAxes)
{
bool result = false;
lock (sampleBuffer)
{
if (CanCalibrate(calibrationAxes))
{
CalibrationOffset = new ThreeDimensionalVector(
calibrationAxes == CalibrationAxes.XAxis ? -averageAcceleration.X : CalibrationOffset.X,
calibrationAxes == CalibrationAxes.YAxis ? -averageAcceleration.Y : CalibrationOffset.Y,
0);
/* Persist data. */
SetCalibrationSetting(CalibrationOffset);
result = true;
}
}
return result;
}
void SetCalibrationSetting(ThreeDimensionalVector vector)
{
settingsService.SetSetting(CalibrationSettingKey + "X", vector.X);
settingsService.SetSetting(CalibrationSettingKey + "Y", vector.Y);
}
public static double CalcVectorAbsoluteValue(ThreeDimensionalVector vector)
{
return(Math.Sqrt(vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z));
}
void SetCalibrationSetting(ThreeDimensionalVector vector)
{
settingsService.SetSetting(CalibrationSettingKey + "X", vector.X);
settingsService.SetSetting(CalibrationSettingKey + "Y", vector.Y);
}
public void OperatorDivision_WhenNumberIsZero_GetArgumentExeption()
{
var threeDimVector1 = new ThreeDimensionalVector(-10, -14, -25);
int number = 0;
ThreeDimensionalVector result = threeDimVector1 / number;
}
public static DataTable CorrectAcc(DateTime startTime, DateTime endTime, InsertDatum datum, DataRow tripRow)
{
/*** LocationListenerのtimestampとSystem.currentTimeMillis()の差分を計算している
* LocationListenerのtimestampはおそらく、システム時間を参照しているからこの処理は意味ないかも ***/
int millisecTimeDiff = AndroidGpsRawDao.GetMilliSencodTimeDiffBetweenJstAndAndroidTime(
tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime), datum);
var accRawTable = AndroidAccRawDao.Get(tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime), millisecTimeDiff, datum);
accRawTable = DataTableUtil.GetTempCorrectedAccTable(accRawTable);
if (accRawTable.Rows.Count == 0)
{
// TODO ログ書き込み
//エラー期間をログファイルに記録
// WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart + "～" + tripEnd + "加速度データなし", LogMode.acc);
return(null);
}
else if (false) // TODO 挿入済みチェックをすべきか？
{
// WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart.ToString() + "～" + tripEnd.ToString() + "はすでに挿入済みです\r\n", LogMode.acc);
return(null);
}
// TODO ファイルに記録
//WriteLog("X方向平均(補正前)＝" + dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正前)＝" + dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正前)＝" + dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
var accurateStoppingAccRawTable = CorrectedAccDao.GetAccurateStoppingAccRaw(millisecTimeDiff, datum);
if (accurateStoppingAccRawTable.Rows.Count == 0)
{
// TODO ログ出力
//WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + tripStart.ToString() + "～" + tripEnd.ToString() + "停止時の加速度データなし or タイムアウトエラーが発生 + \r\n", LogMode.acc);
return(null);
}
// TODO テーブル関数定義の場合、staticフィールドで参照するがよろし
double avgX = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_X"].ToString());
double avgY = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_Y"].ToString());
double avgZ = double.Parse(accurateStoppingAccRawTable.Rows[0]["ACC_Z"].ToString());
var vector1 = new ThreeDimensionalVector(0, 0, 9.8);
var vector2 = new ThreeDimensionalVector(avgX, avgY, avgZ);
var vectorProduct = new ThreeDimensionalVector(vector1.Y * vector2.Z - vector1.Z * vector2.Y, vector1.Z * vector2.X - vector1.X * vector2.Z, vector1.X * vector2.Y - vector1.Y * vector2.X);
var vasisVP = new ThreeDimensionalVector(vectorProduct.X / MathUtil.CalcVectorAbsoluteValue(vectorProduct),
vectorProduct.Y / MathUtil.CalcVectorAbsoluteValue(vectorProduct),
vectorProduct.Z / MathUtil.CalcVectorAbsoluteValue(vectorProduct));
var angle = Math.Asin(MathUtil.CalcVectorAbsoluteValue(vectorProduct) / (MathUtil.CalcVectorAbsoluteValue(vector1) * MathUtil.CalcVectorAbsoluteValue(vector2)));
// TODO ログ出力
//WriteLog("角度(Z軸補正)=" + angle * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("回転軸 = (" + vasisVP.X + ", " + vasisVP.Y + ", " + vasisVP.Z + ")", LogMode.acc);
Quaternion p = new Quaternion(0, vector2.X, vector2.Y, vector2.Z);
Quaternion q = new Quaternion(Math.Cos(angle / 2), vasisVP.X * Math.Sin(angle / 2), vasisVP.Y * Math.Sin(angle / 2), vasisVP.Z * Math.Sin(angle / 2));
Quaternion r = new Quaternion(Math.Cos(angle / 2), -vasisVP.X * Math.Sin(angle / 2), -vasisVP.Y * Math.Sin(angle / 2), -vasisVP.Z * Math.Sin(angle / 2));
Quaternion rp = MathUtil.MultiplyQuaternion(r, p);
Quaternion rpq = MathUtil.MultiplyQuaternion(rp, q);
for (int i = 0; i < accRawTable.Rows.Count; i++)
{
Quaternion p1 = new Quaternion(0,
accRawTable.Rows[i].Field <Single>("LONGITUDINAL_ACC"),
accRawTable.Rows[i].Field <Single>("LATERAL_ACC"),
accRawTable.Rows[i].Field <Single>("VERTICAL_ACC"));
Quaternion rp1 = MathUtil.MultiplyQuaternion(r, p1);
Quaternion rpq1 = MathUtil.MultiplyQuaternion(rp1, q);
// TODO SetFieldに書き直し、する価値ないか
accRawTable.Rows[i]["LONGITUDINAL_ACC"] = Single.Parse(rpq1.X.ToString());
accRawTable.Rows[i]["LATERAL_ACC"] = Single.Parse(rpq1.Y.ToString());
accRawTable.Rows[i]["VERTICAL_ACC"] = Single.Parse(rpq1.Z.ToString());
accRawTable.Rows[i]["ALPHA"] = (Single)(angle * 360 / (2 * Math.PI));
accRawTable.Rows[i]["VECTOR_X"] = (Single)vasisVP.X;
accRawTable.Rows[i]["VECTOR_Y"] = (Single)vasisVP.Y;
accRawTable.Rows[i]["ROLL"] = DBNull.Value;
accRawTable.Rows[i]["PITCH"] = DBNull.Value;
accRawTable.Rows[i]["YAW"] = DBNull.Value;
}
// TODO ログ出力
//WriteLog("X方向平均(補正後)＝" + dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString() + "\r\n", LogMode.acc);
var tableBreaking = CorrectedAccDao.GetAccurateAccBreakingRaw(tripRow.Field <DateTime>(TripsDao.ColumnStartTime),
tripRow.Field <DateTime>(TripsDao.ColumnEndTime),
datum);
double xsum = 0;
double ysum = 0;
int count = 0;
for (int i = 0; i < tableBreaking.Rows.Count; i++)
{
DateTime end = DateTime.Parse(tableBreaking.Rows[i]["JST"].ToString());
DateTime start = end.AddSeconds(-10);
DataRow[] rows = accRawTable.Select("JST >= #" + start + "# AND JST <= #" + end + "#");
foreach (DataRow row in rows)
{
xsum = xsum + double.Parse(row["LONGITUDINAL_ACC"].ToString());
ysum = ysum + double.Parse(row["LATERAL_ACC"].ToString());
count++;
}
}
if (count == 0)
{
// TODO ログ出力
//WriteLog("DRIVER_ID:" + userID.DriverID + "SENSOR_ID:" + userID.SensorID + " " + "減速データがありません\r\n", LogMode.acc);
return(null);
}
double bx = xsum / count;
double by = ysum / count;
var v3 = new ThreeDimensionalVector(-1, 0, 0);
var v4 = new ThreeDimensionalVector(bx, by, 0);
double innerProduct = v3.X * v4.X + v3.Y * v4.Y;
double cos = innerProduct / (MathUtil.CalcVectorAbsoluteValue(v3) * MathUtil.CalcVectorAbsoluteValue(v4));
double anglexy = Math.Acos(cos);//ラジアン(0～180)
for (int j = 0; j < accRawTable.Rows.Count; j++)
{
double x = double.Parse(accRawTable.Rows[j]["LONGITUDINAL_ACC"].ToString());
double y = double.Parse(accRawTable.Rows[j]["LATERAL_ACC"].ToString());
double z = double.Parse(accRawTable.Rows[j]["VERTICAL_ACC"].ToString());
double x2;
double y2;
double z2;
if (by >= 0)
{
x2 = Math.Cos(anglexy) * x - Math.Sin(anglexy) * y;
y2 = Math.Sin(anglexy) * x + Math.Cos(anglexy) * y;
z2 = z;
}
else
{
x2 = Math.Cos(-anglexy) * x - Math.Sin(-anglexy) * y;
y2 = Math.Sin(-anglexy) * x + Math.Cos(-anglexy) * y;
z2 = z;
}
accRawTable.Rows[j]["LONGITUDINAL_ACC"] = Single.Parse(x2.ToString());
accRawTable.Rows[j]["LATERAL_ACC"] = Single.Parse(y2.ToString());
accRawTable.Rows[j]["VERTICAL_ACC"] = Single.Parse(z2.ToString());
accRawTable.Rows[j]["BETA"] = (Single)(anglexy * 360 / (2 * Math.PI));
}
avgX = double.Parse(accRawTable.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
avgZ = double.Parse(accRawTable.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
//WriteLog("平均値 x = " + v4.X + ", y = " + v4.Y, LogMode.acc);
//WriteLog("XY回転角度＝" + "xy = " + anglexy * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("X方向平均(補正後)＝" + averagex.ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + (double.Parse(dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString())).ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + averagez.ToString() + "\r\n", LogMode.acc);
avgX = double.Parse(accRawTable.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripRow.Field <DateTime>(TripsDao.ColumnStartTime) + "# AND JST < #" + tripRow.Field <DateTime>(TripsDao.ColumnEndTime) + "#").ToString());
var v5 = new ThreeDimensionalVector(0, 0, 9.8);
var v6 = new ThreeDimensionalVector(avgX, 0, avgZ);
innerProduct = v5.X * v6.X + v5.Z * v6.Z;
cos = innerProduct / (MathUtil.CalcVectorAbsoluteValue(v5) * MathUtil.CalcVectorAbsoluteValue(v6));
anglexy = Math.Acos(cos);//ラジアン(0～180)
for (int j = 0; j < accRawTable.Rows.Count; j++)
{
double x = double.Parse(accRawTable.Rows[j]["LONGITUDINAL_ACC"].ToString());
double y = double.Parse(accRawTable.Rows[j]["LATERAL_ACC"].ToString());
double z = double.Parse(accRawTable.Rows[j]["VERTICAL_ACC"].ToString());
double x2;
double y2;
double z2;
if (avgX >= 0)
{
x2 = Math.Cos(anglexy) * x - Math.Sin(anglexy) * z;
y2 = y;
z2 = Math.Sin(anglexy) * x + Math.Cos(anglexy) * z;
}
else
{
x2 = Math.Cos(-anglexy) * x - Math.Sin(-anglexy) * z;
y2 = y;
z2 = Math.Sin(-anglexy) * x + Math.Cos(-anglexy) * z;
}
accRawTable.Rows[j]["LONGITUDINAL_ACC"] = Single.Parse(x2.ToString());
accRawTable.Rows[j]["LATERAL_ACC"] = Single.Parse(y2.ToString());
accRawTable.Rows[j]["VERTICAL_ACC"] = Single.Parse(z2.ToString());
accRawTable.Rows[j]["GAMMA"] = (Single)(anglexy * 360 / (2 * Math.PI));
}
// TODO ログ出力
//WriteLog("平均値 x = " + v6.X + ", z = " + v6.Z, LogMode.acc);
//WriteLog("XZ回転角度＝" + "xz = " + anglexy * 360 / (2 * Math.PI), LogMode.acc);
//WriteLog("X方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(LONGITUDINAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString(), LogMode.acc);
//WriteLog("Y方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(LATERAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString(), LogMode.acc);
//WriteLog("Z方向平均(補正後)＝" + double.Parse(dtACC.Compute("AVG(VERTICAL_ACC)", "JST > #" + tripStart + "# AND JST < #" + tripEnd + "#").ToString()).ToString() + "\r\n", LogMode.acc);
return(accRawTable);
}
void ProcessReading(AccelerometerReading accelerometerReading)
{
ThreeDimensionalVector lowPassFilteredAcceleration;
ThreeDimensionalVector optimalFilteredAcceleration;
ThreeDimensionalVector averagedAcceleration;
Vector3 acceleration = accelerometerReading.Acceleration;
ThreeDimensionalVector rawAcceleration = new ThreeDimensionalVector(
acceleration.X, acceleration.Y, acceleration.Z);
lock (sampleBuffer)
{
if (!initialized)
{
/* Initialize buffer with first value. */
sampleSumVector = rawAcceleration * SamplesCount;
averageAcceleration = rawAcceleration;
for (int i = 0; i < SamplesCount; i++)
{
sampleBuffer[i] = averageAcceleration;
}
previousLowPassOutput = averageAcceleration;
previousOptimalFilterOutput = averageAcceleration;
initialized = true;
}
lowPassFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilter(rawAcceleration.X, previousLowPassOutput.X),
ApplyLowPassFilter(rawAcceleration.Y, previousLowPassOutput.Y),
ApplyLowPassFilter(rawAcceleration.Z, previousLowPassOutput.Z));
previousLowPassOutput = lowPassFilteredAcceleration;
optimalFilteredAcceleration = new ThreeDimensionalVector(
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.X, previousOptimalFilterOutput.X),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Y, previousOptimalFilterOutput.Y),
ApplyLowPassFilterWithNoiseThreshold(rawAcceleration.Z, previousOptimalFilterOutput.Z));
previousOptimalFilterOutput = optimalFilteredAcceleration;
/* Increment circular buffer insertion index. */
if (++sampleIndex >= SamplesCount)
{
/* If at max length then wrap samples back
* to the beginning position in the list. */
sampleIndex = 0;
}
/* Add new and remove old at sampleIndex. */
ThreeDimensionalVector vector = optimalFilteredAcceleration;
sampleSumVector += vector;
sampleSumVector -= sampleBuffer[sampleIndex];
sampleBuffer[sampleIndex] = vector;
averagedAcceleration = sampleSumVector / SamplesCount;
averageAcceleration = averagedAcceleration;
/* Stablity check
* If current low-pass filtered sample is deviating
* for more than 1/100 g from average
* (max of 0.5 deg inclination noise if device steady),
* then reset the stability counter.
* The calibration will be prevented until the counter is reaching
* the sample count size.
* Calibration enabled only if entire sampling buffer is "stable" */
ThreeDimensionalVector deltaAcceleration = averagedAcceleration - optimalFilteredAcceleration;
if ((Math.Abs(deltaAcceleration.X) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Y) > maximumStabilityDeltaOffset) ||
(Math.Abs(deltaAcceleration.Z) > maximumStabilityDeltaOffset))
{
/* Unstable */
deviceStableCount = 0;
}
else
{
if (deviceStableCount < SamplesCount)
{
++deviceStableCount;
}
}
/* Adjust with calibration value. */
rawAcceleration += CalibrationOffset;
lowPassFilteredAcceleration += CalibrationOffset;
optimalFilteredAcceleration += CalibrationOffset;
averagedAcceleration += CalibrationOffset;
}
Reading = new EnhancedAccelerometerReading(accelerometerReading.Timestamp,
rawAcceleration,
optimalFilteredAcceleration,
lowPassFilteredAcceleration,
averagedAcceleration);
DetectShake(acceleration);
previousAcceleration = acceleration;
}
