void QuaternionOperations::testQuaternionProd() { QUATERNION q_a, q_b, qTarget, qProd; q_a.q0 = 9.999955e-01f; q_a.q1 = -2.908411e-03f; q_a.q2 = 7.327030e-04f; q_a.q3 = 1.942619e-06f; q_b.q0 = 0.000000e+00f; q_b.q1 = -1.632653e-02f; q_b.q2 = -6.530612e-02f; q_b.q3 = 1.004082e+00f; qTarget.q0 = -1.584820e-06f; qTarget.q1 = -1.559064e-02f; qTarget.q2 = -6.238558e-02f; qTarget.q3 = 1.004279e+00f; CPPUNIT_ASSERT(QuaternionProd(&q_a, &q_b, &qProd)); CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q0, qProd.q0, 1e-8f); CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q1, qProd.q1, 1e-5f); CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q2, qProd.q2, 1e-5f); CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q3, qProd.q3, 1e-5f); }
static void QuaternionIntegralEulerChange( time_T delta_t, const vector qdot, const vector omegadot, const quaternion q0, quaternion phi ) { int_T j; vector delta; real_T delta_norm_sq; quaternion omega0; quaternion omega1; quaternion omega0_omega1; quaternion omega1_omega0; quaternion q; real_T delta_t3; int_T squaring_times; int_T k; real_T max_elem; /* Calculate omega from qdot */ q[0] = 2 * q0[0]; for( j=1; j<4; j++ ) { q[j] = - 2 * q0[j]; } QuaternionProd(q, qdot, omega0); /* Calculate delta as Euler integral of (omega / 2) */ for( j=0; j<3; j++ ) { delta[j] = 0.5*omega0[j+1]*delta_t; } /* quaternion transition matrix */ /* using repeated squaring to improve precision */ max_elem = abs(delta[0]); for( j=1; j<3; j++ ) { if( abs(delta[j]) > max_elem ) max_elem = abs(delta[j]); } squaring_times = max_elem / 0.25; for(k = 0; k < squaring_times; k++ ) for( j=0; j<3; j++ ) { delta[j] = delta[j] / 2; } delta_norm_sq = 0; for( j=0; j<3; j++ ) { delta_norm_sq += delta[j] * delta[j]; } /* third order Taylor approximation */ phi[0] = 1 - 0.5 * delta_norm_sq; for( j=0; j<3; j++ ) { phi[j+1] = delta[j] * (1 - 1/6*delta_norm_sq); } for( k = 0; k < squaring_times; k++ ) { QuaternionProd(phi, phi, q); for( j=0; j<4; j++ ) { phi[j] = q[j]; } } /* coning motion error term */ omega1[0] = 0; for( j=0; j<3; j++ ) { omega1[j+1] = omegadot[j]; } QuaternionProd(omega0, omega1, omega0_omega1); QuaternionProd(omega1, omega0, omega1_omega0); delta_t3 = delta_t * delta_t * delta_t; for( j=0; j<4; j++ ) { phi[j] += 1/48*(omega0_omega1[j]-omega1_omega0[j])*delta_t3; } /* normalize */ /* delta_t3 = 1/(phi[0]*phi[0] + phi[1]*phi[1] + phi[2]*phi[2] + phi[3]*phi[3]); for( j=0; j<4; j++ ) { phi[j] = phi[j]*delta_t3; } */ }
static void mdlOutputs(SimStruct *S, int_T tid) { PCONFIG_DATA config; const real_T *in_xdot; const real_T *in_x0; InputRealPtrsType pin_params; const boolean_T *in_reset; real_T *out_x; real_T *out_t; real_T *xd; real_T *xd_temp; real_T *xd_temp2; time_T initial_time; time_T final_time; quaternion phi; quaternion q; vector omegadot_temp; const real_T *pomegadot; int_T i; /* Retrieve C object from the pointers vector */ config = ssGetPWork(S)[0]; xd = (real_T*) ssGetDWork(S,0); xd_temp = (real_T*) ssGetDWork(S,1); if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2); in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot); if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0); if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params); if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0]; out_x = ssGetOutputPortRealSignal(S, 1); if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time); switch( intval(mxGetScalar(paramSpecificationsSource)) ) { case 1: initial_time = config->initial_time; final_time = ssGetTaskTime(S,0); break; case 2: initial_time = mxGetScalar(paramInitialTime); final_time = mxGetScalar(paramFinalTime); break; case 3: initial_time = *(pin_params[0]); final_time = *(pin_params[1]); break; default: ssSetErrorStatus(S,"Wrong integration algorithm selected"); return; } /* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */ /* Reset the states */ if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) ) { ssSetIWorkValue(S, 0, 0); if( intval(mxGetScalar(paramInitialConditionSource)) == 1 ) { /* Internal initial conditions */ for( i=0; i<config->nstates; i++ ) { xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)]; } } else { /* External initial conditions */ memcpy(xd, in_x0, config->nstates*sizeof(real_T)); } memcpy(out_x, xd, config->nstates*sizeof(real_T)); if( config->idxout_time ) out_t[0] = initial_time; } if( final_time > initial_time ) { if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 ) { /* Euler algorithm */ if( !ssCallSystemWithTid(S,0,tid) ) return; i = 0; while( i<config->nstates ) { if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) ) { pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero ); QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi ); QuaternionProd(&xd[i], phi, &xd[i]); QuaternionNormalize(&xd[i]); i += 4; } else { xd[i] += in_xdot[i] * (final_time-initial_time); i++; } } } if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 ) { /* Runge-Kutta algorithm */ /* f1 */ if( !ssCallSystemWithTid(S,0,tid) ) return; i = 0; while( i<config->nstates ) { if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) ) { pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero ); omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6; QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] ); QuaternionProd(&xd_temp[i], &xd_temp[i], q); QuaternionProd(&xd_temp[i], q, phi); QuaternionProd(&xd[i], phi, &out_x[i]); i += 4; } else { xd_temp[i] = in_xdot[i]; out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i]; i++; } } if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time); /* f2 */ if( !ssCallSystemWithTid(S,0,tid) ) return; i = 0; while( i<config->nstates ) { if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) ) { pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero ); omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6; QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q ); QuaternionProd(q, q, phi); QuaternionProd(&xd_temp[i], phi, &xd_temp[i]); QuaternionProd(phi, q, phi); QuaternionProd(&xd[i], phi, &out_x[i]); i += 4; } else { xd_temp[i] += 2*in_xdot[i]; out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i]; i++; } } if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time); /* f3 */ if( !ssCallSystemWithTid(S,0,tid) ) return; i = 0; while( i<config->nstates ) { if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) ) { pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero ); omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6; QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q ); QuaternionProd(q, q, phi); QuaternionProd(&xd_temp[i], phi, &xd_temp[i]); QuaternionProd(phi, q, phi); QuaternionProd(phi, phi, phi); QuaternionProd(&xd[i], phi, &out_x[i]); i += 4; } else { xd_temp[i] += 2*in_xdot[i]; out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i]; i++; } } if( config->idxout_time ) out_t[0] = final_time; /* f4 */ if( !ssCallSystemWithTid(S,0,tid) ) return; i = 0; while( i<config->nstates ) { if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) ) { pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero ); omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6; QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q ); QuaternionProd(&xd_temp[i], q, &xd_temp[i]); QuaternionProd(&xd[i], &xd_temp[i], &xd[i]); QuaternionNormalize(&xd[i]); i += 4; } else { xd_temp[i] += in_xdot[i]; xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i]; i++; } } } } else { if( !ssCallSystemWithTid(S,0,tid) ) return; } config->initial_time = final_time; /* Update the outputs */ memcpy(out_x, xd, config->nstates*sizeof(real_T)); if( config->idxout_time ) out_t[0] = final_time; }