/* Function Definitions */
real_T Dcoeff(const emlrtStack *sp, const emxArray_real_T *y, real_T j, const
emxArray_real_T *x, real_T t, const emxArray_real_T *gridT, const
emxArray_real_T *f)
{
real_T vals;
emxArray_real_T *a;
int32_T i57;
int32_T unnamed_idx_1;
emxArray_real_T *r7;
int32_T i58;
int32_T i59;
int32_T i60;
int32_T i61;
int32_T i62;
int32_T i63;
int32_T i64;
int32_T i65;
emlrtStack st;
st.prev = sp;
st.tls = sp->tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &a, 2, &c_emlrtRTEI, true);
/* Calculate the integral int_0^1(f(y,t_j)*A_{yj}(x,t)dy) by midpoint */
/* integration rule */
/* the spatial integration of all points y at time t_j */
i57 = a->size[0] * a->size[1];
a->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)a, i57, (int32_T)sizeof(real_T),
&g_emlrtRTEI);
unnamed_idx_1 = y->size[1];
i57 = a->size[0] * a->size[1];
a->size[1] = unnamed_idx_1;
emxEnsureCapacity(sp, (emxArray__common *)a, i57, (int32_T)sizeof(real_T),
&g_emlrtRTEI);
unnamed_idx_1 = y->size[1];
for (i57 = 0; i57 < unnamed_idx_1; i57++) {
a->data[i57] = 0.0;
}
emxInit_real_T(sp, &r7, 2, &g_emlrtRTEI, true);
vals = 0.0;
i57 = y->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i57, &kb_emlrtBCI, sp);
st.site = &v_emlrtRSI;
b_Acoeff(&st, y->data[0], j, x, t, gridT, r7);
i57 = r7->size[1];
emlrtSizeEqCheck1DFastR2012b(1, i57, &k_emlrtECI, sp);
unnamed_idx_1 = y->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, unnamed_idx_1, &jb_emlrtBCI, sp);
i57 = r7->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i57, &pe_emlrtBCI, sp);
a->data[0] = r7->data[0];
/* y = 0:1/(numel(y)-2):1; */
unnamed_idx_1 = 2;
while (unnamed_idx_1 - 2 <= f->size[1] - 2) {
i57 = y->size[1];
st.site = &w_emlrtRSI;
b_Acoeff(&st, y->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1,
i57, &hc_emlrtBCI, sp) - 1], j, x, t, gridT, r7);
i57 = r7->size[1];
emlrtSizeEqCheck1DFastR2012b(1, i57, &l_emlrtECI, sp);
i57 = r7->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i57, &qe_emlrtBCI, sp);
i57 = a->size[1];
a->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1, i57,
&gc_emlrtBCI, sp) - 1] = r7->data[0];
i57 = a->size[1];
i58 = f->size[1];
i59 = a->size[1];
i60 = unnamed_idx_1 - 1;
i61 = f->size[1];
i62 = unnamed_idx_1 - 1;
i63 = y->size[1];
i64 = y->size[1];
i65 = unnamed_idx_1 - 1;
vals += 0.5 * (a->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1,
i57, &ic_emlrtBCI, sp) - 1] * f->data[emlrtDynamicBoundsCheckFastR2012b
(unnamed_idx_1, 1, i58, &ib_emlrtBCI, sp) - 1] + a->
data[emlrtDynamicBoundsCheckFastR2012b(i60, 1, i59,
&jc_emlrtBCI, sp) - 1] * f->data[emlrtDynamicBoundsCheckFastR2012b(i62, 1,
i61, &hb_emlrtBCI, sp) - 1]) * (y->data[emlrtDynamicBoundsCheckFastR2012b
(unnamed_idx_1, 1, i63, &kc_emlrtBCI, sp) - 1] - y->
data[emlrtDynamicBoundsCheckFastR2012b(i65, 1, i64, &lc_emlrtBCI, sp) - 1]);
unnamed_idx_1++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
emxFree_real_T(&r7);
emxFree_real_T(&a);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
return vals;
}
/* Function Definitions */
real_T Dcoeff(const emlrtStack *sp, const emxArray_real_T *y, real_T j, const
emxArray_real_T *x, real_T t, const emxArray_real_T *timePoints,
const emxArray_real_T *f)
{
real_T vals;
emxArray_real_T *a;
int32_T i11;
int32_T unnamed_idx_1;
emxArray_real_T *r4;
int32_T i12;
int32_T i13;
int32_T i14;
int32_T i15;
int32_T i16;
int32_T i17;
int32_T i18;
int32_T i19;
emlrtStack st;
st.prev = sp;
st.tls = sp->tls;
emlrtHeapReferenceStackEnterFcnR2012b(sp);
emxInit_real_T(sp, &a, 2, &e_emlrtRTEI, true);
/* Calculate the integral int_0^1(f(y,t_j)*A_{yj}(x,t)dy) by midpoint */
/* integration rule */
/* the spatial integration of all points y at time t_j */
i11 = a->size[0] * a->size[1];
a->size[0] = 1;
emxEnsureCapacity(sp, (emxArray__common *)a, i11, (int32_T)sizeof(real_T),
&d_emlrtRTEI);
unnamed_idx_1 = y->size[1];
i11 = a->size[0] * a->size[1];
a->size[1] = unnamed_idx_1;
emxEnsureCapacity(sp, (emxArray__common *)a, i11, (int32_T)sizeof(real_T),
&d_emlrtRTEI);
unnamed_idx_1 = y->size[1];
for (i11 = 0; i11 < unnamed_idx_1; i11++) {
a->data[i11] = 0.0;
}
emxInit_real_T(sp, &r4, 2, &d_emlrtRTEI, true);
vals = 0.0;
i11 = y->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i11, &ib_emlrtBCI, sp);
st.site = &q_emlrtRSI;
b_Acoeff(&st, y->data[0], j, x, t, timePoints, r4);
i11 = r4->size[1];
emlrtSizeEqCheck1DFastR2012b(1, i11, &c_emlrtECI, sp);
unnamed_idx_1 = y->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, unnamed_idx_1, &jb_emlrtBCI, sp);
i11 = r4->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i11, &kb_emlrtBCI, sp);
a->data[0] = r4->data[0];
unnamed_idx_1 = 2;
while (unnamed_idx_1 - 2 <= y->size[1] - 2) {
i11 = y->size[1];
st.site = &r_emlrtRSI;
b_Acoeff(&st, y->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1,
i11, &mb_emlrtBCI, sp) - 1], j, x, t, timePoints, r4);
i11 = r4->size[1];
emlrtSizeEqCheck1DFastR2012b(1, i11, &d_emlrtECI, sp);
i11 = r4->size[1];
emlrtDynamicBoundsCheckFastR2012b(1, 1, i11, &lb_emlrtBCI, sp);
i11 = a->size[1];
a->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1, i11,
&nb_emlrtBCI, sp) - 1] = r4->data[0];
i11 = a->size[1];
i12 = f->size[1];
i13 = a->size[1];
i14 = unnamed_idx_1 - 1;
i15 = f->size[1];
i16 = unnamed_idx_1 - 1;
i17 = y->size[1];
i18 = y->size[1];
i19 = unnamed_idx_1 - 1;
vals += 0.5 * (a->data[emlrtDynamicBoundsCheckFastR2012b(unnamed_idx_1, 1,
i11, &ob_emlrtBCI, sp) - 1] * f->data[emlrtDynamicBoundsCheckFastR2012b
(unnamed_idx_1, 1, i12, &pb_emlrtBCI, sp) - 1] + a->
data[emlrtDynamicBoundsCheckFastR2012b(i14, 1, i13,
&qb_emlrtBCI, sp) - 1] * f->data[emlrtDynamicBoundsCheckFastR2012b(i16, 1,
i15, &rb_emlrtBCI, sp) - 1]) * (y->data[emlrtDynamicBoundsCheckFastR2012b
(unnamed_idx_1, 1, i17, &sb_emlrtBCI, sp) - 1] - y->
data[emlrtDynamicBoundsCheckFastR2012b(i19, 1, i18, &tb_emlrtBCI, sp) - 1]);
unnamed_idx_1++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, sp);
}
emxFree_real_T(&r4);
emxFree_real_T(&a);
emlrtHeapReferenceStackLeaveFcnR2012b(sp);
return vals;
}
/* Function Definitions */
void update_eq_17_state(const real_T xk_data[6250], const int32_T xk_size[2],
const emxArray_real_T *vk, const real_T uk_data[250], const int32_T uk_size[1],
real_T xkPlus_data[6250], int32_T xkPlus_size[2])
{
int32_T i0;
int32_T i1;
uint8_T uv0[2];
int32_T loop_ub;
int32_T k;
real_T xdot[17];
real_T vk_data[100];
real_T wi[3];
real_T ai[3];
real_T b_xk_data[25];
real_T xk[9];
real_T b_xk[3];
real_T c_xk[3];
real_T dv0[9];
real_T B;
real_T dv1[9];
real_T d_xk[12];
real_T dv2[12];
static const int8_T b[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
real_T xkPlus[4];
real_T dv3[9];
real_T e_xk[9];
real_T c_xk_data[25];
real_T dv4[9];
uint8_T tmp_data[25];
int32_T iv0[1];
static const int32_T iv1[1] = { 17 };
real_T f_xk[17];
/* xk: 17 x 2N+1, inertial position, inertial velocity, inertial quaternion, relative position, and relative quaternion */
/* vk: 12 x 2N+1, noise on wi,(wj_est),ai,(vj_inertial) */
/* uk[wi;ai;mag_i] : 9 x 2N+1 */
/* IMU measurements of me */
emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &l_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 1 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &j_emlrtBCI,
emlrtRootTLSGlobal);
}
emlrtVectorVectorIndexCheckR2012b(uk_size[0], 1, 1, 3, &k_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 4 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, uk_size[0], &k_emlrtBCI,
emlrtRootTLSGlobal);
}
for (i0 = 0; i0 < 2; i0++) {
uv0[i0] = (uint8_T)xk_size[i0];
}
xkPlus_size[0] = uv0[0];
xkPlus_size[1] = uv0[1];
loop_ub = uv0[0] * uv0[1];
for (i0 = 0; i0 < loop_ub; i0++) {
xkPlus_data[i0] = 0.0;
}
k = 0;
while (k <= xk_size[1] - 1) {
i0 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i0, 1, xk_size[1], &i_emlrtBCI,
emlrtRootTLSGlobal);
/* hist position relative to my. My body frame */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &j_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 11 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &l_emlrtBCI,
emlrtRootTLSGlobal);
}
/* my velocity */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 3, &i_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 3; i0++) {
i1 = 4 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &m_emlrtBCI,
emlrtRootTLSGlobal);
}
/* my attitude */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &h_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = 7 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &n_emlrtBCI,
emlrtRootTLSGlobal);
}
/* his attitude relative to mine. His body frame */
emlrtVectorVectorIndexCheckR2012b(xk_size[0], 1, 1, 4, &g_emlrtECI,
emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = 14 + i0;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, xk_size[0], &o_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &h_emlrtBCI, emlrtRootTLSGlobal);
memset(&xdot[0], 0, 17U * sizeof(real_T));
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &f_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 10 + i1;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &p_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &e_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 4 + i1;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i0, &q_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &d_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
loop_ub = vk->size[0];
for (i1 = 0; i1 < loop_ub; i1++) {
vk_data[i1] = vk->data[i1 + vk->size[0] * k];
}
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 1 + i1;
wi[i1] = uk_data[i1] + vk_data[emlrtDynamicBoundsCheckFastR2012b(loop_ub,
1, i0, &r_emlrtBCI, emlrtRootTLSGlobal) - 1];
}
i0 = vk->size[0];
emlrtVectorVectorIndexCheckR2012b(i0, 1, 1, 3, &c_emlrtECI,
emlrtRootTLSGlobal);
i0 = vk->size[0];
loop_ub = vk->size[0];
for (i1 = 0; i1 < loop_ub; i1++) {
vk_data[i1] = vk->data[i1 + vk->size[0] * k];
}
for (i1 = 0; i1 < 3; i1++) {
loop_ub = 7 + i1;
ai[i1] = uk_data[i1 + 3] + vk_data[emlrtDynamicBoundsCheckFastR2012b
(loop_ub, 1, i0, &s_emlrtBCI, emlrtRootTLSGlobal) - 1];
}
/* cosine matrix */
/* Tim Woodbury */
/* modified for AERO 622 */
/* */
/* [Y] = attpar(X,I,options) */
/* Function to convert between attitude parametrizations */
/* */
/* INPUTS: */
/* X - matrix input of appropriate dimension (detailed later) */
/* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */
/* attitude parametrizations, corresponding to the numbers in the section, */
/* "I/O SPECIFICATION PARAMETERS". */
/* options - a data structure. Currently only allows output Euler angle */
/* sequence to be defined. Supported members are "seq" which should be a */
/* [3 x 1] vector describing the first, second, and third rotations in */
/* the desired output sequence. */
/* */
/* OUTPUTS: */
/* Y - output matrix of appropriate dimensions. */
/* All angles are in radians. */
/* */
/* I/O SPECIFICATION PARAMETERS: */
/* 1 - Direction cosine matrix, dimensions [3 x 3] */
/* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */
/* [1,2] is the principal angle (rad). */
/* 3 - 2-angle parametrization, [3 x 4] */
/* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */
/* and [2,1]-[2,2]-[2,3] is the rotation sequence. */
/* [3-1-3] sequence is default for output. Other sequences may be specified */
/* by passing an optional data structure with a [3 x 1] member "seq" whose */
/* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */
/* may be used by specifying the second column of input appropriately. */
/* 5 - Classical Rodrigues parameters [3 x 1] */
/* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */
/* 7 - modified Rodrigues parameters [3 x 1] */
/* 8 - exponential matrix, [3 x 3] */
/* 9 - Cayley-Klein parameters, [2 x 2] */
/* check if any options are passed */
/* %check if output and inp are the same - if so do nothing */
/* for each inp, convert to DCM */
/* quaternion */
/* disp('Input value specified as quaternion.'); */
/* convert DCM to output form */
/* DCM */
/* disp('Output value specified as direction cosine matrix.'); */
/* time rate of my position */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
xk[0] = ((xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7] -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
xk[1] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] +
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]);
xk[2] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]);
xk[3] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 8] -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 6]);
xk[4] = ((xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) -
xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
xk[5] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] +
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]);
xk[6] = 2.0 * (xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 9] +
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 6]);
xk[7] = 2.0 * (xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 6]);
xk[8] = ((xk_data[xk_size[0] * k + 9] * xk_data[xk_size[0] * k + 9] -
xk_data[xk_size[0] * k + 7] * xk_data[xk_size[0] * k + 7]) -
xk_data[xk_size[0] * k + 8] * xk_data[xk_size[0] * k + 8]) +
xk_data[xk_size[0] * k + 6] * xk_data[xk_size[0] * k + 6];
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = b_xk_data[i0 + 3];
}
for (i0 = 0; i0 < 3; i0++) {
c_xk[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
c_xk[i0] += xk[i0 + 3 * i1] * b_xk[i1];
}
xdot[i0] = c_xk[i0];
}
/* time rate of my velocity */
/* returns the cross product matrix of the vector x */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv0[0] = 0.0;
dv0[3] = -wi[2];
dv0[6] = wi[1];
dv0[1] = wi[2];
dv0[4] = 0.0;
dv0[7] = -wi[0];
dv0[2] = -wi[1];
dv0[5] = wi[0];
dv0[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = b_xk_data[i0 + 3];
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += dv0[i0 + 3 * i1] * b_xk[i1];
}
c_xk[i0] = ai[i0] - B;
}
for (i0 = 0; i0 < 3; i0++) {
xdot[3 + i0] = c_xk[i0];
}
/* time rate of my attitude */
/* A matrix for qin */
/* returns the cross product matrix of the vector x */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv1[0] = 0.0;
dv1[3] = -xk_data[xk_size[0] * k + 9];
dv1[6] = xk_data[xk_size[0] * k + 8];
dv1[1] = xk_data[xk_size[0] * k + 9];
dv1[4] = 0.0;
dv1[7] = -xk_data[xk_size[0] * k + 7];
dv1[2] = -xk_data[xk_size[0] * k + 8];
dv1[5] = xk_data[xk_size[0] * k + 7];
dv1[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
d_xk[i0 << 2] = -b_xk_data[i0 + 7];
for (i1 = 0; i1 < 3; i1++) {
d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 6] * (real_T)b[i1
+ 3 * i0] + dv1[i1 + 3 * i0];
}
for (i1 = 0; i1 < 4; i1++) {
dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)];
}
}
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
xkPlus[i0] += dv2[i0 + (i1 << 2)] * wi[i1];
}
xdot[6 + i0] = xkPlus[i0];
}
/* relative attitude cosine matrix */
/* Tim Woodbury */
/* modified for AERO 622 */
/* */
/* [Y] = attpar(X,I,options) */
/* Function to convert between attitude parametrizations */
/* */
/* INPUTS: */
/* X - matrix input of appropriate dimension (detailed later) */
/* I - 2 x 1 indexing vector indicating the input (I(1)) and output (I(2)) */
/* attitude parametrizations, corresponding to the numbers in the section, */
/* "I/O SPECIFICATION PARAMETERS". */
/* options - a data structure. Currently only allows output Euler angle */
/* sequence to be defined. Supported members are "seq" which should be a */
/* [3 x 1] vector describing the first, second, and third rotations in */
/* the desired output sequence. */
/* */
/* OUTPUTS: */
/* Y - output matrix of appropriate dimensions. */
/* All angles are in radians. */
/* */
/* I/O SPECIFICATION PARAMETERS: */
/* 1 - Direction cosine matrix, dimensions [3 x 3] */
/* 2 - Euler principal axis/angle, [3 x 2]. [:,1] is the principal axis; */
/* [1,2] is the principal angle (rad). */
/* 3 - 2-angle parametrization, [3 x 4] */
/* 4 - Euler angle sequence, [3 x 2]. [:,1] are the rotation angles in radians, */
/* and [2,1]-[2,2]-[2,3] is the rotation sequence. */
/* [3-1-3] sequence is default for output. Other sequences may be specified */
/* by passing an optional data structure with a [3 x 1] member "seq" whose */
/* entries [a;b;c] correspond to the sequence a-b-c. Any input sequence */
/* may be used by specifying the second column of input appropriately. */
/* 5 - Classical Rodrigues parameters [3 x 1] */
/* 6 - quaternion [4 x 1]. The scalar part of the quaternion comes FIRST. */
/* 7 - modified Rodrigues parameters [3 x 1] */
/* 8 - exponential matrix, [3 x 3] */
/* 9 - Cayley-Klein parameters, [2 x 2] */
/* check if any options are passed */
/* %check if output and inp are the same - if so do nothing */
/* for each inp, convert to DCM */
/* quaternion */
/* disp('Input value specified as quaternion.'); */
/* convert DCM to output form */
/* DCM */
/* disp('Output value specified as direction cosine matrix.'); */
/* relative angular velocity in j frame */
/* A matrix for q_ji */
/* returns the cross product matrix of the vector x */
/* time rate of relative attitude */
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv3[0] = 0.0;
dv3[3] = -xk_data[xk_size[0] * k + 16];
dv3[6] = xk_data[xk_size[0] * k + 15];
dv3[1] = xk_data[xk_size[0] * k + 16];
dv3[4] = 0.0;
dv3[7] = -xk_data[xk_size[0] * k + 14];
dv3[2] = -xk_data[xk_size[0] * k + 15];
dv3[5] = xk_data[xk_size[0] * k + 14];
dv3[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
d_xk[i0 << 2] = -b_xk_data[i0 + 14];
for (i1 = 0; i1 < 3; i1++) {
d_xk[(i1 + (i0 << 2)) + 1] = xk_data[xk_size[0] * k + 13] * (real_T)b[i1
+ 3 * i0] + dv3[i1 + 3 * i0];
}
}
loop_ub = vk->size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
vk_data[i0] = vk->data[i0 + vk->size[0] * k];
}
e_xk[0] = ((xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14] -
xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) -
xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
e_xk[3] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15]
+ xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k +
13]);
e_xk[6] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16]
- xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k +
13]);
e_xk[1] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 15]
- xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k +
13]);
e_xk[4] = ((xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15] -
xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) -
xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
e_xk[7] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16]
+ xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k +
13]);
e_xk[2] = 2.0 * (xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 16]
+ xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k +
13]);
e_xk[5] = 2.0 * (xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 16]
- xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k +
13]);
e_xk[8] = ((xk_data[xk_size[0] * k + 16] * xk_data[xk_size[0] * k + 16] -
xk_data[xk_size[0] * k + 14] * xk_data[xk_size[0] * k + 14]) -
xk_data[xk_size[0] * k + 15] * xk_data[xk_size[0] * k + 15]) +
xk_data[xk_size[0] * k + 13] * xk_data[xk_size[0] * k + 13];
for (i0 = 0; i0 < 3; i0++) {
for (i1 = 0; i1 < 4; i1++) {
dv2[i1 + (i0 << 2)] = 0.5 * d_xk[i1 + (i0 << 2)];
}
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += e_xk[i0 + 3 * i1] * wi[i1];
}
c_xk[i0] = vk_data[i0 + 3] - B;
}
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = 0.0;
for (i1 = 0; i1 < 3; i1++) {
xkPlus[i0] += dv2[i0 + (i1 << 2)] * c_xk[i1];
}
xdot[13 + i0] = xkPlus[i0];
}
/* time rate of relative position */
/* returns the cross product matrix of the vector x */
loop_ub = vk->size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
vk_data[i0] = vk->data[i0 + vk->size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
c_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
dv4[0] = 0.0;
dv4[3] = -wi[2];
dv4[6] = wi[1];
dv4[1] = wi[2];
dv4[4] = 0.0;
dv4[7] = -wi[0];
dv4[2] = -wi[1];
dv4[5] = wi[0];
dv4[8] = 0.0;
for (i0 = 0; i0 < 3; i0++) {
b_xk[i0] = c_xk_data[i0 + 10];
}
for (i0 = 0; i0 < 3; i0++) {
B = 0.0;
for (i1 = 0; i1 < 3; i1++) {
B += dv4[i0 + 3 * i1] * b_xk[i1];
}
c_xk[i0] = (vk_data[i0 + 9] - b_xk_data[i0 + 3]) - B;
}
for (i0 = 0; i0 < 3; i0++) {
xdot[10 + i0] = c_xk[i0];
}
emlrtSizeEqCheck1DFastR2012b(xk_size[0], 17, &b_emlrtECI, emlrtRootTLSGlobal);
loop_ub = uv0[0];
for (i0 = 0; i0 < loop_ub; i0++) {
tmp_data[i0] = (uint8_T)i0;
}
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &g_emlrtBCI, emlrtRootTLSGlobal);
iv0[0] = uv0[0];
emlrtSubAssignSizeCheckR2012b(iv0, 1, iv1, 1, &emlrtECI, emlrtRootTLSGlobal);
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
b_xk_data[i0] = xk_data[i0 + xk_size[0] * k];
}
loop_ub = xk_size[0];
for (i0 = 0; i0 < loop_ub; i0++) {
c_xk_data[i0] = b_xk_data[i0];
}
for (i0 = 0; i0 < 17; i0++) {
f_xk[i0] = c_xk_data[i0] + Ts * xdot[i0];
}
loop_ub = uv0[0];
for (i0 = 0; i0 < loop_ub; i0++) {
xkPlus_data[tmp_data[i0] + uv0[0] * k] = f_xk[i0];
}
/* re-normalize */
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &f_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &t_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &e_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1,
&u_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1];
}
B = norm(xkPlus);
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &d_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 6] / B;
}
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 7 + i0;
xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &v_emlrtBCI,
emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0];
}
i0 = uv0[1];
i1 = (int32_T)(1.0 + (real_T)k);
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &c_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &w_emlrtBCI,
emlrtRootTLSGlobal);
}
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &b_emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
xkPlus[i0] = xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1,
&x_emlrtBCI, emlrtRootTLSGlobal) + uv0[0] * k) - 1];
}
B = norm(xkPlus);
i0 = uv0[1];
i1 = 1 + k;
emlrtDynamicBoundsCheckFastR2012b(i1, 1, i0, &emlrtBCI, emlrtRootTLSGlobal);
for (i0 = 0; i0 < 4; i0++) {
xkPlus[i0] = xkPlus_data[(i0 + uv0[0] * k) + 13] / B;
}
for (i0 = 0; i0 < 4; i0++) {
i1 = uv0[0];
loop_ub = 14 + i0;
xkPlus_data[(emlrtDynamicBoundsCheckFastR2012b(loop_ub, 1, i1, &y_emlrtBCI,
emlrtRootTLSGlobal) + uv0[0] * k) - 1] = xkPlus[i0];
}
k++;
emlrtBreakCheckFastR2012b(emlrtBreakCheckR2012bFlagVar, emlrtRootTLSGlobal);
}
}
