static void camOrbit(int dx, int dy)
{
const double radius = 200;
double dist;
Vec3 v = cam_position;
Quaternion o = cam_orientation;
Quaternion q, q2;
/* We invert the transformation because we are transforming the camera
* and not the scene. */
q = quat_conjugate(quat_trackball(dx, dy, radius));
/* The quaternion q gives us an intrinsic transformation, close to unity.
* To make it extrinsic, we compute q2 = o * q * ~o */
q2 = quat_multiply(o, quat_multiply(q, quat_conjugate(o)));
q2 = quat_normalize(q2);
/* As round-off errors accumulate, the distance between the camera and the
* target would normally fluctuate. We take steps to prevent that here. */
dist = vec3_length(v);
v = quat_transform(q2, v);
v = vec3_normalize(v);
v = vec3_scale(v, dist);
cam_position = v;
cam_orientation = quat_multiply(q2, cam_orientation);
}
int quat_Normalize(lua_State *L)
{
quat_t q;
checkquat(L, 1, q);
quat_normalize(q);
return pushquat(L, q);
}
void quat_to_lh_rot_matrix(const union quat *q, float *m)
{
union quat qn;
float qw, qx, qy, qz;
quat_normalize(&qn, q);
qw = qn.v.w;
qx = qn.v.x;
qy = qn.v.y;
qz = qn.v.z;
m[0] = 1.0f - 2.0f * qy * qy - 2.0f * qz * qz;
m[1] = 2.0f * qx * qy - 2.0f * qz * qw;
m[2] = 2.0f * qx * qz + 2.0f * qy * qw;
m[3] = 0.0f;
m[4] = 2.0f * qx * qy + 2.0f * qz * qw;
m[5] = 1.0f - 2.0f * qx * qx - 2.0f * qz * qz;
m[6] = 2.0f * qy * qz - 2.0f * qx * qw;
m[7] = 0.0f;
m[8] = 2.0f * qx * qz - 2.0f * qy * qw;
m[9] = 2.0f * qy * qz + 2.0f * qx * qw;
m[10] = 1.0f - 2.0f * qx * qx - 2.0f * qy * qy;
m[11] = 0.0f;
m[12] = 0.0f;
m[13] = 0.0f;
m[14] = 0.0f;
m[15] = 1.0f;
}
void
quat_q_from_R (quaternion_t q, const double *R, const int ldim)
{
double c2;
c2 = ((R[0] + R[ldim + 1] + R[2*ldim + 2]) + 1.0) / 4.0;
if (c2 < 0.0 || 1.0 < c2)
{
quat_copy (q, quat_one);
return;
}
quat_re (q) = sqrt (c2);
if (quat_re (q) > QUAT_EPS)
{
quat_im (q, 0) = (R[ldim + 2] - R[2*ldim + 1]) / 4.0 / quat_re (q);
quat_im (q, 1) = (R[2*ldim + 0] - R[2]) / 4.0 / quat_re (q);
quat_im (q, 2) = (R[1] - R[ldim]) / 4.0 / quat_re (q);
}
else
{
quat_im (q, 0) = sqrt((R[0] + 1.0)/2.0 - c2);
quat_im (q, 1) = sqrt((R[ldim + 1] + 1.0)/2.0 - c2);
quat_im (q, 2) = sqrt((R[2*ldim + 2] + 1.0)/2.0 - c2);
}
quat_normalize (q);
}
void quat_to_axis_angle(const Quat p, Vec4f axis, float* angle) {
Quat q;
quat_copy(p, q);
quat_normalize(q, q); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
*angle = (float)(2.0 * acos(q[3]));
double s = sqrt(1.0 - q[3] * q[3]); // assuming quaternion normalised then w is less than 1, so term always positive.
if( s < CUTE_EPSILON ) { // test to avoid divide by zero, s is always positive due to sqrt
// if s close to zero then direction of axis not important
axis[0] = q[0]; // if it is important that axis is normalised then replace with x=1; y=z=0;
axis[1] = q[1];
axis[2] = q[2];
axis[3] = 1.0;
} else {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma warning(push)
#pragma warning(disable : 4244)
axis[0] = q[0] / s; // normalise axis
axis[1] = q[1] / s;
axis[2] = q[2] / s;
axis[3] = 1.0;
#pragma warning(pop)
#pragma GCC diagnostic pop
}
float length = vec_length(axis);
if( length < CUTE_EPSILON ) {
*angle = 0.0f;
}
}
static void read_pose(struct ov_pose *pose,
struct _iqm_joint *joint)
{
memcpy(pose->translate, joint->translate, 3 * sizeof(float));
memcpy(pose->rotate, joint->rotate, 4 * sizeof(float));
memcpy(pose->scale, joint->scale, 3 * sizeof(float));
quat_normalize(pose->rotate);
}
int att_of_params(quat_t *att, double a, double b, int c) {
xyz_t v, cross;
quat_t q;
int flag=0;
v.y = a;
v.z = b;
v.x = sqrt(1 - a*a - b*b);
if (isnan(v.x)) {
fprintf(stderr,"att_of_params: %.4f^2 + %.4f^2 > 1. Normalizing and ploughing on regardless.\n",a,b);
// return 1; //invalid parameters
v.x = 0;
xyz_normalize_self(&v);
}
if (!c) {
v.x = -v.x;
if (v.x < -.9999) { // Fix singularity
printf(" singularity\n");
v.y = 1;
flag=1;
}
}
cross.x = 0;
cross.y = -v.z;
cross.z = v.y;
q.q1 = cross.x;
q.q2 = cross.y;
q.q3 = cross.z;
q.q0 = 1 + v.x;
if (quat_norm(&q) < 0.1)
printf(" q_of_params: norm was %.4f for %.3f, %.3f, %d (v.x = %.3f)\n",quat_norm(&q),a,b,c,v.x);
if (flag)
printf(" %.3f, %.3f, %d -> %.3f, %.3f, %.3f, %.3f\n",a,b,c,q.q0,q.q1,q.q2,q.q3);
quat_normalize(&q);
if (flag)
printf(" and after normalizing, that's %.3f, %.3f, %.3f, %.3f\n",q.q0,q.q1,q.q2,q.q3);
quat_inv(att,&q);
return 0;
}
quat
quat_lerp(const quat *q1, const quat *q2, float t)
{
quat q3, q4;
q3 = quat_scale(q1, (1-t));
q4 = quat_scale(q2, t);
q3 = quat_add(&q3, &q4);
return quat_normalize(&q3);
}
static void ahrs_update_imu(ahrs_t *ahrs, real_t gx, real_t gy, real_t gz,
real_t ax, real_t ay, real_t az, real_t dt)
{
/* get rate of change of quaternion from gyroscope: */
vec4_t qdot;
vec4_init(&qdot);
qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);
if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0))))
{
/* normalize accelerometer measurements: */
ahrs_normalize_3(&ax, &ay, &az);
/* auxiliary variables to avoid repeated arithmetic: */
real_t _2q0 = REAL(2.0) * ahrs->quat.q0;
real_t _2q1 = REAL(2.0) * ahrs->quat.q1;
real_t _2q2 = REAL(2.0) * ahrs->quat.q2;
real_t _2q3 = REAL(2.0) * ahrs->quat.q3;
real_t _4q0 = REAL(4.0) * ahrs->quat.q0;
real_t _4q1 = REAL(4.0) * ahrs->quat.q1;
real_t _4q2 = REAL(4.0) * ahrs->quat.q2;
real_t _8q1 = REAL(8.0) * ahrs->quat.q1;
real_t _8q2 = REAL(8.0) * ahrs->quat.q2;
real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
/* gradient decent algorithm corrective step: */
vec4_t s;
vec4_init(&s);
s.ve[0] = _4q0 * q2q2 - _2q2 * ax + _4q0 * q1q1 + _2q1 * ay;
s.ve[1] = _4q1 * q3q3 + _2q3 * ax + REAL(4.0) * q0q0 * ahrs->quat.q1 + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 - _4q1 * az;
s.ve[2] = REAL(4.0) * q0q0 * ahrs->quat.q2 - _2q0 * ax + _4q2 * q3q3 + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 - _4q2 * az;
s.ve[3] = REAL(4.0) * q1q1 * ahrs->quat.q3 + _2q1 * ax + REAL(4.0) * q2q2 * ahrs->quat.q3 + _2q2 * ay;
vec_normalize(&s);
/* apply feedback step: */
vec_scalar_mul(&s, &s, ahrs->beta);
vec_sub(&qdot, &qdot, &s);
}
/* integrate rate of change to yield quaternion: */
vec_scalar_mul(&qdot, &qdot, dt);
vec_add(&ahrs->quat, &ahrs->quat, &qdot);
/* normalise quaternion: */
quat_normalize(&ahrs->quat);
}
/**
* @brief give the normalized quaternion muti result
* @author Yu Yun
* @param[out] q_out: output quaternion
* @param[in] a_in: input quaternion a
* @param[in] b_in: input quaternion b
* @retval None
* @remark benchmarked by: Yu Yun
* result of muliplication has been normalized
*/
void unit_quat_multi(vector4f q_out, const vector4f a, const vector4f b)
{
quat_multi(q_out, a, b);
if(q_out[0] < 0.0f)
{
q_out[0] = -q_out[0];
q_out[1] = -q_out[1];
q_out[2] = -q_out[2];
q_out[3] = -q_out[3];
}
quat_normalize(q_out);
}
static void position_updateAxesFromOrientation (POSITION pdata)
{
QUAT
r;
if (pdata == NULL || pdata->dirty == false)
return;
r = pdata->orientation;
/* FIXME?: wait, why are the side and front vectors transposed here? I
* guess it doesn't really matter, but...
* - xph 2011-03-10
*/
// these values constitute a 3x3 rotation matrix. opengl offsets are commented on each value. see the camera component for how these are used to create the cameraview matrix.
pdata->view.side.x = // [0]
1 -
2 * r.y * r.y -
2 * r.z * r.z;
pdata->view.side.y = // [4]
2 * r.x * r.y -
2 * r.w * r.z;
pdata->view.side.z = // [8]
2 * r.x * r.z +
2 * r.w * r.y;
pdata->view.up.x = // [1]
2 * r.x * r.y +
2 * r.w * r.z;
pdata->view.up.y = // [5]
1 -
2 * r.x * r.x -
2 * r.z * r.z;
pdata->view.up.z = // [9]
2 * r.y * r.z -
2 * r.w * r.x;
pdata->view.front.x = // [2]
2 * r.x * r.z -
2 * r.w * r.y;
pdata->view.front.y = // [6]
2 * r.y * r.z +
2 * r.w * r.x;
pdata->view.front.z = // [10]
1 -
2 * r.x * r.x -
2 * r.y * r.y;
pdata->move.front = vectorCross (&pdata->view.side, &pdata->move.up);
pdata->move.front = vectorNormalize (&pdata->move.front);
pdata->move.side = vectorCross (&pdata->move.up, &pdata->view.front);
pdata->move.side = vectorNormalize (&pdata->move.side);
pdata->move.up = vectorCreate (0, 1, 0);
pdata->orientation = quat_normalize (&pdata->orientation);
pdata->dirty = false;
}
quat
quat_from_axis(const quat *a)
{
float theta = (a->w * 0.5f) * (M_PI/180.f); /* To radians */
float sin_theta = sin(theta);
vec3 tmp = {a->x, a->y, a->z};
quat q;
//tmp = vec3_normalize(&tmp);
q.w = cos(theta);
q.x = tmp.x * sin_theta;
q.y = tmp.y * sin_theta;
q.z = tmp.z * sin_theta;
q = quat_normalize(&q);
return q;
}
// q_out = qa * qb
void
quat_mult(quat_t *q_out, const quat_t * const qa, const quat_t * const qb)
{
q_out->q0 = qa->q0*qb->q0 - qa->q1*qb->q1 - qa->q2*qb->q2 - qa->q3*qb->q3;
q_out->q1 = qa->q0*qb->q1 + qa->q1*qb->q0 + qa->q2*qb->q3 - qa->q3*qb->q2;
q_out->q2 = qa->q0*qb->q2 - qa->q1*qb->q3 + qa->q2*qb->q0 + qa->q3*qb->q1;
q_out->q3 = qa->q0*qb->q3 + qa->q1*qb->q2 - qa->q2*qb->q1 + qa->q3*qb->q0;
if (q_out->q0 < 0){
q_out->q0 = -q_out->q0;
q_out->q1 = -q_out->q1;
q_out->q2 = -q_out->q2;
q_out->q3 = -q_out->q3;
}
quat_normalize( q_out );
}
//Propagation of the states
void ForwardEuler::propagation()
{
geometry_msgs::Vector3 tempVect;
// Propagate the NED coordinates from earth velocity
parentObj->states.pose.position.x = parentObj->states.pose.position.x + parentObj->kinematics.posDot.x * parentObj->dt;
parentObj->states.pose.position.y = parentObj->states.pose.position.y + parentObj->kinematics.posDot.y * parentObj->dt;
parentObj->states.pose.position.z = parentObj->states.pose.position.z + parentObj->kinematics.posDot.z * parentObj->dt;
tempVect.x = parentObj->states.pose.position.x;
tempVect.y = parentObj->states.pose.position.y;
tempVect.z = parentObj->states.pose.position.z;
if (isnan(tempVect)) {ROS_FATAL("NaN member in position vector"); ros::shutdown();}
// Propagate orientation quaternion from angular derivatives quaternion
geometry_msgs::Quaternion quat = parentObj->states.pose.orientation;
quat_product(quat,parentObj->kinematics.quatDot,&(parentObj->states.pose.orientation));
quat_normalize(&(parentObj->states.pose.orientation));
if (isnan(parentObj->states.pose.orientation)) {ROS_FATAL("NaN member in orientation quaternion"); ros::shutdown();}
// Propagate body velocity from body velocity derivatives
tempVect = parentObj->dt*parentObj->kinematics.speedDot;
parentObj->states.velocity.linear = parentObj->states.velocity.linear + tempVect;
if (isnan(parentObj->states.velocity.linear)) {ROS_FATAL("NaN member in linear velocity vector"); ros::shutdown();}
// Propagates angular velocity from angular derivatives
tempVect = parentObj->dt*parentObj->kinematics.rateDot;
parentObj->states.velocity.angular = parentObj->states.velocity.angular + tempVect;
if (isnan(parentObj->states.velocity.angular)) {ROS_FATAL("NaN member in angular velocity vector"); ros::shutdown();}
// Set linear acceleration from the speed derivatives
parentObj->states.acceleration.linear = parentObj->kinematics.speedDot;
// Set angular acceleration from the angular rate derivatives
parentObj->states.acceleration.angular = parentObj->kinematics.rateDot;
//Update Geoid stuff using the NED coordinates
parentObj->states.geoid.latitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.x * parentObj->dt / WGS84_RM(parentObj->states.geoid.latitude));
parentObj->states.geoid.longitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.y * parentObj->dt / WGS84_RN(parentObj->states.geoid.longitude));
parentObj->states.geoid.altitude += -parentObj->kinematics.posDot.z * parentObj->dt;
parentObj->states.geoid.velocity.x = parentObj->kinematics.posDot.x;
parentObj->states.geoid.velocity.y = parentObj->kinematics.posDot.y;
parentObj->states.geoid.velocity.z = -parentObj->kinematics.posDot.z; // Upwards velocity
}
void jedi_processInput_Acceleration(double acc[3], uint32_t accel_time) {
delta_time = accel_time;
const double curAcc =
pow(acc[0] * acc[0] + acc[1] * acc[1] + acc[2] * acc[2], 0.5);
if ((curAcc > gravity - ACC_THRESHOLD) &&
(curAcc < gravity + ACC_THRESHOLD)) {
quat *grav_quat = quat_fromGravityVector(acc);
global_quat = *quat_normalize(quat_add(&global_quat, grav_quat));
// fprintf(stderr, "global_quat %3.2f %3.2f %3.2f %3.2f\n", global_quat.W,
// global_quat.X, global_quat.Y, global_quat.Z);
free(grav_quat);
const double spdSqr = spd_hpf[0] * spd_hpf[0] + spd_hpf[1] * spd_hpf[1] +
spd_hpf[2] * spd_hpf[2];
if (spdSqr < ZERO_SPEED_THRESHOLD * ZERO_SPEED_THRESHOLD) {
spd[0] = 0;
spd[1] = 0;
spd[2] = 0;
spd_lpf[0] = 0;
spd_lpf[1] = 0;
spd_lpf[2] = 0;
spd_hpf[0] = 0;
spd_hpf[1] = 0;
spd_hpf[2] = 0;
if (action_done) {
printf("Ready for the next gesture.\n");
action_done = false;
}
}
vec3 rot_acc = quat_rotateVector(&global_quat, acc);
acc_hold[0] = 0.7 * acc_hold[0] + 0.3 * rot_acc.X;
acc_hold[1] = 0.7 * acc_hold[1] + 0.3 * rot_acc.Y;
acc_hold[2] = 0.7 * acc_hold[2] + 0.3 * rot_acc.Z;
for (int i = 0; i < 3; i++) {
spd[i] += acc_hold[i] * accel_time / 200.0;
spd_lpf[i] += (spd[i] - spd_lpf[i]) *
(1 - exp(-(double)accel_time / SPEED_LPF_TIME_CONSTANT));
spd_hpf[i] = spd[i] - spd_lpf[i];
}
}
}
void jedi_processInput_RotationSpeed(double speed[3], uint32_t gyro_time) {
// if (!global_quat)
// jedi_initQuat();
if (fabs(speed[0]) > GYRO_THRESHOLD)
speed[0] *= 0.07 * gyro_time / 10000.0;
else
speed[0] = 0.0;
if (fabs(speed[1]) > GYRO_THRESHOLD)
speed[1] *= 0.07 * gyro_time / 10000.0;
else
speed[1] = 0.0;
if (fabs(speed[2]) > GYRO_THRESHOLD)
speed[2] *= 0.07 * gyro_time / 10000.0;
else
speed[2] = 0.0;
quat *local = quat_fromAngles(speed);
global_quat = *quat_normalize(quat_multiply(&global_quat, local));
free(local);
// fprintf(stderr, "global_quat %3.2f %3.2f %3.2f %3.2f\n", global_quat.W,
// global_quat.X, global_quat.Y, global_quat.Z);
}
QUAT quat_eulerToQuat (const float x, const float y, const float z)
{
QUAT
q;
float
rx = x / 180.0 * M_PI_2,
ry = y / 180.0 * M_PI_2,
rz = z / 180.0 * M_PI_2,
cx = cos (rx),
cy = cos (ry),
cz = cos (rz),
sx = sin (rx),
sy = sin (ry),
sz = sin (rz);
q.w = cx * cy * cz + sx * sy * sz;
q.x = sx * cy * cz - cx * sy * sz;
q.y = cx * sy * cz + sx * cy * sz;
q.z = cx * cy * sz - sx * sy * cz;
q = quat_normalize (&q);
return q;
}
void quat_normalize_self(union quat *q)
{
quat_normalize(q, q);
}
QuatP* qnormalize(Quat q) {
quat_normalize(q, q);
return q;
}
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time)
{
static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o;
static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL};
static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta;
static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0;
VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL,
*err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL,
*v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL;
MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL},
*err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL},
*P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL};
int update_mag_vec, update_sun_vec, update_omega, i, j;
double d_res;
if (inertia == MNULL)
{
inertia = m_get(3,3);
m_ident(inertia);
inertia->me[0][0] = 0.007;
inertia->me[1][1] = 0.014;
inertia->me[2][2] = 0.015;
}
if (initialize == 0){
iter_num = 1;
n_states = (7+6);
n_err_states = (6+6);
n_sig_pts = 2*n_err_states+1;
alpha = sqrt(3);
kappa = 3 - n_states;
lambda = alpha*alpha * (n_err_states+kappa) - n_err_states;
beta = -(1-(alpha*alpha));
w_m_0 = (lambda)/(n_err_states + lambda);
w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta);
w_i = 0.5/(n_err_states +lambda);
initialize = 1;
sqrt_lambda = (lambda+n_err_states);
if(q_s_c == VNULL)
{
q_s_c = v_get(4);
q_s_c->ve[0] = -0.020656;
q_s_c->ve[1] = 0.71468;
q_s_c->ve[2] = -0.007319;
q_s_c->ve[3] = 0.6991;
}
if(Torq_prev == VNULL)
{
Torq_prev = v_get(3);
v_zero(Torq_prev);
}
quat_normalize(q_s_c);
}
result = m_get(9,9);
m_zero(result);
result1 = m_get(n_err_states, n_err_states);
m_zero(result1);
if(x_m_o == VNULL)
{
x_m_o = v_get(n_states);
v_zero(x_m_o);
}
x = v_get(n_states);
v_zero(x);
x_err_priori = v_get(n_err_states);
v_zero(x_err_priori);
x_ang_vel = v_get(3);
v_zero(x_ang_vel);
sig_pt = m_get(n_states, n_err_states);
m_zero(sig_pt);
if (C == MNULL)
{
C = m_get(9, 12);
m_zero(C);
}
if (P_priori == MNULL)
{
P_priori = m_get(n_err_states, n_err_states);
m_zero(P_priori);
}
if (Q == MNULL)
{
Q = m_get(n_err_states, n_err_states);
m_ident(Q);
//
Q->me[0][0] = 0.0001;
Q->me[1][1] = 0.0001;
Q->me[2][2] = 0.0001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.000001;
Q->me[7][7] = 0.000001;
Q->me[8][8] = 0.000001;
Q->me[9][9] = 0.000001;
Q->me[10][10] = 0.000001;
Q->me[11][11] = 0.000001;
}
if( Pprev == MNULL)
{
Pprev = m_get(n_err_states, n_err_states);
m_ident(Pprev);
Pprev->me[0][0] = 1e-3;
Pprev->me[1][1] = 1e-3;
Pprev->me[2][2] = 1e-3;
Pprev->me[3][3] = 1e-3;
Pprev->me[4][4] = 1e-3;
Pprev->me[5][5] = 1e-3;
Pprev->me[6][6] = 1e-4;
Pprev->me[7][7] = 1e-4;
Pprev->me[8][8] = 1e-4;
Pprev->me[9][9] = 1e-3;
Pprev->me[10][10] = 1e-3;
Pprev->me[11][11] = 1e-3;
}
if (R == MNULL)
{
R = m_get(9,9);
m_ident(R);
R->me[0][0] = 0.034;
R->me[1][1] = 0.034;
R->me[2][2] = 0.034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.000012;
R->me[7][7] = 0.000012;
R->me[8][8] = 0.000012;
}
if(eclipse==0)
{
R->me[0][0] = 0.00034;
R->me[1][1] = 0.00034;
R->me[2][2] = 0.00034;
R->me[3][3] = 0.00027;
R->me[4][4] = 0.00027;
R->me[5][5] = 0.00027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3;
Q->me[4][4] = 0.0001;//0.0175;//1e-3;
Q->me[5][5] = 0.0001;//0.0175;//1e-3;
Q->me[6][6] = 0.0000000001;//1e-6;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
else
{
R->me[0][0] = 0.34;
R->me[1][1] = 0.34;
R->me[2][2] = 0.34;
R->me[3][3] = 0.0027;
R->me[4][4] = 0.0027;
R->me[5][5] = 0.0027;
R->me[6][6] = 0.0000012;
R->me[7][7] = 0.0000012;
R->me[8][8] = 0.0000012;
Q->me[0][0] = 0.00001;
Q->me[1][1] = 0.00001;
Q->me[2][2] = 0.00001;
Q->me[3][3] = 0.0001;
Q->me[4][4] = 0.0001;
Q->me[5][5] = 0.0001;
Q->me[6][6] = 0.0000000001;
Q->me[7][7] = 0.0000000001;
Q->me[8][8] = 0.0000000001;
Q->me[9][9] = 0.0000000001;
Q->me[10][10] = 0.0000000001;
Q->me[11][11] = 0.0000000001;
}
if(omega_prev == VNULL)
{
omega_prev = v_get(3);
v_zero(omega_prev);
}
if(mag_vec_prev == VNULL)
{
mag_vec_prev = v_get(3);
v_zero(mag_vec_prev);
}
if(sun_vec_prev == VNULL)
{
sun_vec_prev = v_get(3);
v_zero(sun_vec_prev);
}
if (err_sig_pt_mat == MNULL)
{
err_sig_pt_mat = m_get(n_err_states, n_sig_pts);
m_zero(err_sig_pt_mat);
}
if(q_err_quat == VNULL)
{
q_err_quat = v_get(4);
// q_err_quat = v_resize(q_err_quat,4);
v_zero(q_err_quat);
}
if(err_vec == VNULL)
{
err_vec = v_get(3);
v_zero(err_vec);
}
v_temp = v_get(9);
v_resize(v_temp,3);
if(x_prev == VNULL)
{
x_prev = v_get(n_states);
v_zero(x_prev);
x_prev->ve[3] = 1;
quat_mul(x_prev,q_s_c,x_prev);
x_prev->ve[4] = 0.0;
x_prev->ve[5] = 0.0;
x_prev->ve[6] = 0.0;
x_prev->ve[7] = 0.0;
x_prev->ve[8] = 0.0;
x_prev->ve[9] = 0.0;
x_prev->ve[10] = 0.0;
x_prev->ve[11] = 0.0;
x_prev->ve[12] = 0.0;
}
sqrt_P = m_get(n_err_states, n_err_states);
m_zero(sqrt_P);
//result = m_resize(result, n_err_states, n_err_states);
result_larger = m_get(n_err_states, n_err_states);
int n, m;
for(n = 0; n < result->n; n++)
{
for(m = 0; m < result->m; m++)
{
result_larger->me[m][n] = result->me[m][n];
}
}
//v_resize(v_temp, n_err_states);
V_FREE(v_temp);
v_temp = v_get(n_err_states);
symmeig(Pprev, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_copy(Pprev, result1);
sm_mlt(sqrt_lambda, result1, result_larger);
catchall(CHfactor(result_larger), printerr(sim_time));
for(i=0; i<n_err_states; i++){
for(j=i+1; j<n_err_states; j++){
result_larger->me[i][j] = 0;
}
}
expandstate(result_larger, x_prev, sig_pt);
sig_vec_mat = m_get(n_states, n_sig_pts);
m_zero(sig_vec_mat);
for(j = 0; j<(n_err_states+1); j++)
{
for(i = 0; i<n_states; i++)
{
if(j==0)
{
sig_vec_mat->me[i][j] = x_prev->ve[i];
}
else if(j>0)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-1];
}
}
}
sm_mlt(-1,result_larger,result_larger);
expandstate(result_larger, x_prev, sig_pt);
for(j = (n_err_states+1); j<n_sig_pts; j++)
{
for(i = 0; i<n_states; i++)
{
sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)];
}
}
single_sig_pt = v_get(n_states);
quat_rot_vec(q_s_c, Torq_ext);
for(j=0; j<(n_sig_pts); j++)
{
//v_temp = v_resize(v_temp,n_states);
V_FREE(v_temp);
v_temp = v_get(n_states);
get_col(sig_vec_mat, j, single_sig_pt);
v_copy(single_sig_pt, v_temp);
rk4(t, v_temp, h, Torq_prev);
set_col(sig_vec_mat, j, v_temp);
}
v_copy(Torq_ext, Torq_prev);
x_priori = v_get(n_states);
v_zero(x_priori);
v_resize(v_temp,n_states);
v_zero(v_temp);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp);
if(j == 0)
{
v_mltadd(x_priori, v_temp, w_m_0, x_priori);
}
else
{
v_mltadd(x_priori, v_temp, w_i, x_priori);
}
}
v_copy(x_priori, v_temp);
v_resize(v_temp,4);
quat_normalize(v_temp);//zaroori hai ye
for(i=0; i<4; i++)
{
x_priori->ve[i] = v_temp->ve[i];
}
v_resize(v_temp, n_states);
v_copy(x_priori, v_temp);
v_resize(v_temp, 4);
quat_inv(v_temp, v_temp);
for(i=0; i<3; i++)
{
x_ang_vel->ve[i] = x_priori->ve[i+4];
}
x_b_m = v_get(3);
v_zero(x_b_m);
x_b_g = v_get(3);
v_zero(x_b_g);
/////////////////////////check it!!!!!!!! checked... doesnt change much the estimate
for(i=0; i<3; i++)
{
x_b_m->ve[i] = x_priori->ve[i+7];
x_b_g->ve[i] = x_priori->ve[i+10];
}
v_temp2 = v_get(n_states);
v_zero(v_temp2);
for(j=0; j<n_sig_pts; j++)
{
v_resize(v_temp2, n_states);
get_col( sig_vec_mat, j, v_temp2);
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+4];
}
v_resize(v_temp2, 4);
quat_mul(v_temp2, v_temp, q_err_quat);
v_resize(q_err_quat, n_err_states);
v_sub(err_vec, x_ang_vel, err_vec);
for(i=3; i<6; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-3];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+7];
}
v_sub(err_vec, x_b_m, err_vec);
for(i=6; i<9; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-6];
}
for(i=0; i<3; i++)
{
err_vec->ve[i] = v_temp2->ve[i+10];
}
v_sub(err_vec, x_b_g, err_vec);
for(i=9; i<12; i++)
{
q_err_quat->ve[i] = err_vec->ve[i-9];
}
set_col(err_sig_pt_mat, j, q_err_quat);
if(j==0){
v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori);
}
else{
v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori);
}
}
v_resize(v_temp,n_err_states);
for (j=0;j<13;j++)
{
get_col(err_sig_pt_mat, j, v_temp);
v_sub(v_temp, x_err_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
}
else{
sm_mlt(w_i, result_larger, result_larger);
}
m_add(P_priori, result_larger, P_priori);
}
symmeig(P_priori, result_larger, v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_priori, Q, P_priori);
v_resize(v_temp,3);
meas = v_get(9);
if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){
update_sun_vec =1;
v_copy(sun_vec, sun_vec_prev);
v_copy(sun_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i = 0; i<3;i++){
meas->ve[i] = v_temp->ve[i];
}
}
else{
update_sun_vec =0;
for(i = 0; i<3;i++){
meas->ve[i] = 0;
}
}
if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){
update_mag_vec =1;
v_copy(mag_vec, mag_vec_prev);
v_copy(mag_vec, v_temp);
normalize_vec(v_temp);
quat_rot_vec(q_s_c, v_temp);
normalize_vec(v_temp);
for(i=3; i<6; i++){
meas->ve[i] = v_temp->ve[i-3];
}
}
else{
update_mag_vec =0;
for(i=3; i<6; i++){
meas->ve[i] = 0;//mag_vec_prev->ve[i-3];
}
}
if (!(is_vec_equal(omega, omega_prev) ) ){
update_omega =1;
v_copy(omega, omega_prev);
v_copy(omega, v_temp);
quat_rot_vec(q_s_c, v_temp);
for(i=6; i<9; i++){
meas->ve[i] = v_temp->ve[i-6];
}
}
else{
update_omega =0;
for(i=6; i<9; i++){
meas->ve[i] = 0;
}
}
v_resize(v_temp, 9);
v_resize(v_temp2, n_states);
v_temp3 = v_get(3);
Meas_err_mat = m_get(9, n_sig_pts);
m_zero(Meas_err_mat);
meas_priori = v_get(9);
v_zero(meas_priori);
for(j=0; j<n_sig_pts; j++)
{
get_col( sig_vec_mat, j, v_temp2);
if(update_omega){
for(i=6;i<9;i++){
v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6];
}
}
else{
for(i=6;i<9;i++){
v_temp->ve[i] = 0;
}
}
v_resize(v_temp2, 4);
if(update_sun_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = sun_vec_I->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp->ve[i] = v_temp3->ve[i];
}
}
else{
for(i=0;i<3;i++){
v_temp->ve[i] = 0;
}
}
if(update_mag_vec){
for(i=0;i<3;i++){
v_temp3->ve[i] = mag_vec_I->ve[i];
}
normalize_vec(v_temp3);
for(i=0;i<3;i++){
v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i];
}
quat_rot_vec(v_temp2, v_temp3);
normalize_vec(v_temp3);
for(i=3;i<6;i++){
v_temp->ve[i] = v_temp3->ve[i-3];
}
}
else{
for(i=3;i<6;i++){
v_temp->ve[i] = 0;
}
}
set_col(Meas_err_mat, j, v_temp);
if(j==0){
v_mltadd(meas_priori, v_temp, w_m_0, meas_priori);
}
else{
v_mltadd(meas_priori, v_temp, w_i, meas_priori);
}
}
v_resize(v_temp, 9);
m_resize(result_larger, 9, 9);
m_zero(result_larger);
P_zz = m_get(9, 9);
m_zero(P_zz);
iP_vv = m_get(9, 9);
m_zero(iP_vv);
P_xz = m_get(n_err_states, 9);
m_zero(P_xz);
v_resize(v_temp2, n_err_states);
result1 = m_resize(result1,n_err_states,9);
for (j=0; j<n_sig_pts; j++)
{
get_col( Meas_err_mat, j, v_temp);
get_col( err_sig_pt_mat, j, v_temp2);
v_sub(v_temp, meas_priori, v_temp);
get_dyad(v_temp, v_temp, result_larger);
get_dyad(v_temp2, v_temp, result1);
if(j==0){
sm_mlt(w_c_0, result_larger, result_larger);
sm_mlt(w_c_0, result1, result1);
}
else{
sm_mlt(w_i, result_larger, result_larger);
sm_mlt(w_i, result1, result1);
}
m_add(P_zz, result_larger, P_zz);
m_add(P_xz, result1, P_xz);
}
symmeig(P_zz, result_larger, v_temp);
i = 0;
for (j=0; j<9; j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
m_add(P_zz, R, P_zz);
m_inverse(P_zz, iP_vv);
K = m_get(n_err_states, 9);
m_zero(K);
m_mlt(P_xz, iP_vv, K);
if(x_posteriori_err == VNULL)
{
x_posteriori_err = v_get(n_err_states);
v_zero(x_posteriori_err);
}
v_resize(v_temp,9);
v_sub(meas, meas_priori, v_temp);
v_copy(v_temp, residual);
mv_mlt(K, v_temp, x_posteriori_err);
v_resize(v_temp2,3);
for(i=0;i<3;i++){
v_temp2->ve[i] = x_posteriori_err->ve[i];
}
for(i=4; i<n_states; i++){
x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]);
}
d_res = v_norm2(v_temp2);
v_resize(v_temp2,4);
if(d_res<=1 /*&& d_res!=0*/){
v_temp2->ve[0] = v_temp2->ve[0];
v_temp2->ve[1] = v_temp2->ve[1];
v_temp2->ve[2] = v_temp2->ve[2];
v_temp2->ve[3] = sqrt(1-d_res);
}
else//baad main daala hai
{
v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res));
v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res));
v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res));
v_temp2->ve[3] = 1/sqrt(1 + d_res);
}
v_resize(x_posteriori_err, n_states);
for(i=(n_states-1); i>3; i--){
x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1];
}
for(i=0; i<4; i++){
x_posteriori_err->ve[i] = v_temp2->ve[i];
}
quat_mul(v_temp2, x_priori, v_temp2);
for(i=0;i<4;i++){
x_prev->ve[i] = v_temp2->ve[i];
}
m_resize(result_larger, n_err_states, 9);
m_mlt(K, P_zz, result_larger);
result2 = m_get(9, n_err_states);
m_transp(K,result2);
m_resize(result1, n_err_states, n_err_states);
m_mlt(result_larger, result2, result1);
v_resize(v_temp, n_err_states);
m_sub(P_priori, result1, Pprev);
symmeig(Pprev, result1 , v_temp);
i = 0;
for (j=0;j<n_err_states;j++){
if(v_temp->ve[j]>=0);
else{
i = 1;
}
}
v_copy(x_prev, v_temp);
v_resize(v_temp,4);
v_copy(x_prev, v_temp2);
v_resize(v_temp2,4);
v_copy(x_prev, x_m_o);
//v_resize(x_m_o, 4);
v_resize(v_temp,3);
quat_inv(q_s_c, v_temp2);
v_copy( x_prev, state);
quat_mul(state, v_temp2, state);
for(i=0; i<3; i++){
v_temp->ve[i] = state->ve[i+4];
}
quat_rot_vec(v_temp2, v_temp);
for(i=0; i<3; i++){
state->ve[i+4] = v_temp->ve[i];
}
v_copy( x_posteriori_err, st_error);
iter_num++;
V_FREE(x);
V_FREE(x_priori);
V_FREE(x_err_priori);
V_FREE(single_sig_pt);
V_FREE(v_temp);
V_FREE(q_err_quat);
V_FREE(err_vec);
V_FREE(v_temp2);
V_FREE(x_ang_vel);
V_FREE(meas);
V_FREE(meas_priori);
V_FREE(v_temp3);
V_FREE(x_posteriori_err);
V_FREE(x_b_m);
V_FREE(x_b_g);
M_FREE(sqrt_P);
M_FREE(P);
M_FREE(P_priori);
M_FREE(sig_pt);
M_FREE(sig_vec_mat);
M_FREE(err_sig_pt_mat);
M_FREE(result);
M_FREE(result_larger);
M_FREE(result1);
M_FREE(Meas_err_mat);
M_FREE(P_zz);
M_FREE(iP_vv);
M_FREE(P_xz);
M_FREE(K);
M_FREE(result2);
M_FREE(result3);
}
int ahrs_update(ahrs_t *ahrs, const marg_data_t *marg_data, const real_t dt)
{
int ret;
if (ahrs->beta > ahrs->beta_end)
{
ahrs->beta -= ahrs->beta_step * dt;
if (ahrs->beta < ahrs->beta_end)
{
ahrs->beta = ahrs->beta_end;
ret = 1;
}
else
{
ret = -1;
}
}
else
{
ret = 0;
}
real_t gx = marg_data->gyro.x;
real_t gy = marg_data->gyro.y;
real_t gz = marg_data->gyro.z;
real_t ax = marg_data->acc.x;
real_t ay = marg_data->acc.y;
real_t az = marg_data->acc.z;
real_t mx = marg_data->mag.x;
real_t my = marg_data->mag.y;
real_t mz = marg_data->mag.z;
/* use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
or if requested upon initialization: */
if (((mx == REAL(0.0)) && (my == REAL(0.0)) && (mz == REAL(0.0))) || ahrs->type == AHRS_ACC)
{
ahrs_update_imu(ahrs, gx, gy, gz, ax, ay, az, dt);
goto out;
}
/* Rate of change of quaternion from gyroscope */
vec4_t qdot;
vec4_init(&qdot);
qdot.ve[0] = REAL(0.5) * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - ahrs->quat.q3 * gz);
qdot.ve[1] = REAL(0.5) * ( ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - ahrs->quat.q3 * gy);
qdot.ve[2] = REAL(0.5) * ( ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + ahrs->quat.q3 * gx);
qdot.ve[3] = REAL(0.5) * ( ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - ahrs->quat.q2 * gx);
if (!((ax == REAL(0.0)) && (ay == REAL(0.0)) && (az == REAL(0.0))))
{
/* normalise accelerometer and magnetometer measurements: */
ahrs_normalize_3(&ax, &ay, &az);
ahrs_normalize_3(&mx, &my, &mz);
/* auxiliary variables to avoid repeated arithmetic: */
real_t _2q0mx = REAL(2.0) * ahrs->quat.q0 * mx;
real_t _2q0my = REAL(2.0) * ahrs->quat.q0 * my;
real_t _2q0mz = REAL(2.0) * ahrs->quat.q0 * mz;
real_t _2q1mx = REAL(2.0) * ahrs->quat.q1 * mx;
real_t _2q0 = REAL(2.0) * ahrs->quat.q0;
real_t _2q1 = REAL(2.0) * ahrs->quat.q1;
real_t _2q2 = REAL(2.0) * ahrs->quat.q2;
real_t _2q3 = REAL(2.0) * ahrs->quat.q3;
real_t _2q0q2 = REAL(2.0) * ahrs->quat.q0 * ahrs->quat.q2;
real_t _2q2q3 = REAL(2.0) * ahrs->quat.q2 * ahrs->quat.q3;
real_t q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
real_t q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
real_t q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
real_t q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
real_t q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
real_t q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
real_t q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
real_t q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
real_t q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
real_t q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
float q0 = ahrs->quat.q0;
float q1 = ahrs->quat.q1;
float q2 = ahrs->quat.q2;
float q3 = ahrs->quat.q3;
/* reference direction of Earth's magnetic field: */
real_t hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
real_t hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
real_t _2bx = sqrt(hx * hx + hy * hy);
real_t _2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
real_t _4bx = 2.0f * _2bx;
real_t _4bz = 2.0f * _2bz;
real_t _8bx = 2.0f * _4bx;
real_t _8bz = 2.0f * _4bz;
// Gradient decent algorithm corrective step
vec4_t s;
vec4_init(&s);
s.ve[0] = -_2q2*(2.0f*(q1q3 - q0q2) - ax) + _2q1*(2.0f*(q0q1 + q2q3) - ay) + -_4bz*q2*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (-_4bx*q3+_4bz*q1)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + _4bx*q2*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[1] = _2q3*(2.0f*(q1q3 - q0q2) - ax) + _2q0*(2.0f*(q0q1 + q2q3) - ay) + -4.0f*q1*(2.0f*(0.5 - q1q1 - q2q2) - az) + _4bz*q3*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx) + (_4bx*q2+_4bz*q0)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my) + (_4bx*q3-_8bz*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[2] = -_2q0*(2.0f*(q1q3 - q0q2) - ax) + _2q3*(2.0f*(q0q1 + q2q3) - ay) + (-4.0f*q2)*(2.0f*(0.5 - q1q1 - q2q2) - az) + (-_8bx*q2-_4bz*q0)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(_4bx*q1+_4bz*q3)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q0-_8bz*q2)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
s.ve[3] = _2q1*(2.0f*(q1q3 - q0q2) - ax) + _2q2*(2.0f*(q0q1 + q2q3) - ay)+(-_8bx*q3+_4bz*q1)*(_4bx*(0.5 - q2q2 - q3q3) + _4bz*(q1q3 - q0q2) - mx)+(-_4bx*q0+_4bz*q2)*(_4bx*(q1q2 - q0q3) + _4bz*(q0q1 + q2q3) - my)+(_4bx*q1)*(_4bx*(q0q2 + q1q3) + _4bz*(0.5 - q1q1 - q2q2) - mz);
vec_normalize(&s);
/* apply feedback step: */
vec_scalar_mul(&s, &s, ahrs->beta);
vec_sub(&qdot, &qdot, &s);
}
/* integrate rate of change to yield quaternion: */
vec_scalar_mul(&qdot, &qdot, dt);
vec_add(&ahrs->quat, &ahrs->quat, &qdot);
/* normalise quaternion: */
quat_normalize(&ahrs->quat);
out:
return ret;
}