//gyro_data unit rad/s dT unit s
static void EKF_StatePrediction(float gyro_data[3], float dT)
{
static float U[3];
static float q[4], euler[3];
U[0] = gyro_data[0];
U[1] = gyro_data[1];
U[2] = gyro_data[2];
EKF_LinearizeFG(EKF_X, U, EKF_F, EKF_G);
EKF_RungeKutta(EKF_X, U, dT);
#if 1 // ·Àֹƫº½½ÇÔÚ+/-180¡ãÊÇ·¢ÉúÍ»±ä
q[0] = EKF_X[0];
q[1] = EKF_X[1];
q[2] = EKF_X[2];
q[3] = EKF_X[3];
Quaternion2Euler(q, euler);
if(euler[2] > 180)
{
euler[2] = -180;
}
else if(euler[2] < -180)
{
euler[2] = 180;
}
Euler2Quaternion(euler, q);
EKF_X[0] = q[0];
EKF_X[1] = q[1];
EKF_X[2] = q[2];
EKF_X[3] = q[3];
#endif
}
void EKF_Attitude(void)
{
float gyro[3], accel[3], mag_data[3];
float euler[3];
float dT = Deal_Period_ms / 1000.0f;
static uint8_t isInit = false;
if(!isInit)
{
EKF_AttitudeInit();
isInit = true;
}
else
{
gyro[0] = Global_Gyro_Sensor.filtered.x;
gyro[1] = Global_Gyro_Sensor.filtered.y;
gyro[2] = Global_Gyro_Sensor.filtered.z;
accel[0] = Global_Accel_Sensor.filtered.x;
accel[1] = Global_Accel_Sensor.filtered.y;
accel[2] = Global_Accel_Sensor.filtered.z;
mag_data[0] = (float)Global_Mag_Val[0];
mag_data[1] = (float)Global_Mag_Val[1];
mag_data[2] = (float)Global_Mag_Val[2];
EKF_StatePrediction(gyro, dT);
EKF_INSCovariancePrediction(dT);
EKF_Z_Cal(accel, mag_data, EKF_Z);
EKF_LinearizeH(EKF_H);
#if 1
EKF_K_Cal(EKF_P, EKF_H, EKF_R, EKF_K);
EKF_Correction_X(EKF_Z, EKF_H, EKF_K, EKF_X);
EKF_Correction_P(EKF_K, EKF_H, EKF_P);
#endif
Nav.q[0] = EKF_X[0];
Nav.q[1] = EKF_X[1];
Nav.q[2] = EKF_X[2];
Nav.q[3] = EKF_X[3];
Quaternion2Euler(Nav.q, euler);
Global_Now_Euler[0] = euler[0];
Global_Now_Euler[1] = euler[1];
Global_Now_Euler[2] = euler[2];
}
}
// function for SLERP quaternion
void Interpolator::LinearInterpolationQuaternion(Motion * pInputMotion, Motion * pOutputMotion, int N)
{
int inputLength = pInputMotion->GetNumFrames(); // frames are indexed 0, ..., inputLength-1
int startKeyframe = 0;
while (startKeyframe + N + 1 < inputLength)
{
int endKeyframe = startKeyframe + N + 1;
Posture * startPosture = pInputMotion->GetPosture(startKeyframe);
Posture * endPosture = pInputMotion->GetPosture(endKeyframe);
// copy start and end keyframe
pOutputMotion->SetPosture(startKeyframe, *startPosture);
pOutputMotion->SetPosture(endKeyframe, *endPosture);
// interpolate in between
for(int frame=1; frame<=N; frame++)
{
Posture interpolatedPosture;
double t = 1.0 * frame / (N+1);
// interpolate root position
interpolatedPosture.root_pos = startPosture->root_pos * (1-t) + endPosture->root_pos * t;
// interpolate bone rotations
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
// convert euler angles to quaternions
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]);
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]);
// do slerp for converted quaternions at t
double interpolatedAngles[3];
Quaternion2Euler(Slerp(t, boneStartRot, boneEndRot), interpolatedAngles);
interpolatedPosture.bone_rotation[bone].setValue(interpolatedAngles);
}
pOutputMotion->SetPosture(startKeyframe + frame, interpolatedPosture);
}
startKeyframe = endKeyframe;
}
for(int frame=startKeyframe+1; frame<inputLength; frame++)
pOutputMotion->SetPosture(frame, *(pInputMotion->GetPosture(frame)));
}
static void EKF_Z_Cal(float accel[3], float mag_data[3], float z[Zn])
{
#if defined USE_MAG
#if defined USE_MAG_HMC5883L
euler[2] = atan2(mag_data[1], mag_data[0]) * RAD_DEG;
#elif defined USE_MAG_LSM303D
Get_Ref_Euler(accel, mag_data, &Mag_LSM303D_Calibration, euler);
#endif
Global_Show_Val[5] = euler[0];
Global_Show_Val[6] = euler[1];
Global_Show_Val[7] = euler[2];
//euler[2] -= Global_Heading_Ref;
#else
u[0] = accel[0];
u[1] = accel[1];
u[2] = accel[2];
q[0] = EKF_X[0];
q[1] = EKF_X[1];
q[2] = EKF_X[2];
q[3] = EKF_X[3];
Quaternion2Euler(q, euler);
Global_Show_Val[5] = euler[0] = atan2(u[1], u[2]) * RAD_DEG;
Global_Show_Val[6] = euler[1] = atan2(-1 * u[0], u[2]) * RAD_DEG;
#endif
Euler2Quaternion(euler, z);
qmag = sqrt(z[0]*z[0] + z[1]*z[1] + z[2]*z[2] + z[3]*z[3]);
z[0] /= qmag;
z[1] /= qmag;
z[2] /= qmag;
z[3] /= qmag;
}
// function for SLERP bezier quaternion interpolation
void Interpolator::BezierInterpolationQuaternion(Motion * pInputMotion, Motion * pOutputMotion, int N)
{
// students should implement this
int inputLength = pInputMotion->GetNumFrames(); // frames are indexed 0, ..., inputLength-1
int startKeyframe = 0;
// default and next 'a' values for bezier are stored in memory
Quaternion<double> aDef[MAX_BONES_IN_ASF_FILE];
Quaternion<double> aNext[MAX_BONES_IN_ASF_FILE];
double bezRatio = 1.0/3.0;
vector aNextRoot;
// use a0 for the first frame by backtracking from next 2 known frames
vector v1 = pInputMotion->GetPosture(0)->root_pos;
vector v2 = pInputMotion->GetPosture(N+1)->root_pos;
vector v3 = pInputMotion->GetPosture(N+1+N+1)->root_pos;
vector aDefRoot = v1*(1-bezRatio) + (v3 * (-1) + v2 * 2) * bezRatio;
for (int i = 0; i < MAX_BONES_IN_ASF_FILE; i++)
{
Quaternion<double> q1 = Vector2Quaternion(pInputMotion->GetPosture(0)->bone_rotation[i]);
Quaternion<double> q2 = Vector2Quaternion(pInputMotion->GetPosture(N+1)->bone_rotation[i]);
Quaternion<double> q3 = Vector2Quaternion(pInputMotion->GetPosture(N+1+N+1)->bone_rotation[i]);
aDef[i] = Slerp(bezRatio, q1, Slerp(2.0, q3, q2));
}
while (startKeyframe + N + 1 < inputLength)
{
int endKeyframe = startKeyframe + N + 1;
Posture * startPosture = pInputMotion->GetPosture(startKeyframe);
Posture * endPosture = pInputMotion->GetPosture(endKeyframe);
Posture * nextPosture;
if (endKeyframe + N + 1 >= inputLength) nextPosture = pInputMotion->GetPosture(inputLength-1);
else nextPosture = pInputMotion->GetPosture(endKeyframe + (N+1));
// copy start and end keyframe
pOutputMotion->SetPosture(startKeyframe, *startPosture);
pOutputMotion->SetPosture(endKeyframe, *endPosture);
// interpolate in between
for(int frame=1; frame<=N; frame++)
{
Posture interpolatedPosture;
double t = 1.0 * frame / (N+1);
// bez interpolate root position
vector rootStartPos = startPosture->root_pos;
vector rootEndPos = endPosture->root_pos;
vector rootNextPos = nextPosture->root_pos;
vector p1 = startKeyframe == 0? aDefRoot : aNextRoot; // use a0 if first frame, otherwise use a calculated in previous iteration
vector aN = rootEndPos*(1-bezRatio) + ((rootStartPos*-1+ rootEndPos*2)*0.5+ rootNextPos*0.5)*bezRatio;
vector p2 = aN*-1+rootEndPos*2; // bn
interpolatedPosture.root_pos = DeCasteljauEuler(t, rootStartPos, p1, p2, rootEndPos);
// interpolate bone rotations
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]); // qn
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]); // qn+1
Quaternion<double> boneNextRot = Vector2Quaternion(nextPosture->bone_rotation[bone]); // qn+2
Quaternion<double> p1 = startKeyframe == 0? aDef[bone] : aNext[bone]; // use a0 if first frame, otherwise use a calculated in previous iteration
Quaternion<double> aBar = Slerp(0.5, Slerp(2.0, boneStartRot, boneEndRot), boneNextRot);
Quaternion<double> p2 = Slerp(-bezRatio, boneEndRot, aBar);
double interpolatedAngles[3];
Quaternion2Euler(DeCasteljauQuaternion(t, boneStartRot, p1, p2, boneEndRot), interpolatedAngles);
interpolatedPosture.bone_rotation[bone].setValue(interpolatedAngles);
}
pOutputMotion->SetPosture(startKeyframe + frame, interpolatedPosture);
}
// set the aN values for the next N frames (bezier)
vector rootStartPos = startPosture->root_pos;
vector rootEndPos = endPosture->root_pos;
vector rootNextPos = nextPosture->root_pos;
aNextRoot = rootEndPos*(1-bezRatio) + ((rootStartPos*-1+ rootEndPos*2)*0.5+ rootNextPos*0.5)*bezRatio;
for (int bone = 0; bone < MAX_BONES_IN_ASF_FILE; bone++)
{
Quaternion<double> boneStartRot = Vector2Quaternion(startPosture->bone_rotation[bone]); // qn
Quaternion<double> boneEndRot = Vector2Quaternion(endPosture->bone_rotation[bone]); // qn+1
Quaternion<double> boneNextRot = Vector2Quaternion(nextPosture->bone_rotation[bone]); // qn+2
aNext[bone] = Slerp(bezRatio, boneEndRot, Slerp(0.5, Slerp(2.0, boneStartRot, boneEndRot), boneNextRot)); // an+1
}
startKeyframe = endKeyframe;
}
for(int frame=startKeyframe+1; frame<inputLength; frame++)
pOutputMotion->SetPosture(frame, *(pInputMotion->GetPosture(frame)));
}
