//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
}
// wrapper around Euler2Quaternion to use vectors instead of arrays
Quaternion<double> Interpolator::Vector2Quaternion(vector v)
{
Quaternion<double> result;
double angles[3];
v.getValue(angles);
Euler2Quaternion(angles, result);
return result;
}
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;
}
