core::vector3df interpolate_rotation(const core::vector3df &a, const core::vector3df &b, f32 alpha)
{
core::quaternion qCurrent(core::DEGTORAD * a);
core::quaternion qDesired(core::DEGTORAD * b);
core::quaternion qInterpolated;
qInterpolated.slerp(qCurrent,qDesired,alpha);
return quaternion_to_euler(qInterpolated);
}
// this function is untested
core::vector3df rotation_difference(const core::vector3df &a, const core::vector3df &b)
{
core::quaternion q1(core::DEGTORAD * a);
core::quaternion q2(core::DEGTORAD * b);
// Delta
q1.makeInverse();
core::quaternion qDelta = q1 * q2;
return quaternion_to_euler(qDelta);
}
core::vector3df rotation_required(const core::vector3df &vec, const core::vector3df &opposite)
{
const core::vector3df startVec(0,0,1);
if (vec == -startVec)
return opposite;
core::quaternion quat;
quat.rotationFromTo(startVec, vec);
return quaternion_to_euler(quat);
}
int ekf_estimator::update(EKF_U u,EKF_Mesurement mesurement,const float dT)
{
//Declear F G U
float U[6],F[169],G[117];
static int wait_convergence;
//Declear Mesurement
float Mag_data[3],Pos[3],Vel[2];
//Declear EulerAngle
if(!ekf_is_init)//if not init , return
return -1;
/*caution: the accel sign is all opposite with APM and CC3D*/
U[0]=u.gyro_x - gyro_bias[0];
U[1]=u.gyro_y - gyro_bias[1];
U[2]=u.gyro_z - gyro_bias[2];
U[3]=-u.accel_x;
U[4]=-u.accel_y;
U[5]=-u.accel_z;
/*caution: the mag_x,mag_y sign is opposite with APM and CC3D & the mag_z sign is same*/
Mag_data[0]=-mesurement.Mag_x;
Mag_data[1]=-mesurement.Mag_y;
Mag_data[2]= mesurement.Mag_z;
Pos[0]=mesurement.Pos_GPS_x;
Pos[1]=mesurement.Pos_GPS_y;
Pos[2]=mesurement.Pos_Baro_z;//use baro replace gps_d
Vel[0]=mesurement.Vel_GPS_x;
Vel[1]=mesurement.Vel_GPS_y;
if(ekf_is_init)
{
//To-Do:wait for ekf convergence
//caculate Jacobian to linear F G
LinearFG(X,U,F,G);
//Predeict X
RungeKutta(X,U,dT);
//Predict P
INS_CovariancePrediction(F,G,Q,dT,P);
//Update P,X
INS_Correction(Mag_data,Pos,Vel,X,R,P,Be);
/*Fill ekf result with data*/
ekf_result.Pos_x=X[0];
ekf_result.Pos_y=X[1];
ekf_result.Pos_z=X[2];
ekf_result.Vel_x=X[3];
ekf_result.Vel_y=X[4];
ekf_result.Vel_z=X[5];
ekf_result.q0=X[6];
ekf_result.q1=X[7];
ekf_result.q2=X[8];
ekf_result.q3=X[9];
//Transfer quaternion to euler angle
quaternion_to_euler(1/*is radian:1 OR is Angle:0*/,X[6],X[7],X[8],X[9],&ekf_result.roll,&ekf_result.pitch,&ekf_result.yaw);
}
//check ekf if is ready to arm
ekf_is_ready();
return 1;
}
core::vector3df combine_rotations(const core::vector3df &a, const core::vector3df &b)
{
return quaternion_to_euler(
core::quaternion(a*core::DEGTORAD) * core::quaternion(b*core::DEGTORAD)
);
}
