/* * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) */ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], const real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { real32_T a[12]; int32_T i; real32_T b_a[12]; real32_T Q[144]; real32_T O[9]; real_T dv0[9]; real32_T c_a[9]; real32_T d_a[9]; real32_T x_n_b[3]; real32_T z_n_b[3]; real32_T x_apriori[12]; real32_T y_n_b[3]; int32_T i0; real32_T e_a[3]; real_T dv1[144]; real32_T A_lin[144]; real32_T b_A_lin[144]; int32_T i1; real32_T y; real32_T P_apriori[144]; real32_T R[81]; real32_T b_P_apriori[108]; static const int8_T iv0[108] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T K_k[108]; static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv0[81]; real32_T c_P_apriori[36]; static const int8_T iv2[36] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0 }; real32_T fv1[36]; static const int8_T iv3[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T S_k[36]; real32_T d_P_apriori[72]; static const int8_T iv4[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; real32_T b_K_k[72]; static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_r[6]; static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; static const int8_T iv7[72] = { 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv2[6]; real32_T b_z[6]; real32_T b_y; /* Extended Attitude Kalmanfilter */ /* */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ /* */ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ /* */ /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ /* 'attitudeKalmanfilter:27' Q = diag(q.^2*dt); */ power(q, 2.0, a); for (i = 0; i < 12; i++) { b_a[i] = a[i] * dt; } diag(b_a, Q); /* observation matrix */ /* 'attitudeKalmanfilter:37' wx= x_aposteriori_k(1); */ /* 'attitudeKalmanfilter:38' wy= x_aposteriori_k(2); */ /* 'attitudeKalmanfilter:39' wz= x_aposteriori_k(3); */ /* 'attitudeKalmanfilter:41' wox= x_aposteriori_k(4); */ /* 'attitudeKalmanfilter:42' woy= x_aposteriori_k(5); */ /* 'attitudeKalmanfilter:43' woz= x_aposteriori_k(6); */ /* 'attitudeKalmanfilter:45' zex= x_aposteriori_k(7); */ /* 'attitudeKalmanfilter:46' zey= x_aposteriori_k(8); */ /* 'attitudeKalmanfilter:47' zez= x_aposteriori_k(9); */ /* 'attitudeKalmanfilter:49' mux= x_aposteriori_k(10); */ /* 'attitudeKalmanfilter:50' muy= x_aposteriori_k(11); */ /* 'attitudeKalmanfilter:51' muz= x_aposteriori_k(12); */ /* 'attitudeKalmanfilter:54' wk =[wx; */ /* 'attitudeKalmanfilter:55' wy; */ /* 'attitudeKalmanfilter:56' wz]; */ /* 'attitudeKalmanfilter:58' wok =[wox;woy;woz]; */ /* 'attitudeKalmanfilter:59' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; O[3] = x_aposteriori_k[2]; O[4] = 0.0F; O[5] = -x_aposteriori_k[0]; O[6] = -x_aposteriori_k[1]; O[7] = x_aposteriori_k[0]; O[8] = 0.0F; /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { c_a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 'attitudeKalmanfilter:61' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { d_a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 'attitudeKalmanfilter:63' EZ=[0,zez,-zey; */ /* 'attitudeKalmanfilter:64' -zez,0,zex; */ /* 'attitudeKalmanfilter:65' zey,-zex,0]'; */ /* 'attitudeKalmanfilter:66' MA=[0,muz,-muy; */ /* 'attitudeKalmanfilter:67' -muz,0,mux; */ /* 'attitudeKalmanfilter:68' zey,-mux,0]'; */ /* 'attitudeKalmanfilter:72' E=eye(3); */ /* 'attitudeKalmanfilter:73' Z=zeros(3); */ /* 'attitudeKalmanfilter:74' x_apriori=[wk;wok;zek;muk]; */ x_n_b[0] = x_aposteriori_k[6]; x_n_b[1] = x_aposteriori_k[7]; x_n_b[2] = x_aposteriori_k[8]; z_n_b[0] = x_aposteriori_k[9]; z_n_b[1] = x_aposteriori_k[10]; z_n_b[2] = x_aposteriori_k[11]; x_apriori[0] = x_aposteriori_k[0]; x_apriori[1] = x_aposteriori_k[1]; x_apriori[2] = x_aposteriori_k[2]; x_apriori[3] = x_aposteriori_k[3]; x_apriori[4] = x_aposteriori_k[4]; x_apriori[5] = x_aposteriori_k[5]; for (i = 0; i < 3; i++) { y_n_b[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { y_n_b[i] += c_a[i + 3 * i0] * x_n_b[i0]; } e_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { e_a[i] += d_a[i + 3 * i0] * z_n_b[i0]; } x_apriori[i + 6] = y_n_b[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 9] = e_a[i]; } /* 'attitudeKalmanfilter:76' A_lin=[ Z, Z, Z, Z */ /* 'attitudeKalmanfilter:77' Z, Z, Z, Z */ /* 'attitudeKalmanfilter:78' EZ, Z, O, Z */ /* 'attitudeKalmanfilter:79' MA, Z, Z, O]; */ /* 'attitudeKalmanfilter:82' A_lin=eye(12)+A_lin*dt; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[i0 + 12 * i] = 0.0F; } for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * i) + 3] = 0.0F; } } A_lin[6] = 0.0F; A_lin[7] = x_aposteriori_k[8]; A_lin[8] = -x_aposteriori_k[7]; A_lin[18] = -x_aposteriori_k[8]; A_lin[19] = 0.0F; A_lin[20] = x_aposteriori_k[6]; A_lin[30] = x_aposteriori_k[7]; A_lin[31] = -x_aposteriori_k[6]; A_lin[32] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; } } A_lin[9] = 0.0F; A_lin[10] = x_aposteriori_k[11]; A_lin[11] = -x_aposteriori_k[10]; A_lin[21] = -x_aposteriori_k[11]; A_lin[22] = 0.0F; A_lin[23] = x_aposteriori_k[9]; A_lin[33] = x_aposteriori_k[7]; A_lin[34] = -x_aposteriori_k[9]; A_lin[35] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * dt; } } /* 'attitudeKalmanfilter:88' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0]; } } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 12; i1++) { y += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } P_apriori[i + 12 * i0] = y + Q[i + 12 * i0]; } } /* %update */ /* 'attitudeKalmanfilter:92' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:93' R=diag(r); */ b_diag(r, R); /* observation matrix */ /* 'attitudeKalmanfilter:96' H_k=[ E, E, Z, Z; */ /* 'attitudeKalmanfilter:97' Z, Z, E, Z; */ /* 'attitudeKalmanfilter:98' Z, Z, Z, E]; */ /* 'attitudeKalmanfilter:100' y_k=z(1:9)-H_k*x_apriori; */ /* 'attitudeKalmanfilter:102' S_k=H_k*P_apriori*H_k'+R; */ /* 'attitudeKalmanfilter:103' K_k=(P_apriori*H_k'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv0[i1 + 12 * i0]; } } } for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 12; i0++) { K_k[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { K_k[i + 9 * i0] += (real32_T)iv1[i + 9 * i1] * P_apriori[i1 + 12 * i0]; } } } for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { y = 0.0F; for (i1 = 0; i1 < 12; i1++) { y += K_k[i + 9 * i1] * (real32_T)iv0[i1 + 12 * i0]; } fv0[i + 9 * i0] = y + R[i + 9 * i0]; } } mrdivide(b_P_apriori, fv0, K_k); /* 'attitudeKalmanfilter:106' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { y = 0.0F; for (i0 = 0; i0 < 12; i0++) { y += (real32_T)iv1[i + 9 * i0] * x_apriori[i0]; } c_a[i] = z[i] - y; } for (i = 0; i < 12; i++) { y = 0.0F; for (i0 = 0; i0 < 9; i0++) { y += K_k[i + 12 * i0] * c_a[i0]; } x_aposteriori[i] = x_apriori[i] + y; } /* 'attitudeKalmanfilter:107' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 9; i1++) { y += K_k[i + 12 * i1] * (real32_T)iv1[i1 + 9 * i0]; } Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:108' else */ /* 'attitudeKalmanfilter:109' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:110' R=diag(r(1:3)); */ c_diag(*(real32_T (*)[3])&r[0], O); /* observation matrix */ /* 'attitudeKalmanfilter:113' H_k=[ E, E, Z, Z]; */ /* 'attitudeKalmanfilter:115' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:117' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ /* 'attitudeKalmanfilter:118' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv2[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { fv1[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv1[i + 3 * i0] += (real32_T)iv3[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { y = 0.0F; for (i1 = 0; i1 < 12; i1++) { y += fv1[i + 3 * i1] * (real32_T)iv2[i1 + 12 * i0]; } c_a[i + 3 * i0] = y + O[i + 3 * i0]; } } b_mrdivide(c_P_apriori, c_a, S_k); /* 'attitudeKalmanfilter:121' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { y = 0.0F; for (i0 = 0; i0 < 12; i0++) { y += (real32_T)iv3[i + 3 * i0] * x_apriori[i0]; } x_n_b[i] = z[i] - y; } for (i = 0; i < 12; i++) { y = 0.0F; for (i0 = 0; i0 < 3; i0++) { y += S_k[i + 12 * i0] * x_n_b[i0]; } x_aposteriori[i] = x_apriori[i] + y; } /* 'attitudeKalmanfilter:122' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 3; i1++) { y += S_k[i + 12 * i1] * (real32_T)iv3[i1 + 3 * i0]; } Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:123' else */ /* 'attitudeKalmanfilter:124' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:125' R=diag(r(1:6)); */ d_diag(*(real32_T (*)[6])&r[0], S_k); /* observation matrix */ /* 'attitudeKalmanfilter:128' H_k=[ E, E, Z, Z; */ /* 'attitudeKalmanfilter:129' Z, Z, E, Z]; */ /* 'attitudeKalmanfilter:131' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:133' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ /* 'attitudeKalmanfilter:134' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv4[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { b_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { b_K_k[i + 6 * i0] += (real32_T)iv5[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { y = 0.0F; for (i1 = 0; i1 < 12; i1++) { y += b_K_k[i + 6 * i1] * (real32_T)iv4[i1 + 12 * i0]; } fv1[i + 6 * i0] = y + S_k[i + 6 * i0]; } } c_mrdivide(d_P_apriori, fv1, b_K_k); /* 'attitudeKalmanfilter:137' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { y = 0.0F; for (i0 = 0; i0 < 12; i0++) { y += (real32_T)iv5[i + 6 * i0] * x_apriori[i0]; } b_r[i] = z[i] - y; } for (i = 0; i < 12; i++) { y = 0.0F; for (i0 = 0; i0 < 6; i0++) { y += b_K_k[i + 12 * i0] * b_r[i0]; } x_aposteriori[i] = x_apriori[i] + y; } /* 'attitudeKalmanfilter:138' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 6; i1++) { y += b_K_k[i + 12 * i1] * (real32_T)iv5[i1 + 6 * i0]; } Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:139' else */ /* 'attitudeKalmanfilter:140' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:141' R=diag([r(1:3);r(7:9)]); */ /* observation matrix */ /* 'attitudeKalmanfilter:144' H_k=[ E, E, Z, Z; */ /* 'attitudeKalmanfilter:145' Z, Z, Z, E]; */ /* 'attitudeKalmanfilter:147' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:149' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { b_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { b_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { b_r[i << 1] = r[i]; b_r[1 + (i << 1)] = r[6 + i]; } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { y = 0.0F; for (i1 = 0; i1 < 12; i1++) { y += b_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; } S_k[i + 6 * i0] = y + b_r[3 * (i + i0)]; } } /* 'attitudeKalmanfilter:150' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv7[i1 + 12 * i0]; } } } c_mrdivide(d_P_apriori, S_k, b_K_k); /* 'attitudeKalmanfilter:153' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { b_r[i] = z[i]; } for (i = 0; i < 3; i++) { b_r[i + 3] = z[i + 6]; } for (i = 0; i < 6; i++) { fv2[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { fv2[i] += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } b_z[i] = b_r[i] - fv2[i]; } for (i = 0; i < 12; i++) { y = 0.0F; for (i0 = 0; i0 < 6; i0++) { y += b_K_k[i + 12 * i0] * b_z[i0]; } x_aposteriori[i] = x_apriori[i] + y; } /* 'attitudeKalmanfilter:154' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { y = 0.0F; for (i1 = 0; i1 < 6; i1++) { y += b_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } Q[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - y; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += Q[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:155' else */ /* 'attitudeKalmanfilter:156' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } /* 'attitudeKalmanfilter:157' P_aposteriori=P_apriori; */ memcpy((void *)&P_aposteriori[0], (void *)&P_apriori[0], 144U * sizeof (real32_T)); } } } } /* % euler anglels extraction */ /* 'attitudeKalmanfilter:166' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ y = norm(*(real32_T (*)[3])&x_aposteriori[6]); /* 'attitudeKalmanfilter:167' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ b_y = norm(*(real32_T (*)[3])&x_aposteriori[9]); /* 'attitudeKalmanfilter:169' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { z_n_b[i] = -x_aposteriori[i + 6] / y; x_n_b[i] = x_aposteriori[i + 9] / b_y; } cross(z_n_b, x_n_b, y_n_b); /* 'attitudeKalmanfilter:170' y_n_b=y_n_b./norm(y_n_b); */ y = norm(y_n_b); for (i = 0; i < 3; i++) { y_n_b[i] /= y; } /* 'attitudeKalmanfilter:172' x_n_b=(cross(y_n_b,z_n_b)); */ cross(y_n_b, z_n_b, x_n_b); /* 'attitudeKalmanfilter:173' x_n_b=x_n_b./norm(x_n_b); */ y = norm(x_n_b); for (i = 0; i < 3; i++) { /* 'attitudeKalmanfilter:179' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ Rot_matrix[i] = x_n_b[i] / y; Rot_matrix[3 + i] = y_n_b[i]; Rot_matrix[6 + i] = z_n_b[i]; } /* 'attitudeKalmanfilter:183' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ /* 'attitudeKalmanfilter:184' theta=-asin(Rot_matrix(1,3)); */ /* 'attitudeKalmanfilter:185' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ /* 'attitudeKalmanfilter:186' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asinf(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); }
/* * function [eulerAngles,Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter_wo(updateVect,dt,z,x_aposteriori_k,P_aposteriori_k,q,r) */ void attitudeKalmanfilter(const uint8_T updateVect[3], real32_T dt, const real32_T z[9], const real32_T x_aposteriori_k[12], const real32_T P_aposteriori_k[144], const real32_T q[12], real32_T r[9], real32_T eulerAngles[3], real32_T Rot_matrix[9], real32_T x_aposteriori[12], real32_T P_aposteriori[144]) { real32_T wak[3]; real32_T O[9]; real_T dv0[9]; real32_T a[9]; int32_T i; real32_T b_a[9]; real32_T x_n_b[3]; real32_T b_x_aposteriori_k[3]; real32_T z_n_b[3]; real32_T c_a[3]; real32_T d_a[3]; int32_T i0; real32_T x_apriori[12]; real_T dv1[144]; real32_T A_lin[144]; static const int8_T iv0[36] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_A_lin[144]; real32_T b_q[144]; real32_T c_A_lin[144]; real32_T d_A_lin[144]; real32_T e_A_lin[144]; int32_T i1; real32_T P_apriori[144]; real32_T b_P_apriori[108]; static const int8_T iv1[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T K_k[108]; real32_T fv0[81]; static const int8_T iv2[108] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; real32_T b_r[81]; real32_T fv1[81]; real32_T f0; real32_T c_P_apriori[36]; static const int8_T iv3[36] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T fv2[36]; static const int8_T iv4[36] = { 1, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T c_r[9]; real32_T b_K_k[36]; real32_T d_P_apriori[72]; static const int8_T iv5[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0 }; real32_T c_K_k[72]; static const int8_T iv6[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; real32_T b_z[6]; static const int8_T iv7[72] = { 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1 }; static const int8_T iv8[72] = { 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1 }; real32_T fv3[6]; real32_T c_z[6]; /* Extended Attitude Kalmanfilter */ /* */ /* state vector x has the following entries [ax,ay,az||mx,my,mz||wox,woy,woz||wx,wy,wz]' */ /* measurement vector z has the following entries [ax,ay,az||mx,my,mz||wmx,wmy,wmz]' */ /* knownConst has the following entries [PrvaA,PrvarM,PrvarWO,PrvarW||MsvarA,MsvarM,MsvarW] */ /* */ /* [x_aposteriori,P_aposteriori] = AttKalman(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ /* */ /* Example.... */ /* */ /* $Author: Tobias Naegeli $ $Date: 2012 $ $Revision: 1 $ */ /* coder.varsize('udpIndVect', [9,1], [1,0]) */ /* udpIndVect=find(updVect); */ /* process and measurement noise covariance matrix */ /* Q = diag(q.^2*dt); */ /* observation matrix */ /* 'attitudeKalmanfilter:33' wx= x_aposteriori_k(1); */ /* 'attitudeKalmanfilter:34' wy= x_aposteriori_k(2); */ /* 'attitudeKalmanfilter:35' wz= x_aposteriori_k(3); */ /* 'attitudeKalmanfilter:37' wax= x_aposteriori_k(4); */ /* 'attitudeKalmanfilter:38' way= x_aposteriori_k(5); */ /* 'attitudeKalmanfilter:39' waz= x_aposteriori_k(6); */ /* 'attitudeKalmanfilter:41' zex= x_aposteriori_k(7); */ /* 'attitudeKalmanfilter:42' zey= x_aposteriori_k(8); */ /* 'attitudeKalmanfilter:43' zez= x_aposteriori_k(9); */ /* 'attitudeKalmanfilter:45' mux= x_aposteriori_k(10); */ /* 'attitudeKalmanfilter:46' muy= x_aposteriori_k(11); */ /* 'attitudeKalmanfilter:47' muz= x_aposteriori_k(12); */ /* % prediction section */ /* body angular accelerations */ /* 'attitudeKalmanfilter:51' wak =[wax;way;waz]; */ wak[0] = x_aposteriori_k[3]; wak[1] = x_aposteriori_k[4]; wak[2] = x_aposteriori_k[5]; /* body angular rates */ /* 'attitudeKalmanfilter:54' wk =[wx; wy; wz] + dt*wak; */ /* derivative of the prediction rotation matrix */ /* 'attitudeKalmanfilter:57' O=[0,-wz,wy;wz,0,-wx;-wy,wx,0]'; */ O[0] = 0.0F; O[1] = -x_aposteriori_k[2]; O[2] = x_aposteriori_k[1]; O[3] = x_aposteriori_k[2]; O[4] = 0.0F; O[5] = -x_aposteriori_k[0]; O[6] = -x_aposteriori_k[1]; O[7] = x_aposteriori_k[0]; O[8] = 0.0F; /* prediction of the earth z vector */ /* 'attitudeKalmanfilter:60' zek =(eye(3)+O*dt)*[zex;zey;zez]; */ eye(dv0); for (i = 0; i < 9; i++) { a[i] = (real32_T)dv0[i] + O[i] * dt; } /* prediction of the magnetic vector */ /* 'attitudeKalmanfilter:63' muk =(eye(3)+O*dt)*[mux;muy;muz]; */ eye(dv0); for (i = 0; i < 9; i++) { b_a[i] = (real32_T)dv0[i] + O[i] * dt; } /* 'attitudeKalmanfilter:65' EZ=[0,zez,-zey; */ /* 'attitudeKalmanfilter:66' -zez,0,zex; */ /* 'attitudeKalmanfilter:67' zey,-zex,0]'; */ /* 'attitudeKalmanfilter:68' MA=[0,muz,-muy; */ /* 'attitudeKalmanfilter:69' -muz,0,mux; */ /* 'attitudeKalmanfilter:70' zey,-mux,0]'; */ /* 'attitudeKalmanfilter:74' E=eye(3); */ /* 'attitudeKalmanfilter:76' Z=zeros(3); */ /* 'attitudeKalmanfilter:77' x_apriori=[wk;wak;zek;muk]; */ x_n_b[0] = x_aposteriori_k[0]; x_n_b[1] = x_aposteriori_k[1]; x_n_b[2] = x_aposteriori_k[2]; b_x_aposteriori_k[0] = x_aposteriori_k[6]; b_x_aposteriori_k[1] = x_aposteriori_k[7]; b_x_aposteriori_k[2] = x_aposteriori_k[8]; z_n_b[0] = x_aposteriori_k[9]; z_n_b[1] = x_aposteriori_k[10]; z_n_b[2] = x_aposteriori_k[11]; for (i = 0; i < 3; i++) { c_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { c_a[i] += a[i + 3 * i0] * b_x_aposteriori_k[i0]; } d_a[i] = 0.0F; for (i0 = 0; i0 < 3; i0++) { d_a[i] += b_a[i + 3 * i0] * z_n_b[i0]; } x_apriori[i] = x_n_b[i] + dt * wak[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 3] = wak[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 6] = c_a[i]; } for (i = 0; i < 3; i++) { x_apriori[i + 9] = d_a[i]; } /* 'attitudeKalmanfilter:81' A_lin=[ Z, E, Z, Z */ /* 'attitudeKalmanfilter:82' Z, Z, Z, Z */ /* 'attitudeKalmanfilter:83' EZ, Z, O, Z */ /* 'attitudeKalmanfilter:84' MA, Z, Z, O]; */ /* 'attitudeKalmanfilter:86' A_lin=eye(12)+A_lin*dt; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[i0 + 12 * i] = (real32_T)iv0[i0 + 3 * i]; } for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * i) + 3] = 0.0F; } } A_lin[6] = 0.0F; A_lin[7] = x_aposteriori_k[8]; A_lin[8] = -x_aposteriori_k[7]; A_lin[18] = -x_aposteriori_k[8]; A_lin[19] = 0.0F; A_lin[20] = x_aposteriori_k[6]; A_lin[30] = x_aposteriori_k[7]; A_lin[31] = -x_aposteriori_k[6]; A_lin[32] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 6] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 6] = O[i0 + 3 * i]; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 6] = 0.0F; } } A_lin[9] = 0.0F; A_lin[10] = x_aposteriori_k[11]; A_lin[11] = -x_aposteriori_k[10]; A_lin[21] = -x_aposteriori_k[11]; A_lin[22] = 0.0F; A_lin[23] = x_aposteriori_k[9]; A_lin[33] = x_aposteriori_k[7]; A_lin[34] = -x_aposteriori_k[9]; A_lin[35] = 0.0F; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 3)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 6)) + 9] = 0.0F; } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { A_lin[(i0 + 12 * (i + 9)) + 9] = O[i0 + 3 * i]; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { b_A_lin[i0 + 12 * i] = (real32_T)dv1[i0 + 12 * i] + A_lin[i0 + 12 * i] * dt; } } /* 'attitudeKalmanfilter:88' Qtemp=[ q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:89' 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:90' 0, 0, q(1), 0, 0, 0, 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:91' 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:92' 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:93' 0, 0, 0, 0, 0, q(2), 0, 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:94' 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:95' 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0, 0; */ /* 'attitudeKalmanfilter:96' 0, 0, 0, 0, 0, 0, 0, 0, q(3), 0, 0, 0; */ /* 'attitudeKalmanfilter:97' 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0, 0; */ /* 'attitudeKalmanfilter:98' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4), 0; */ /* 'attitudeKalmanfilter:99' 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, q(4)]; */ /* 'attitudeKalmanfilter:103' Q=A_lin*Qtemp*A_lin'; */ /* 'attitudeKalmanfilter:106' P_apriori=A_lin*P_aposteriori_k*A_lin'+Q; */ b_q[0] = q[0]; b_q[12] = 0.0F; b_q[24] = 0.0F; b_q[36] = 0.0F; b_q[48] = 0.0F; b_q[60] = 0.0F; b_q[72] = 0.0F; b_q[84] = 0.0F; b_q[96] = 0.0F; b_q[108] = 0.0F; b_q[120] = 0.0F; b_q[132] = 0.0F; b_q[1] = 0.0F; b_q[13] = q[0]; b_q[25] = 0.0F; b_q[37] = 0.0F; b_q[49] = 0.0F; b_q[61] = 0.0F; b_q[73] = 0.0F; b_q[85] = 0.0F; b_q[97] = 0.0F; b_q[109] = 0.0F; b_q[121] = 0.0F; b_q[133] = 0.0F; b_q[2] = 0.0F; b_q[14] = 0.0F; b_q[26] = q[0]; b_q[38] = 0.0F; b_q[50] = 0.0F; b_q[62] = 0.0F; b_q[74] = 0.0F; b_q[86] = 0.0F; b_q[98] = 0.0F; b_q[110] = 0.0F; b_q[122] = 0.0F; b_q[134] = 0.0F; b_q[3] = 0.0F; b_q[15] = 0.0F; b_q[27] = 0.0F; b_q[39] = q[1]; b_q[51] = 0.0F; b_q[63] = 0.0F; b_q[75] = 0.0F; b_q[87] = 0.0F; b_q[99] = 0.0F; b_q[111] = 0.0F; b_q[123] = 0.0F; b_q[135] = 0.0F; b_q[4] = 0.0F; b_q[16] = 0.0F; b_q[28] = 0.0F; b_q[40] = 0.0F; b_q[52] = q[1]; b_q[64] = 0.0F; b_q[76] = 0.0F; b_q[88] = 0.0F; b_q[100] = 0.0F; b_q[112] = 0.0F; b_q[124] = 0.0F; b_q[136] = 0.0F; b_q[5] = 0.0F; b_q[17] = 0.0F; b_q[29] = 0.0F; b_q[41] = 0.0F; b_q[53] = 0.0F; b_q[65] = q[1]; b_q[77] = 0.0F; b_q[89] = 0.0F; b_q[101] = 0.0F; b_q[113] = 0.0F; b_q[125] = 0.0F; b_q[137] = 0.0F; b_q[6] = 0.0F; b_q[18] = 0.0F; b_q[30] = 0.0F; b_q[42] = 0.0F; b_q[54] = 0.0F; b_q[66] = 0.0F; b_q[78] = q[2]; b_q[90] = 0.0F; b_q[102] = 0.0F; b_q[114] = 0.0F; b_q[126] = 0.0F; b_q[138] = 0.0F; b_q[7] = 0.0F; b_q[19] = 0.0F; b_q[31] = 0.0F; b_q[43] = 0.0F; b_q[55] = 0.0F; b_q[67] = 0.0F; b_q[79] = 0.0F; b_q[91] = q[2]; b_q[103] = 0.0F; b_q[115] = 0.0F; b_q[127] = 0.0F; b_q[139] = 0.0F; b_q[8] = 0.0F; b_q[20] = 0.0F; b_q[32] = 0.0F; b_q[44] = 0.0F; b_q[56] = 0.0F; b_q[68] = 0.0F; b_q[80] = 0.0F; b_q[92] = 0.0F; b_q[104] = q[2]; b_q[116] = 0.0F; b_q[128] = 0.0F; b_q[140] = 0.0F; b_q[9] = 0.0F; b_q[21] = 0.0F; b_q[33] = 0.0F; b_q[45] = 0.0F; b_q[57] = 0.0F; b_q[69] = 0.0F; b_q[81] = 0.0F; b_q[93] = 0.0F; b_q[105] = 0.0F; b_q[117] = q[3]; b_q[129] = 0.0F; b_q[141] = 0.0F; b_q[10] = 0.0F; b_q[22] = 0.0F; b_q[34] = 0.0F; b_q[46] = 0.0F; b_q[58] = 0.0F; b_q[70] = 0.0F; b_q[82] = 0.0F; b_q[94] = 0.0F; b_q[106] = 0.0F; b_q[118] = 0.0F; b_q[130] = q[3]; b_q[142] = 0.0F; b_q[11] = 0.0F; b_q[23] = 0.0F; b_q[35] = 0.0F; b_q[47] = 0.0F; b_q[59] = 0.0F; b_q[71] = 0.0F; b_q[83] = 0.0F; b_q[95] = 0.0F; b_q[107] = 0.0F; b_q[119] = 0.0F; b_q[131] = 0.0F; b_q[143] = q[3]; for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_aposteriori_k[i1 + 12 * i0]; } c_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_A_lin[i + 12 * i0] += b_A_lin[i + 12 * i1] * b_q[i1 + 12 * i0]; } } for (i0 = 0; i0 < 12; i0++) { d_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_A_lin[i + 12 * i0] += A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } e_A_lin[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { e_A_lin[i + 12 * i0] += c_A_lin[i + 12 * i1] * b_A_lin[i0 + 12 * i1]; } } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_apriori[i0 + 12 * i] = d_A_lin[i0 + 12 * i] + e_A_lin[i0 + 12 * i]; } } /* % update */ /* 'attitudeKalmanfilter:110' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:111' if z(6)<4 || z(5)>15 */ if ((z[5] < 4.0F) || (z[4] > 15.0F)) { /* 'attitudeKalmanfilter:112' r(2)=10000; */ r[1] = 10000.0F; } /* 'attitudeKalmanfilter:114' R=[r(1),0,0,0,0,0,0,0,0; */ /* 'attitudeKalmanfilter:115' 0,r(1),0,0,0,0,0,0,0; */ /* 'attitudeKalmanfilter:116' 0,0,r(1),0,0,0,0,0,0; */ /* 'attitudeKalmanfilter:117' 0,0,0,r(2),0,0,0,0,0; */ /* 'attitudeKalmanfilter:118' 0,0,0,0,r(2),0,0,0,0; */ /* 'attitudeKalmanfilter:119' 0,0,0,0,0,r(2),0,0,0; */ /* 'attitudeKalmanfilter:120' 0,0,0,0,0,0,r(3),0,0; */ /* 'attitudeKalmanfilter:121' 0,0,0,0,0,0,0,r(3),0; */ /* 'attitudeKalmanfilter:122' 0,0,0,0,0,0,0,0,r(3)]; */ /* observation matrix */ /* [zw;ze;zmk]; */ /* 'attitudeKalmanfilter:125' H_k=[ E, Z, Z, Z; */ /* 'attitudeKalmanfilter:126' Z, Z, E, Z; */ /* 'attitudeKalmanfilter:127' Z, Z, Z, E]; */ /* 'attitudeKalmanfilter:129' y_k=z(1:9)-H_k*x_apriori; */ /* 'attitudeKalmanfilter:132' S_k=H_k*P_apriori*H_k'+R; */ /* 'attitudeKalmanfilter:133' K_k=(P_apriori*H_k'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 9; i0++) { b_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { b_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } } for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 12; i0++) { K_k[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { K_k[i + 9 * i0] += (real32_T)iv2[i + 9 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 9; i0++) { fv0[i + 9 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv0[i + 9 * i0] += K_k[i + 9 * i1] * (real32_T)iv1[i1 + 12 * i0]; } } } b_r[0] = r[0]; b_r[9] = 0.0F; b_r[18] = 0.0F; b_r[27] = 0.0F; b_r[36] = 0.0F; b_r[45] = 0.0F; b_r[54] = 0.0F; b_r[63] = 0.0F; b_r[72] = 0.0F; b_r[1] = 0.0F; b_r[10] = r[0]; b_r[19] = 0.0F; b_r[28] = 0.0F; b_r[37] = 0.0F; b_r[46] = 0.0F; b_r[55] = 0.0F; b_r[64] = 0.0F; b_r[73] = 0.0F; b_r[2] = 0.0F; b_r[11] = 0.0F; b_r[20] = r[0]; b_r[29] = 0.0F; b_r[38] = 0.0F; b_r[47] = 0.0F; b_r[56] = 0.0F; b_r[65] = 0.0F; b_r[74] = 0.0F; b_r[3] = 0.0F; b_r[12] = 0.0F; b_r[21] = 0.0F; b_r[30] = r[1]; b_r[39] = 0.0F; b_r[48] = 0.0F; b_r[57] = 0.0F; b_r[66] = 0.0F; b_r[75] = 0.0F; b_r[4] = 0.0F; b_r[13] = 0.0F; b_r[22] = 0.0F; b_r[31] = 0.0F; b_r[40] = r[1]; b_r[49] = 0.0F; b_r[58] = 0.0F; b_r[67] = 0.0F; b_r[76] = 0.0F; b_r[5] = 0.0F; b_r[14] = 0.0F; b_r[23] = 0.0F; b_r[32] = 0.0F; b_r[41] = 0.0F; b_r[50] = r[1]; b_r[59] = 0.0F; b_r[68] = 0.0F; b_r[77] = 0.0F; b_r[6] = 0.0F; b_r[15] = 0.0F; b_r[24] = 0.0F; b_r[33] = 0.0F; b_r[42] = 0.0F; b_r[51] = 0.0F; b_r[60] = r[2]; b_r[69] = 0.0F; b_r[78] = 0.0F; b_r[7] = 0.0F; b_r[16] = 0.0F; b_r[25] = 0.0F; b_r[34] = 0.0F; b_r[43] = 0.0F; b_r[52] = 0.0F; b_r[61] = 0.0F; b_r[70] = r[2]; b_r[79] = 0.0F; b_r[8] = 0.0F; b_r[17] = 0.0F; b_r[26] = 0.0F; b_r[35] = 0.0F; b_r[44] = 0.0F; b_r[53] = 0.0F; b_r[62] = 0.0F; b_r[71] = 0.0F; b_r[80] = r[2]; for (i = 0; i < 9; i++) { for (i0 = 0; i0 < 9; i0++) { fv1[i0 + 9 * i] = fv0[i0 + 9 * i] + b_r[i0 + 9 * i]; } } mrdivide(b_P_apriori, fv1, K_k); /* 'attitudeKalmanfilter:136' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 9; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv2[i + 9 * i0] * x_apriori[i0]; } O[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 9; i0++) { f0 += K_k[i + 12 * i0] * O[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:137' P_aposteriori=(eye(12)-K_k*H_k)*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 9; i1++) { f0 += K_k[i + 12 * i1] * (real32_T)iv2[i1 + 9 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:138' else */ /* 'attitudeKalmanfilter:139' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:141' R=[r(1),0,0; */ /* 'attitudeKalmanfilter:142' 0,r(1),0; */ /* 'attitudeKalmanfilter:143' 0,0,r(1)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:146' H_k=[ E, Z, Z, Z]; */ /* 'attitudeKalmanfilter:148' y_k=z(1:3)-H_k(1:3,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:150' S_k=H_k(1:3,1:12)*P_apriori*H_k(1:3,1:12)'+R(1:3,1:3); */ /* 'attitudeKalmanfilter:151' K_k=(P_apriori*H_k(1:3,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 3; i0++) { c_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv3[i1 + 12 * i0]; } } } for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 12; i0++) { fv2[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 3 * i0] += (real32_T)iv4[i + 3 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 3; i0++) { O[i + 3 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { O[i + 3 * i0] += fv2[i + 3 * i1] * (real32_T)iv3[i1 + 12 * i0]; } } } c_r[0] = r[0]; c_r[3] = 0.0F; c_r[6] = 0.0F; c_r[1] = 0.0F; c_r[4] = r[0]; c_r[7] = 0.0F; c_r[2] = 0.0F; c_r[5] = 0.0F; c_r[8] = r[0]; for (i = 0; i < 3; i++) { for (i0 = 0; i0 < 3; i0++) { a[i0 + 3 * i] = O[i0 + 3 * i] + c_r[i0 + 3 * i]; } } b_mrdivide(c_P_apriori, a, b_K_k); /* 'attitudeKalmanfilter:154' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv4[i + 3 * i0] * x_apriori[i0]; } x_n_b[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 3; i0++) { f0 += b_K_k[i + 12 * i0] * x_n_b[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:155' P_aposteriori=(eye(12)-K_k*H_k(1:3,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 3; i1++) { f0 += b_K_k[i + 12 * i1] * (real32_T)iv4[i1 + 3 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:156' else */ /* 'attitudeKalmanfilter:157' if updateVect(1)==1&&updateVect(2)==1&&updateVect(3)==0 */ if ((updateVect[0] == 1) && (updateVect[1] == 1) && (updateVect[2] == 0)) { /* 'attitudeKalmanfilter:158' if z(6)<4 || z(5)>15 */ if ((z[5] < 4.0F) || (z[4] > 15.0F)) { /* 'attitudeKalmanfilter:159' r(2)=10000; */ r[1] = 10000.0F; } /* 'attitudeKalmanfilter:162' R=[r(1),0,0,0,0,0; */ /* 'attitudeKalmanfilter:163' 0,r(1),0,0,0,0; */ /* 'attitudeKalmanfilter:164' 0,0,r(1),0,0,0; */ /* 'attitudeKalmanfilter:165' 0,0,0,r(2),0,0; */ /* 'attitudeKalmanfilter:166' 0,0,0,0,r(2),0; */ /* 'attitudeKalmanfilter:167' 0,0,0,0,0,r(2)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:170' H_k=[ E, Z, Z, Z; */ /* 'attitudeKalmanfilter:171' Z, Z, E, Z]; */ /* 'attitudeKalmanfilter:173' y_k=z(1:6)-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:175' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ /* 'attitudeKalmanfilter:176' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv5[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { c_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_K_k[i + 6 * i0] += (real32_T)iv6[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 6; i0++) { fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv5[i1 + 12 * i0]; } } } b_K_k[0] = r[0]; b_K_k[6] = 0.0F; b_K_k[12] = 0.0F; b_K_k[18] = 0.0F; b_K_k[24] = 0.0F; b_K_k[30] = 0.0F; b_K_k[1] = 0.0F; b_K_k[7] = r[0]; b_K_k[13] = 0.0F; b_K_k[19] = 0.0F; b_K_k[25] = 0.0F; b_K_k[31] = 0.0F; b_K_k[2] = 0.0F; b_K_k[8] = 0.0F; b_K_k[14] = r[0]; b_K_k[20] = 0.0F; b_K_k[26] = 0.0F; b_K_k[32] = 0.0F; b_K_k[3] = 0.0F; b_K_k[9] = 0.0F; b_K_k[15] = 0.0F; b_K_k[21] = r[1]; b_K_k[27] = 0.0F; b_K_k[33] = 0.0F; b_K_k[4] = 0.0F; b_K_k[10] = 0.0F; b_K_k[16] = 0.0F; b_K_k[22] = 0.0F; b_K_k[28] = r[1]; b_K_k[34] = 0.0F; b_K_k[5] = 0.0F; b_K_k[11] = 0.0F; b_K_k[17] = 0.0F; b_K_k[23] = 0.0F; b_K_k[29] = 0.0F; b_K_k[35] = r[1]; for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); /* 'attitudeKalmanfilter:179' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 6; i++) { f0 = 0.0F; for (i0 = 0; i0 < 12; i0++) { f0 += (real32_T)iv6[i + 6 * i0] * x_apriori[i0]; } b_z[i] = z[i] - f0; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { f0 += c_K_k[i + 12 * i0] * b_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:180' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { f0 += c_K_k[i + 12 * i1] * (real32_T)iv6[i1 + 6 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:181' else */ /* 'attitudeKalmanfilter:182' if updateVect(1)==1&&updateVect(2)==0&&updateVect(3)==1 */ if ((updateVect[0] == 1) && (updateVect[1] == 0) && (updateVect[2] == 1)) { /* 'attitudeKalmanfilter:183' R=[r(1),0,0,0,0,0; */ /* 'attitudeKalmanfilter:184' 0,r(1),0,0,0,0; */ /* 'attitudeKalmanfilter:185' 0,0,r(1),0,0,0; */ /* 'attitudeKalmanfilter:186' 0,0,0,r(3),0,0; */ /* 'attitudeKalmanfilter:187' 0,0,0,0,r(3),0; */ /* 'attitudeKalmanfilter:188' 0,0,0,0,0,r(3)]; */ /* observation matrix */ /* 'attitudeKalmanfilter:191' H_k=[ E, Z, Z, Z; */ /* 'attitudeKalmanfilter:192' Z, Z, Z, E]; */ /* 'attitudeKalmanfilter:194' y_k=[z(1:3);z(7:9)]-H_k(1:6,1:12)*x_apriori; */ /* 'attitudeKalmanfilter:196' S_k=H_k(1:6,1:12)*P_apriori*H_k(1:6,1:12)'+R(1:6,1:6); */ /* 'attitudeKalmanfilter:197' K_k=(P_apriori*H_k(1:6,1:12)'/(S_k)); */ for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 6; i0++) { d_P_apriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { d_P_apriori[i + 12 * i0] += P_apriori[i + 12 * i1] * (real32_T) iv7[i1 + 12 * i0]; } } } for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 12; i0++) { c_K_k[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { c_K_k[i + 6 * i0] += (real32_T)iv8[i + 6 * i1] * P_apriori[i1 + 12 * i0]; } } for (i0 = 0; i0 < 6; i0++) { fv2[i + 6 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { fv2[i + 6 * i0] += c_K_k[i + 6 * i1] * (real32_T)iv7[i1 + 12 * i0]; } } } b_K_k[0] = r[0]; b_K_k[6] = 0.0F; b_K_k[12] = 0.0F; b_K_k[18] = 0.0F; b_K_k[24] = 0.0F; b_K_k[30] = 0.0F; b_K_k[1] = 0.0F; b_K_k[7] = r[0]; b_K_k[13] = 0.0F; b_K_k[19] = 0.0F; b_K_k[25] = 0.0F; b_K_k[31] = 0.0F; b_K_k[2] = 0.0F; b_K_k[8] = 0.0F; b_K_k[14] = r[0]; b_K_k[20] = 0.0F; b_K_k[26] = 0.0F; b_K_k[32] = 0.0F; b_K_k[3] = 0.0F; b_K_k[9] = 0.0F; b_K_k[15] = 0.0F; b_K_k[21] = r[2]; b_K_k[27] = 0.0F; b_K_k[33] = 0.0F; b_K_k[4] = 0.0F; b_K_k[10] = 0.0F; b_K_k[16] = 0.0F; b_K_k[22] = 0.0F; b_K_k[28] = r[2]; b_K_k[34] = 0.0F; b_K_k[5] = 0.0F; b_K_k[11] = 0.0F; b_K_k[17] = 0.0F; b_K_k[23] = 0.0F; b_K_k[29] = 0.0F; b_K_k[35] = r[2]; for (i = 0; i < 6; i++) { for (i0 = 0; i0 < 6; i0++) { c_P_apriori[i0 + 6 * i] = fv2[i0 + 6 * i] + b_K_k[i0 + 6 * i]; } } c_mrdivide(d_P_apriori, c_P_apriori, c_K_k); /* 'attitudeKalmanfilter:200' x_aposteriori=x_apriori+K_k*y_k; */ for (i = 0; i < 3; i++) { b_z[i] = z[i]; } for (i = 0; i < 3; i++) { b_z[i + 3] = z[i + 6]; } for (i = 0; i < 6; i++) { fv3[i] = 0.0F; for (i0 = 0; i0 < 12; i0++) { fv3[i] += (real32_T)iv8[i + 6 * i0] * x_apriori[i0]; } c_z[i] = b_z[i] - fv3[i]; } for (i = 0; i < 12; i++) { f0 = 0.0F; for (i0 = 0; i0 < 6; i0++) { f0 += c_K_k[i + 12 * i0] * c_z[i0]; } x_aposteriori[i] = x_apriori[i] + f0; } /* 'attitudeKalmanfilter:201' P_aposteriori=(eye(12)-K_k*H_k(1:6,1:12))*P_apriori; */ b_eye(dv1); for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { f0 = 0.0F; for (i1 = 0; i1 < 6; i1++) { f0 += c_K_k[i + 12 * i1] * (real32_T)iv8[i1 + 6 * i0]; } b_A_lin[i + 12 * i0] = (real32_T)dv1[i + 12 * i0] - f0; } } for (i = 0; i < 12; i++) { for (i0 = 0; i0 < 12; i0++) { P_aposteriori[i + 12 * i0] = 0.0F; for (i1 = 0; i1 < 12; i1++) { P_aposteriori[i + 12 * i0] += b_A_lin[i + 12 * i1] * P_apriori[i1 + 12 * i0]; } } } } else { /* 'attitudeKalmanfilter:202' else */ /* 'attitudeKalmanfilter:203' x_aposteriori=x_apriori; */ for (i = 0; i < 12; i++) { x_aposteriori[i] = x_apriori[i]; } /* 'attitudeKalmanfilter:204' P_aposteriori=P_apriori; */ memcpy(&P_aposteriori[0], &P_apriori[0], 144U * sizeof(real32_T)); } } } } /* % euler anglels extraction */ /* 'attitudeKalmanfilter:213' z_n_b = -x_aposteriori(7:9)./norm(x_aposteriori(7:9)); */ for (i = 0; i < 3; i++) { x_n_b[i] = -x_aposteriori[i + 6]; } rdivide(x_n_b, norm(*(real32_T (*)[3])&x_aposteriori[6]), z_n_b); /* 'attitudeKalmanfilter:214' m_n_b = x_aposteriori(10:12)./norm(x_aposteriori(10:12)); */ rdivide(*(real32_T (*)[3])&x_aposteriori[9], norm(*(real32_T (*)[3])& x_aposteriori[9]), wak); /* 'attitudeKalmanfilter:216' y_n_b=cross(z_n_b,m_n_b); */ for (i = 0; i < 3; i++) { x_n_b[i] = wak[i]; } cross(z_n_b, x_n_b, wak); /* 'attitudeKalmanfilter:217' y_n_b=y_n_b./norm(y_n_b); */ for (i = 0; i < 3; i++) { x_n_b[i] = wak[i]; } rdivide(x_n_b, norm(wak), wak); /* 'attitudeKalmanfilter:219' x_n_b=(cross(y_n_b,z_n_b)); */ cross(wak, z_n_b, x_n_b); /* 'attitudeKalmanfilter:220' x_n_b=x_n_b./norm(x_n_b); */ for (i = 0; i < 3; i++) { b_x_aposteriori_k[i] = x_n_b[i]; } rdivide(b_x_aposteriori_k, norm(x_n_b), x_n_b); /* 'attitudeKalmanfilter:226' Rot_matrix=[x_n_b,y_n_b,z_n_b]; */ for (i = 0; i < 3; i++) { Rot_matrix[i] = x_n_b[i]; Rot_matrix[3 + i] = wak[i]; Rot_matrix[6 + i] = z_n_b[i]; } /* 'attitudeKalmanfilter:230' phi=atan2(Rot_matrix(2,3),Rot_matrix(3,3)); */ /* 'attitudeKalmanfilter:231' theta=-asin(Rot_matrix(1,3)); */ /* 'attitudeKalmanfilter:232' psi=atan2(Rot_matrix(1,2),Rot_matrix(1,1)); */ /* 'attitudeKalmanfilter:233' eulerAngles=[phi;theta;psi]; */ eulerAngles[0] = rt_atan2f_snf(Rot_matrix[7], Rot_matrix[8]); eulerAngles[1] = -(real32_T)asin(Rot_matrix[6]); eulerAngles[2] = rt_atan2f_snf(Rot_matrix[3], Rot_matrix[0]); }
void b_Acoeff(const emlrtStack *sp, real_T ksi, real_T j, const emxArray_real_T * x, real_T t, const emxArray_real_T *gridT, emxArray_real_T *vals) { emxArray_real_T *b; emxArray_real_T *r8; int32_T b_x; int32_T i; emxArray_boolean_T *b_t; real_T c_x; emxArray_boolean_T *c_t; emxArray_real_T *z0; emxArray_real_T *d_x; emxArray_real_T *e_x; emxArray_real_T *r9; int32_T b_b[2]; int32_T f_x[2]; emxArray_real_T *g_x; emxArray_real_T *r10; const mxArray *y; static const int32_T iv16[2] = { 1, 45 }; const mxArray *m6; char_T cv18[45]; static const char_T cv19[45] = { 'C', 'o', 'd', 'e', 'r', ':', 't', 'o', 'o', 'l', 'b', 'o', 'x', ':', 'm', 't', 'i', 'm', 'e', 's', '_', 'n', 'o', 'D', 'y', 'n', 'a', 'm', 'i', 'c', 'S', 'c', 'a', 'l', 'a', 'r', 'E', 'x', 'p', 'a', 'n', 's', 'i', 'o', 'n' }; const mxArray *b_y; static const int32_T iv17[2] = { 1, 21 }; char_T cv20[21]; static const char_T cv21[21] = { 'C', 'o', 'd', 'e', 'r', ':', 'M', 'A', 'T', 'L', 'A', 'B', ':', 'i', 'n', 'n', 'e', 'r', 'd', 'i', 'm' }; emxArray_boolean_T *d_t; real_T h_x; emxArray_boolean_T *e_t; emxArray_real_T *i_x; emxArray_real_T *r11; emxArray_real_T *j_x; emxArray_real_T *r12; emxArray_real_T *z1; int32_T b_z0[2]; emxArray_real_T *c_z0; emxArray_real_T *k_x; const mxArray *c_y; static const int32_T iv18[2] = { 1, 45 }; const mxArray *d_y; static const int32_T iv19[2] = { 1, 21 }; emlrtStack st; emlrtStack b_st; emlrtStack c_st; emlrtStack d_st; st.prev = sp; st.tls = sp->tls; b_st.prev = &st; b_st.tls = st.tls; c_st.prev = &b_st; c_st.tls = b_st.tls; d_st.prev = &b_st; d_st.tls = b_st.tls; emlrtHeapReferenceStackEnterFcnR2012b(sp); emxInit_real_T(sp, &b, 2, &bb_emlrtRTEI, true); emxInit_real_T(sp, &r8, 2, &bb_emlrtRTEI, true); /* evaluate the coefficient A at the boundary ksi=0 or ksi=1; */ /* for the index j which describes the time steps timePoints_j, at time t and space */ /* point x */ /* timePoints is a vector describing the time descritized domain */ b_x = gridT->size[1]; i = (int32_T)emlrtIntegerCheckFastR2012b(j, &emlrtDCI, sp); if (t <= gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &d_emlrtBCI, sp) - 1]) { b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = 1; emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof(real_T), &bb_emlrtRTEI); vals->data[0] = 0.0; } else { emxInit_boolean_T(sp, &b_t, 2, &bb_emlrtRTEI, true); b_x = b_t->size[0] * b_t->size[1]; b_t->size[0] = 1; b_t->size[1] = 2 + x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)b_t, b_x, (int32_T)sizeof (boolean_T), &bb_emlrtRTEI); b_x = gridT->size[1]; i = (int32_T)j; b_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &e_emlrtBCI, sp) - 1]); b_x = gridT->size[1]; i = (int32_T)((uint32_T)j + 1U); b_t->data[b_t->size[0]] = (t <= gridT-> data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &f_emlrtBCI, sp) - 1]); i = x->size[1]; for (b_x = 0; b_x < i; b_x++) { b_t->data[b_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] == ksi); } st.site = &pe_emlrtRSI; if (all(&st, b_t)) { b_x = gridT->size[1]; i = (int32_T)j; emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &c_emlrtBCI, sp); c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931; st.site = &emlrtRSI; if (c_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = 1; emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); vals->data[0] = muDoubleScalarSqrt(c_x); } else { emxInit_boolean_T(sp, &c_t, 2, &bb_emlrtRTEI, true); b_x = c_t->size[0] * c_t->size[1]; c_t->size[0] = 1; c_t->size[1] = 2 + x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)c_t, b_x, (int32_T)sizeof (boolean_T), &bb_emlrtRTEI); b_x = gridT->size[1]; i = (int32_T)j; c_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &g_emlrtBCI, sp) - 1]); b_x = gridT->size[1]; i = (int32_T)((uint32_T)j + 1U); c_t->data[c_t->size[0]] = (t <= gridT-> data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &h_emlrtBCI, sp) - 1]); i = x->size[1]; for (b_x = 0; b_x < i; b_x++) { c_t->data[c_t->size[0] * (b_x + 2)] = (x->data[x->size[0] * b_x] != ksi); } emxInit_real_T(sp, &z0, 2, &cb_emlrtRTEI, true); emxInit_real_T(sp, &d_x, 2, &bb_emlrtRTEI, true); st.site = &qe_emlrtRSI; if (all(&st, c_t)) { st.site = &b_emlrtRSI; b_x = gridT->size[1]; i = (int32_T)j; c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &m_emlrtBCI, &st) - 1]; if (c_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } emxInit_real_T(&st, &e_x, 2, &bb_emlrtRTEI, true); b_x = e_x->size[0] * e_x->size[1]; e_x->size[0] = 1; e_x->size[1] = x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)e_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = x->size[0] * x->size[1]; for (b_x = 0; b_x < i; b_x++) { e_x->data[b_x] = x->data[b_x] - ksi; } emxInit_real_T(sp, &r9, 2, &bb_emlrtRTEI, true); b_abs(sp, e_x, r9); b_x = r8->size[0] * r8->size[1]; r8->size[0] = 1; r8->size[1] = r9->size[1]; emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = r9->size[0] * r9->size[1]; emxFree_real_T(&e_x); for (b_x = 0; b_x < i; b_x++) { r8->data[b_x] = r9->data[b_x]; } emxFree_real_T(&r9); rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0); st.site = &re_emlrtRSI; mpower(&st, z0, d_x); b_x = d_x->size[0] * d_x->size[1]; d_x->size[0] = 1; emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0]; b_x = d_x->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { d_x->data[b_x] = -d_x->data[b_x]; } b_x = b->size[0] * b->size[1]; b->size[0] = 1; b->size[1] = d_x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof(real_T), &bb_emlrtRTEI); i = d_x->size[0] * d_x->size[1]; for (b_x = 0; b_x < i; b_x++) { b->data[b_x] = d_x->data[b_x]; } for (i = 0; i < d_x->size[1]; i++) { b->data[i] = muDoubleScalarExp(b->data[i]); } st.site = &re_emlrtRSI; b_mrdivide(&st, b, z0); for (b_x = 0; b_x < 2; b_x++) { i = d_x->size[0] * d_x->size[1]; d_x->size[b_x] = z0->size[b_x]; emxEnsureCapacity(sp, (emxArray__common *)d_x, i, (int32_T)sizeof (real_T), &ab_emlrtRTEI); } for (i = 0; i < z0->size[1]; i++) { d_x->data[i] = scalar_erf(z0->data[i]); } b_x = d_x->size[0] * d_x->size[1]; d_x->size[0] = 1; emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0]; b_x = d_x->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { d_x->data[b_x] *= 1.7724538509055159; } for (b_x = 0; b_x < 2; b_x++) { b_b[b_x] = b->size[b_x]; } for (b_x = 0; b_x < 2; b_x++) { f_x[b_x] = d_x->size[b_x]; } emxInit_real_T(sp, &g_x, 2, &bb_emlrtRTEI, true); emlrtSizeEqCheck2DFastR2012b(b_b, f_x, &o_emlrtECI, sp); b_x = g_x->size[0] * g_x->size[1]; g_x->size[0] = 1; g_x->size[1] = x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)g_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = x->size[0] * x->size[1]; for (b_x = 0; b_x < i; b_x++) { g_x->data[b_x] = x->data[b_x] - ksi; } emxInit_real_T(sp, &r10, 2, &bb_emlrtRTEI, true); b_abs(sp, g_x, r10); b_x = r8->size[0] * r8->size[1]; r8->size[0] = 1; r8->size[1] = r10->size[1]; emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = r10->size[0] * r10->size[1]; emxFree_real_T(&g_x); for (b_x = 0; b_x < i; b_x++) { r8->data[b_x] = r10->data[b_x]; } emxFree_real_T(&r10); rdivide(sp, r8, 3.5449077018110318, vals); st.site = &re_emlrtRSI; b_x = b->size[0] * b->size[1]; b->size[0] = 1; emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = b->size[0]; b_x = b->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { b->data[b_x] -= d_x->data[b_x]; } b_st.site = &he_emlrtRSI; if (!(vals->size[1] == 1)) { if ((vals->size[1] == 1) || (b->size[1] == 1)) { y = NULL; m6 = emlrtCreateCharArray(2, iv16); for (i = 0; i < 45; i++) { cv18[i] = cv19[i]; } emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18); emlrtAssign(&y, m6); c_st.site = &fh_emlrtRSI; d_st.site = &vg_emlrtRSI; b_error(&c_st, message(&d_st, y, &j_emlrtMCI), &k_emlrtMCI); } else { b_y = NULL; m6 = emlrtCreateCharArray(2, iv17); for (i = 0; i < 21; i++) { cv20[i] = cv21[i]; } emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20); emlrtAssign(&b_y, m6); c_st.site = &gh_emlrtRSI; d_st.site = &wg_emlrtRSI; b_error(&c_st, message(&d_st, b_y, &l_emlrtMCI), &m_emlrtMCI); } } c_x = vals->data[0]; b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = b->size[1]; emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = b->size[1]; for (b_x = 0; b_x < i; b_x++) { vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x]; } } else { emxInit_boolean_T(sp, &d_t, 2, &bb_emlrtRTEI, true); b_x = d_t->size[0] * d_t->size[1]; d_t->size[0] = 1; d_t->size[1] = 1 + x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)d_t, b_x, (int32_T)sizeof (boolean_T), &bb_emlrtRTEI); b_x = gridT->size[1]; i = (int32_T)((uint32_T)j + 1U); d_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &i_emlrtBCI, sp) - 1]); i = x->size[1]; for (b_x = 0; b_x < i; b_x++) { d_t->data[d_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] == ksi); } st.site = &se_emlrtRSI; if (all(&st, d_t)) { b_x = gridT->size[1]; i = (int32_T)j; emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &b_emlrtBCI, sp); c_x = (t - gridT->data[(int32_T)j - 1]) / 3.1415926535897931; b_x = gridT->size[1]; i = (int32_T)(j + 1.0); emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &emlrtBCI, sp); h_x = (t - gridT->data[(int32_T)(j + 1.0) - 1]) / 3.1415926535897931; st.site = &c_emlrtRSI; if (c_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } st.site = &c_emlrtRSI; if (h_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = 1; emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); vals->data[0] = muDoubleScalarSqrt(c_x) - muDoubleScalarSqrt(h_x); } else { emxInit_boolean_T(sp, &e_t, 2, &bb_emlrtRTEI, true); b_x = e_t->size[0] * e_t->size[1]; e_t->size[0] = 1; e_t->size[1] = 1 + x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)e_t, b_x, (int32_T)sizeof (boolean_T), &bb_emlrtRTEI); b_x = gridT->size[1]; i = (int32_T)((uint32_T)j + 1U); e_t->data[0] = (t > gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &j_emlrtBCI, sp) - 1]); i = x->size[1]; for (b_x = 0; b_x < i; b_x++) { e_t->data[e_t->size[0] * (b_x + 1)] = (x->data[x->size[0] * b_x] != ksi); } st.site = &te_emlrtRSI; if (all(&st, e_t)) { st.site = &d_emlrtRSI; b_x = gridT->size[1]; i = (int32_T)j; c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &k_emlrtBCI, &st) - 1]; if (c_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } emxInit_real_T(&st, &i_x, 2, &bb_emlrtRTEI, true); b_x = i_x->size[0] * i_x->size[1]; i_x->size[0] = 1; i_x->size[1] = x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)i_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = x->size[0] * x->size[1]; for (b_x = 0; b_x < i; b_x++) { i_x->data[b_x] = x->data[b_x] - ksi; } emxInit_real_T(sp, &r11, 2, &bb_emlrtRTEI, true); b_abs(sp, i_x, r11); b_x = r8->size[0] * r8->size[1]; r8->size[0] = 1; r8->size[1] = r11->size[1]; emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = r11->size[0] * r11->size[1]; emxFree_real_T(&i_x); for (b_x = 0; b_x < i; b_x++) { r8->data[b_x] = r11->data[b_x]; } emxFree_real_T(&r11); rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z0); st.site = &e_emlrtRSI; b_x = gridT->size[1]; i = (int32_T)((uint32_T)j + 1U); c_x = t - gridT->data[emlrtDynamicBoundsCheckFastR2012b(i, 1, b_x, &l_emlrtBCI, &st) - 1]; if (c_x < 0.0) { b_st.site = &f_emlrtRSI; eml_error(&b_st); } emxInit_real_T(&st, &j_x, 2, &bb_emlrtRTEI, true); b_x = j_x->size[0] * j_x->size[1]; j_x->size[0] = 1; j_x->size[1] = x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)j_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = x->size[0] * x->size[1]; for (b_x = 0; b_x < i; b_x++) { j_x->data[b_x] = x->data[b_x] - ksi; } emxInit_real_T(sp, &r12, 2, &bb_emlrtRTEI, true); b_abs(sp, j_x, r12); b_x = r8->size[0] * r8->size[1]; r8->size[0] = 1; r8->size[1] = r12->size[1]; emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = r12->size[0] * r12->size[1]; emxFree_real_T(&j_x); for (b_x = 0; b_x < i; b_x++) { r8->data[b_x] = r12->data[b_x]; } emxFree_real_T(&r12); emxInit_real_T(sp, &z1, 2, &db_emlrtRTEI, true); rdivide(sp, r8, 2.0 * muDoubleScalarSqrt(c_x), z1); st.site = &ue_emlrtRSI; mpower(&st, z0, d_x); b_x = d_x->size[0] * d_x->size[1]; d_x->size[0] = 1; emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0]; b_x = d_x->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { d_x->data[b_x] = -d_x->data[b_x]; } b_x = b->size[0] * b->size[1]; b->size[0] = 1; b->size[1] = d_x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)b, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0] * d_x->size[1]; for (b_x = 0; b_x < i; b_x++) { b->data[b_x] = d_x->data[b_x]; } for (i = 0; i < d_x->size[1]; i++) { b->data[i] = muDoubleScalarExp(b->data[i]); } st.site = &ue_emlrtRSI; b_mrdivide(&st, b, z0); st.site = &ue_emlrtRSI; mpower(&st, z1, d_x); b_x = d_x->size[0] * d_x->size[1]; d_x->size[0] = 1; emxEnsureCapacity(sp, (emxArray__common *)d_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0]; b_x = d_x->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { d_x->data[b_x] = -d_x->data[b_x]; } b_x = r8->size[0] * r8->size[1]; r8->size[0] = 1; r8->size[1] = d_x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)r8, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = d_x->size[0] * d_x->size[1]; for (b_x = 0; b_x < i; b_x++) { r8->data[b_x] = d_x->data[b_x]; } for (i = 0; i < d_x->size[1]; i++) { r8->data[i] = muDoubleScalarExp(r8->data[i]); } st.site = &ue_emlrtRSI; b_mrdivide(&st, r8, z1); for (b_x = 0; b_x < 2; b_x++) { b_b[b_x] = b->size[b_x]; } for (b_x = 0; b_x < 2; b_x++) { b_z0[b_x] = r8->size[b_x]; } emxInit_real_T(sp, &c_z0, 2, &bb_emlrtRTEI, true); emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp); b_x = c_z0->size[0] * c_z0->size[1]; c_z0->size[0] = 1; c_z0->size[1] = z0->size[1]; emxEnsureCapacity(sp, (emxArray__common *)c_z0, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = z0->size[0] * z0->size[1]; for (b_x = 0; b_x < i; b_x++) { c_z0->data[b_x] = z0->data[b_x]; } b_erf(sp, c_z0, z0); b_erf(sp, z1, d_x); emxFree_real_T(&c_z0); emxFree_real_T(&z1); for (b_x = 0; b_x < 2; b_x++) { b_z0[b_x] = z0->size[b_x]; } for (b_x = 0; b_x < 2; b_x++) { f_x[b_x] = d_x->size[b_x]; } emlrtSizeEqCheck2DFastR2012b(b_z0, f_x, &n_emlrtECI, sp); b_x = z0->size[0] * z0->size[1]; z0->size[0] = 1; emxEnsureCapacity(sp, (emxArray__common *)z0, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = z0->size[0]; b_x = z0->size[1]; i *= b_x; for (b_x = 0; b_x < i; b_x++) { z0->data[b_x] = 1.7724538509055159 * (z0->data[b_x] - d_x-> data[b_x]); } for (b_x = 0; b_x < 2; b_x++) { b_b[b_x] = b->size[b_x]; } for (b_x = 0; b_x < 2; b_x++) { b_z0[b_x] = z0->size[b_x]; } emxInit_real_T(sp, &k_x, 2, &bb_emlrtRTEI, true); emlrtSizeEqCheck2DFastR2012b(b_b, b_z0, &m_emlrtECI, sp); b_x = k_x->size[0] * k_x->size[1]; k_x->size[0] = 1; k_x->size[1] = x->size[1]; emxEnsureCapacity(sp, (emxArray__common *)k_x, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = x->size[0] * x->size[1]; for (b_x = 0; b_x < i; b_x++) { k_x->data[b_x] = x->data[b_x] - ksi; } b_abs(sp, k_x, d_x); rdivide(sp, d_x, 3.5449077018110318, vals); st.site = &ue_emlrtRSI; b_x = b->size[0] * b->size[1]; b->size[0] = 1; emxEnsureCapacity(&st, (emxArray__common *)b, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); i = b->size[0]; b_x = b->size[1]; i *= b_x; emxFree_real_T(&k_x); for (b_x = 0; b_x < i; b_x++) { b->data[b_x] = (b->data[b_x] - r8->data[b_x]) + z0->data[b_x]; } b_st.site = &he_emlrtRSI; if (!(vals->size[1] == 1)) { if ((vals->size[1] == 1) || (b->size[1] == 1)) { c_y = NULL; m6 = emlrtCreateCharArray(2, iv18); for (i = 0; i < 45; i++) { cv18[i] = cv19[i]; } emlrtInitCharArrayR2013a(&b_st, 45, m6, cv18); emlrtAssign(&c_y, m6); c_st.site = &fh_emlrtRSI; d_st.site = &vg_emlrtRSI; b_error(&c_st, message(&d_st, c_y, &j_emlrtMCI), &k_emlrtMCI); } else { d_y = NULL; m6 = emlrtCreateCharArray(2, iv19); for (i = 0; i < 21; i++) { cv20[i] = cv21[i]; } emlrtInitCharArrayR2013a(&b_st, 21, m6, cv20); emlrtAssign(&d_y, m6); c_st.site = &gh_emlrtRSI; d_st.site = &wg_emlrtRSI; b_error(&c_st, message(&d_st, d_y, &l_emlrtMCI), &m_emlrtMCI); } } c_x = vals->data[0]; b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = b->size[1]; emxEnsureCapacity(&st, (emxArray__common *)vals, b_x, (int32_T) sizeof(real_T), &bb_emlrtRTEI); i = b->size[1]; for (b_x = 0; b_x < i; b_x++) { vals->data[vals->size[0] * b_x] = c_x * b->data[b->size[0] * b_x]; } } else { b_x = vals->size[0] * vals->size[1]; vals->size[0] = 1; vals->size[1] = 1; emxEnsureCapacity(sp, (emxArray__common *)vals, b_x, (int32_T)sizeof (real_T), &bb_emlrtRTEI); vals->data[0] = 0.0; } emxFree_boolean_T(&e_t); } emxFree_boolean_T(&d_t); } emxFree_boolean_T(&c_t); emxFree_real_T(&d_x); emxFree_real_T(&z0); } emxFree_boolean_T(&b_t); } emxFree_real_T(&r8); emxFree_real_T(&b); emlrtHeapReferenceStackLeaveFcnR2012b(sp); }