// AA: Interface function for filtering of individual joints
public Vector3 Update(Vector3 jointPos, JOINT_TYPE jointType )
{
Vector3 newJointPos = Vector3.zero;
switch (jointType) {
case JOINT_TYPE.JOINT:
// Loop arond the joint history buffers if necessary
if(jointIndex > numHistory - 1)
jointIndex = jointIndex % numHistory;
jointHistory[jointIndex] = jointPos;
newJointPos = applyFilter(jointHistory, jointType);
jointHistory[jointIndex] = newJointPos;
jointIndex++;
break;
case JOINT_TYPE.RELATIVEJOINT:
// Loop arond the joint history buffers if necessary
if(relativeJointIndex > numHistory - 1)
relativeJointIndex = relativeJointIndex % numHistory;
// Store joint in history
relativeJointHistory[relativeJointIndex] = jointPos;
newJointPos = applyFilter(relativeJointHistory, jointType);
relativeJointHistory[relativeJointIndex] = newJointPos;
relativeJointIndex++;
break;
default:
break;
}
return newJointPos;
}
private Vector3 applyFilter(Vector3 [] array, int arrIndex, JOINT_TYPE jointType, Vector3 [] arrayOutput = null, Filter.FILTER_NAME fname = Filter.FILTER_NAME.NONE)
{
Vector3 sum = Vector3.zero;
switch (fname)
{
// Simplest joint filter, where the filter output is the average of N recent inputs
case FILTER_NAME.SIMPLE_AVG:
for (int i = 0; i < numHistory; i++)
{
sum = sum + array[i];
}
sum /= numHistory;
break;
// Moving average (MA) filters are a special case of ARMA filters where the auto regressive term is zero
case FILTER_NAME.MOVING_AVG:
for (int i = 0; i < numHistory; i++)
{
sum = sum + array[i] * weights[i];
}
break;
// Double moving average with window size = 2 (for fast implementation)
case FILTER_NAME.DOUBLE_MOVING_AVG:
sum = (5.0f / 9) * array[getIndex(arrIndex)] +
(4.0f / 9) * array[getIndex(arrIndex - 1)] +
(1.0f / 3) * array[getIndex(arrIndex - 2)] -
(2.0f / 9) * array[getIndex(arrIndex - 3)] -
(1.0f / 9) * array[getIndex(arrIndex - 4)];
break;
// Exponential Smoothing with window size = 2 (for fast implementation)
case FILTER_NAME.EXP_SMOOTHING:
// The dynamic code works very slowly:
// for (int i = 0; i < windowSize; i++) {
// sum += Mathf.Pow((1-alpha), i )*array[getIndex(arrIndex - i)];
// }
// sum = alpha*sum;
// The unrolled loop for window size of two code:
sum = alpha * array[getIndex(arrIndex)] + (1 - alpha) * array[getIndex(arrIndex - 1)] + (1 - alpha) * (1 - alpha) * array[getIndex(arrIndex - 2)];
break;
// Double Exponential Smoothing with window size = 2 (for fast implementation)
case FILTER_NAME.DOUBLE_EXP_SMOOTHING:
if (jointType == JOINT_TYPE.JOINT)
{
Vector3 trend = gamma * (arrayOutput[getIndex(jointOutputIndex - 1)] - arrayOutput[getIndex(jointOutputIndex - 2)]) + (1 - gamma) * (gamma * (arrayOutput[getIndex(jointOutputIndex - 3)] - arrayOutput[getIndex(jointOutputIndex - 4)]));
sum = alpha * array[getIndex(arrIndex)] + (1 - alpha) * (arrayOutput[getIndex(arrIndex - 2)] + trend);
}
else
{
Vector3 trend = gamma * (arrayOutput[getIndex(relativeJointOutputIndex - 1)] - arrayOutput[getIndex(relativeJointOutputIndex - 2)]) + (1 - gamma) * (gamma * (arrayOutput[getIndex(relativeJointOutputIndex - 3)] - arrayOutput[getIndex(relativeJointOutputIndex - 4)]));
sum = alpha * array[getIndex(arrIndex)] + (1 - alpha) * (arrayOutput[getIndex(arrIndex - 2)] + trend);
}
break;
// Median filter
case FILTER_NAME.MEDIAN:
if (jointType == JOINT_TYPE.JOINT)
{
sum = findMedian(jointsMedian, array, arrIndex);
}
else if (jointType == JOINT_TYPE.RELATIVEJOINT)
{
sum = findMedian(relativeJointsMedian, array, arrIndex);
}
break;
// Hand joint filtered by simple avg, shoulder joint by median
case FILTER_NAME.COMBINATION1:
if (jointType == JOINT_TYPE.JOINT)
{
sum = applyFilter(array, arrIndex, jointType, arrayOutput, Filter.FILTER_NAME.SIMPLE_AVG);
}
else if (jointType == JOINT_TYPE.RELATIVEJOINT)
{
sum = findMedian(relativeJointsMedian, array, arrIndex);
}
break;
// Hand joint filtered by double moving average, shoulder joint by median
case FILTER_NAME.COMBINATION2:
if (jointType == JOINT_TYPE.JOINT)
{
sum = applyFilter(array, arrIndex, jointType, arrayOutput, Filter.FILTER_NAME.DOUBLE_MOVING_AVG);
}
else if (jointType == JOINT_TYPE.RELATIVEJOINT)
{
sum = findMedian(relativeJointsMedian, array, arrIndex);
}
break;
// Jitter removal filter is like a simple case of moving average filter using only current and previous input
case FILTER_NAME.JITTER_REMOVAL:
sum = alpha * array[arrIndex] + (1 - alpha) * array[getIndex(arrIndex - 1)];
break;
// ADAPTIVE_DOUBLE_EXP_SMOOTHING filter uses a_low and a_high (alpha low and alpha high values)
case FILTER_NAME.ADAPTIVE_DOUBLE_EXP_SMOOTHING:
Vector3 diff = array[arrIndex] - array[getIndex(arrIndex - 1)];
Vector2 velocity = new Vector2(diff.x, diff.y);
float vn = velocity.sqrMagnitude;
if (vn < v_low)
{
alpha = a_low;
gamma = y_low;
}
else if (vn > v_high)
{
alpha = a_high;
gamma = y_high;
}
else
{
alpha = a_high + ((vn - v_high) / (v_low - v_high)) * (a_low - a_high);
gamma = y_high + ((vn - v_high) / (v_low - v_high)) * (y_low - y_high);
}
if (jointType == JOINT_TYPE.JOINT)
{
Vector3 myTrend = gamma * (arrayOutput[getIndex(jointOutputIndex - 1)] - arrayOutput[getIndex(jointOutputIndex - 2)]) + (1 - gamma) * (gamma * (arrayOutput[getIndex(jointOutputIndex - 3)] - arrayOutput[getIndex(jointOutputIndex - 4)]));
sum = alpha * array[getIndex(arrIndex)] + (1 - alpha) * (arrayOutput[getIndex(arrIndex - 2)] + myTrend);
}
else
{
Vector3 myTrend = gamma * (arrayOutput[getIndex(relativeJointOutputIndex - 1)] - arrayOutput[getIndex(relativeJointOutputIndex - 2)]) + (1 - gamma) * (gamma * (arrayOutput[getIndex(relativeJointOutputIndex - 3)] - arrayOutput[getIndex(relativeJointOutputIndex - 4)]));
sum = alpha * array[getIndex(arrIndex)] + (1 - alpha) * (arrayOutput[getIndex(arrIndex - 2)] + myTrend);
}
break;
// Taylor series can forecast 1 data point
// case FILTER_NAME.TAYLOR_SERIES:
// if(useSmoothedInput)
// {
// // Populate the smoothed inputs array for joint/relative joint
// Vector2 newJointPos = applyFilter(array, arrIndex, jointType, arrayOutput, FILTER_NAME.SIMPLE_AVG);
//
// sum = array[arrIndex] - 3*array[getIndex(arrIndex-1)] + 3*array[getIndex(arrIndex-2)] - array[getIndex(arrIndex-3)];
//
// if(jointType == JOINT_TYPE.JOINT)
// {
// TS_SmoothInputsJ[jointOutputIndex] = newJointPos;
// sum = (8.0f/3)*TS_SmoothInputsJ[arrIndex] - (5.0f/2)* TS_SmoothInputsJ[getIndex(arrIndex-1)] + TS_SmoothInputsJ[getIndex(arrIndex-2)] - (1.0f/6)*TS_SmoothInputsJ[getIndex(arrIndex-3)];
// }
// else
// {
// TS_SmoothInputsRJ[relativeJointOutputIndex] = newJointPos;
// sum = (8.0f/3)*TS_SmoothInputsRJ[arrIndex] - (5.0f/2)* TS_SmoothInputsRJ[getIndex(arrIndex-1)] + TS_SmoothInputsRJ[getIndex(arrIndex-2)] - (1.0f/6)*TS_SmoothInputsRJ[getIndex(arrIndex-3)];
//
// }
//
//
// }
default:
break;
}
return(sum);
}
// AA: Interface function for filtering of individual joints
public Vector3 Update(Vector3 jointPos, JOINT_TYPE jointType)
{
Vector3 newJointPos = Vector3.zero;
if (name == FILTER_NAME.NONE)
{
return(jointPos);
}
switch (jointType)
{
case JOINT_TYPE.JOINT:
// Loop arond the joint history buffers if necessary
if (jointIndex > numHistory - 1)
{
jointIndex = jointIndex % numHistory;
}
if (jointOutputIndex > numHistory - 1)
{
jointOutputIndex = jointOutputIndex % numHistory;
}
// Store input in joint's history
jointHistory[jointIndex] = jointPos;
newJointPos = applyFilter(jointHistory, jointIndex, jointType, jointOutputs, name);
jointIndex++;
// Store the filter output value in the joint's history
jointOutputs[jointOutputIndex] = newJointPos;
jointOutputIndex++;
break;
case JOINT_TYPE.RELATIVEJOINT:
// Loop arond the joint history buffers if necessary
if (relativeJointIndex > numHistory - 1)
{
relativeJointIndex = relativeJointIndex % numHistory;
}
if (relativeJointOutputIndex > numHistory - 1)
{
relativeJointOutputIndex = relativeJointOutputIndex % numHistory;
}
// Store input in joint's history
relativeJointHistory[relativeJointIndex] = jointPos;
newJointPos = applyFilter(relativeJointHistory, relativeJointIndex, jointType, relativeJointOutputs, name);
relativeJointIndex++;
// Store the filter output value in the joint's history
relativeJointOutputs[relativeJointOutputIndex] = newJointPos;
relativeJointOutputIndex++;
break;
default:
break;
}
return(newJointPos);
}
private Vector3 applyFilter(Vector3 [] array, JOINT_TYPE jointType)
{
Vector3 sum = Vector3.zero;
for (int i = 0; i < numHistory; i++) {
sum = sum + array[i]*weights[i];
}
return sum;
}
