MATRIX mat_cart2pol(MATRIX A, int dim, MATRIX result) { int m, n, i; m = MatCol(A); n = MatRow(A); if(dim==0 && n>1) { if(result==NULL)if((result = mat_creat(2, m, ZERO_MATRIX))==NULL) return NULL; for(i=0; i<m; ++i) __cart2pol(A[0][i], A[1][i], result[0][i], result[1][i]); return result; } if(dim==1 && m>1) { if(result==NULL)if((result = mat_creat(n, 2, ZERO_MATRIX))==NULL) return NULL; for(i=0; i<n; ++i) __cart2pol(A[i][0], A[i][1], result[i][0], result[i][1]); return result; } return mat_error (MAT_SIZEMISMATCH); }
MATRIX mat_get_sub_matrix_from_cols(MATRIX A, INT_VECTOR indices, MATRIX result) { int i, j, k, n; k = MatRow(A); n = Int_VecLen(indices); if(result==NULL) if((result = mat_creat(k, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<n; ++i) { for(j=0; j<k; ++j) result[j][i] = A[j][indices[i]]; } return result; }
MATRIX mat_sum_col(MATRIX A, MATRIX result) { int i, j, m, n; m = MatCol(A); n = MatRow(A); if(result==NULL) if((result = mat_creat(1, m, ZERO_MATRIX))==NULL) mat_error(MAT_MALLOC); #pragma omp parallel for private(j) for(i=0; i<m; ++i) { result[0][i] = 0.0; for(j=0; j<n; ++j) result[0][i] += A[j][i]; } return result; }
MATRIX mat_creat_diag(MATRIX diag_vals, MATRIX result) { int i, sz; if(MatCol(diag_vals)==1) sz = MatRow(diag_vals); else sz = MatCol(diag_vals); if(result==NULL) if((result = mat_creat(sz, sz, ZERO_MATRIX))==NULL) mat_error(MAT_MALLOC); { if(MatCol(diag_vals)==1) for(i=0; i<sz; ++i) result[i][i] = diag_vals[i][0]; else for(i=0; i<sz; ++i) result[i][i] = diag_vals[0][i]; return result; } }
/* *----------------------------------------------------------------------------- * funct: mat_minor * desct: find minor * given: A = a square matrix, * i=row, j=col * retrn: the minor of Aij *----------------------------------------------------------------------------- */ double mat_minor(MATRIX A,int i,int j) { MATRIX S; double result; if ( ( S = mat_creat(MatRow(A)-1, MatCol(A)-1, UNDEFINED) ) == NULL ) return -1.0; mat_submat(A, i, j, S); result = mat_det( S ); mat_free(S); return (result); }
MATRIX mat_randperm(int m, int n, MATRIX result) { int i, j; MATRIX tmp = NULL; if(result==NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) { tmp = mat_randperm_n(n, tmp); for(j=0; j<n; ++j) result[i][j] = tmp[0][j]; } mat_free(tmp); return result; }
MATRIX mat_tran(MATRIX A, MATRIX result) { int i, j, m, n; m = MatCol(A); n = MatRow(A); if(result== NULL) if((result = mat_creat(m,n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) for (j=0; j<n; ++j) { result[i][j] = A[j][i]; } return result; }
MATRIX mat_copy(MATRIX A, MATRIX result) { int i, j, m, n; m = MatCol(A); n = MatRow(A); if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); #pragma omp parallel for private(j) for(i=0; i<n; ++i) for(j=0; j<m; ++j) { result[i][j] = A[i][j]; } return result; }
MATRIX mat_randfun(int n, int m, mtype (*fun)(mtype), mtype xmin, mtype xmax, MATRIX result) { int i, j; if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); if(!MAT_SET_SEED)mat_set_seed(0); for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = __mat_randfun(fun, xmin, xmax); } } return result; }
MATSTACK __matstack_creat(int len) { MATSTACK matrixstack; int i; if((matrixstack = (MATRIX *)malloc(sizeof(MATRIX)*(1+len)))==NULL) return (matstack_error( MATSTACK_MALLOC)); for(i=0; i<=len; ++i) { matrixstack[i]= NULL; } matrixstack[0] = mat_creat(1,1,0); matrixstack[0][0][0] = len; return (matrixstack+1); }
MATRIX mat_rand(int n, int m, MATRIX result) { int i, j; if(result==NULL) if((result = mat_creat(n, m, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); if(!MAT_SET_SEED)mat_set_seed(0); for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = ((mtype)rand())/((mtype)RAND_MAX+1.0); } } return result; }
MATRIX mat_xcopy(MATRIX A, int si, int ei, int sj, int ej, MATRIX result) { int i, j, m, n; m = MatCol(A); n = MatRow(A); if(si<0 || sj<0 || ei>n || ei>m) mat_error(MAT_SIZEMISMATCH); if(result== NULL) if((result = mat_creat(ei-si, ej-sj, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); #pragma omp parallel for private(j) for(i=si; i<ei; ++i) for(j=sj; j<ej; ++j) { result[i-si][j-sj] = A[i][j]; } return result; }
//--------------------------------------------------------------------------- // Compute inverse matrix NaMatrix& NaMatrix::inv (NaMatrix& mInv) const { #ifdef __matrix_h if(dim_rows() != dim_cols()){ throw(na_size_mismatch); } MATRIX mat = mat_creat(dim_rows(), dim_cols(), UNDEFINED); unsigned i, j, n = dim_rows(); for(i = 0; i < n; ++i){ for(j = 0; j < n; ++j){ mat[i][j] = get(i, j); } } MATRIX minv = mat_inv(mat); if(NULL == minv){ throw(na_bad_value); } mat_free(mat); mInv.new_dim(n, n); for(i = 0; i < n; ++i){ for(j = 0; j < n; ++j){ mInv.fetch(i, j) = minv[i][j]; } } mat_free(minv); #if 0 trans(mInv); NaReal vDet = det(); if(0 == vDet) throw(na_bad_value); else mInv.multiply(1/vDet); #endif return mInv; #else throw(na_not_implemented); #endif /* __matrix_h */ }
MATRIX mat_div_dot(MATRIX A, MATRIX B, MATRIX result) { int i, j, m, n, o, p; m = MatCol(A); n = MatRow(A); o = MatCol(B); p = MatRow(B); if(result==NULL) if((result = mat_creat(MatRow(A), MatCol(A), UNDEFINED))==NULL) return mat_error(MAT_MALLOC); if(o==m &&p==n) { #pragma omp parallel for private(j) for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = A[i][j]/B[i][j]; } } } else if(o==1 && p!=1) { #pragma omp parallel for private(j) for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = A[i][j]/B[i][0]; } } } else if(p==1 && o!=1) { #pragma omp parallel for private(j) for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { result[i][j] = A[i][j]/B[0][j]; } } } else gen_error(GEN_SIZEMISMATCH); return result; }
MATRIX mat_randperm_n(int n, MATRIX result) { int i, j; mtype t = 0.0; if(result==NULL) if((result = mat_creat(1, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); if(!MAT_SET_SEED) mat_set_seed(0); for(i=0; i<n; ++i) result[0][i] = i; for(i=0; i<n; ++i) { j = rand()%(n-i)+i; t = result[0][j]; result[0][j] =result[0][i]; result[0][i] = t; } return result; }
MATRIX mat_calc_dist_sq(MATRIX A, MATRIX d, MATRIX result) { int i, j, m, n; mtype dist; m = MatRow(A); n = MatCol(A); if(result==NULL) if((result = mat_creat(1, n, ZERO_MATRIX))==NULL) return mat_error(MAT_MALLOC);; for(i=0; i<n; ++i) { dist = 0.0; for(j=0; j<m; ++j) { dist += (A[j][i]-d[j][0])*(A[j][i]-d[j][0]); } result[0][i] = dist; } return result; }
/* *----------------------------------------------------------------------------- * funct: mat_lsolve_durbin * desct: Solve simultaneous linear eqns using * Levinson-Durbin algorithm * * This function solve the linear eqns Ax = B: * * | v0 v1 v2 .. vn-1 | | a1 | | v1 | * | v1 v0 v1 .. vn-2 | | a2 | | v2 | * | v2 v1 v0 .. vn-3 | | a3 | = | .. | * | ... | | .. | | .. | * | vn-1 vn-2 .. .. v0 | | an | | vn | * * domain: where A is a symmetric Toeplitz matrix and B * in the above format (related to A) * * given: A, B * retrn: x (of Ax = B) * *----------------------------------------------------------------------------- */ MATRIX mat_lsolve_durbin(MATRIX A,MATRIX B,MATRIX X) { MATRIX R; int i, n; n = MatRow(A); if ( ( R = mat_creat(n+1, 1, UNDEFINED) ) == NULL ) return (NULL); for (i=0; i<n; i++) { R[i][0] = A[i][0]; } R[n][0] = B[n-1][0]; mat_durbin( R, X ); mat_free( R ); return(X); }
MATRIX mat_scpcol(MATRIX data) { int i, j1, j2, m, n; MATRIX spco = NULL; m = MatCol(data); n = MatRow(data); spco = mat_creat(m, m, ZERO_MATRIX); for(j1=0; j1<m; ++j1) { for(j2=j1; j2<m; ++j2) { spco[j1][j2] = 0.0; for(i=0; i<n; ++i) { spco[j1][j2] += data[i][j1]*data[i][j2]; } spco[j2][j1] = spco[j1][j2]; } } return spco; }
/*-----------------------------------------------------------------------------*/ static MATRIX buildTVMatrix(MATRIX U1V, MATRIX U2V){ MATRIX T, T3V; int i; normalizeVector(U2V); T3V = vectorCrossProduct(U1V,U2V); normalizeVector(T3V); if(T3V == NULL){ return NULL; } T = mat_creat(3,3,ZERO_MATRIX); if(T == NULL){ killVector(T3V); return NULL; } for(i = 0; i < 3; i++){ T[i][0] = U1V[i][0]; T[i][1] = U2V[i][0]; T[i][2] = T3V[i][0]; } killVector(T3V); return T; }
/*--------------------------------------------------------------------*/ MATRIX calcTasUBFromTwoReflections(lattice cell, tasReflection r1, tasReflection r2, int *errorCode){ MATRIX B, HT, UT, U, UB, HTT ; MATRIX u1, u2, h1, h2, planeNormal; double ud[3]; int status; *errorCode = 1; /* calculate the B matrix and the HT matrix */ B = mat_creat(3,3,ZERO_MATRIX); status = calculateBMatrix(cell,B); if(status < 0){ *errorCode = status; return NULL; } h1 = tasReflectionToHC(r1.qe,B); h2 = tasReflectionToHC(r2.qe,B); if(h1 == NULL || h2 == NULL){ *errorCode = UBNOMEMORY; mat_free(B); return NULL; } HT = matFromTwoVectors(h1,h2); if(HT == NULL){ *errorCode = UBNOMEMORY; mat_free(B); mat_free(h1); mat_free(h2); return NULL; } /* calculate U vectors and UT matrix */ u1 = calcTasUVectorFromAngles(r1); u2 = calcTasUVectorFromAngles(r2); if(u1 == NULL || u2 == NULL){ *errorCode = UBNOMEMORY; mat_free(B); mat_free(h1); mat_free(h2); return NULL; } UT = matFromTwoVectors(u1,u2); if(UT == NULL){ *errorCode = UBNOMEMORY; mat_free(B); mat_free(h1); mat_free(h2); mat_free(u1); mat_free(u2); mat_free(HT); return NULL; } /* UT = U * HT */ HTT = mat_tran(HT); if(HTT == NULL){ *errorCode = UBNOMEMORY; mat_free(B); mat_free(h1); mat_free(h2); mat_free(u1); mat_free(u2); mat_free(HT); return NULL; } U = mat_mul(UT,HTT); if(U == NULL){ *errorCode = UBNOMEMORY; mat_free(B); mat_free(h1); mat_free(h2); mat_free(u1); mat_free(u2); mat_free(HT); mat_free(HTT); return NULL; } UB = mat_mul(U,B); if(UB == NULL){ mat_free(B); mat_free(h1); mat_free(h2); mat_free(u1); mat_free(u2); mat_free(HT); mat_free(HTT); mat_free(U); *errorCode = UBNOMEMORY; } /* clean up */ killVector(h1); killVector(h2); mat_free(HT); mat_free(HTT); killVector(u1); killVector(u2); mat_free(UT); mat_free(U); mat_free(B); return UB; }
MATRIX ortho(MATRIX C, MATRIX C_ortho) { /* This function orthogonalizes a DCM by method presented in the paper Bar-Itzhack: "Orthogonalization Techniques of DCM" (1969, IEEE) Input: C: DCM, 3x3 Output: C_ortho: Orthogonalized DCM, 3x3 Programmer: Adhika Lie Created: May 10, 2011 Last Modified: May 10, 2011 */ MATRIX e = mat_creat(3,1,ONES_MATRIX); MATRIX w1 = mat_creat(3,1,ONES_MATRIX); MATRIX w1_p = mat_creat(3,1,ONES_MATRIX); MATRIX w1_n = mat_creat(3,1,ONES_MATRIX); MATRIX w2 = mat_creat(3,1,ONES_MATRIX); MATRIX w2_p = mat_creat(3,1,ONES_MATRIX); MATRIX w2_n = mat_creat(3,1,ONES_MATRIX); MATRIX w3 = mat_creat(3,1,ONES_MATRIX); MATRIX w3_p = mat_creat(3,1,ONES_MATRIX); MATRIX w3_n = mat_creat(3,1,ONES_MATRIX); double mag_w1, mag_w2, mag_w3; w1[0][0] = C[0][0]; w1[1][0] = C[1][0]; w1[2][0] = C[2][0]; mag_w1 = norm(w1); mag_w1 = 1.0/mag_w1; w1 = mat_scalMul(w1,mag_w1,w1); w2[0][0] = C[0][1]; w2[1][0] = C[1][1]; w2[2][0] = C[2][1]; mag_w2 = norm(w2); mag_w2 = 1.0/mag_w2; w2 = mat_scalMul(w2,mag_w2,w2); w3[0][0] = C[0][2]; w3[1][0] = C[1][2]; w3[2][0] = C[2][2]; mag_w3 = norm(w3); mag_w3 = 1.0/mag_w3; w3 = mat_scalMul(w3,mag_w3,w3); while (norm(e) > 1e-15){ w1_p = cross(w2,w3,w1_p); w2_p = cross(w3,w1,w2_p); w3_p = cross(w1,w2,w3_p); w1_n = mat_add(w1,w1_p,w1_n); w1_n = mat_scalMul(w1_n,0.5,w1_n); w1 = mat_scalMul(w1_n,1.0/norm(w1_n),w1); w2_n = mat_add(w2,w2_p,w2_n); w2_n = mat_scalMul(w2_n,0.5,w2_n); w2 = mat_scalMul(w2_n,1.0/norm(w2_n),w2); w3_n = mat_add(w3,w3_p,w3_n); w3_n = mat_scalMul(w3_n,0.5,w3_n); w3 = mat_scalMul(w3_n,1.0/norm(w3_n),w3); w1_p = cross(w2,w3,w1_p); w2_p = cross(w3,w1,w2_p); w3_p = cross(w1,w2,w3_p); w1_n = mat_sub(w1,w1_p,w1_n); e[0][0] = norm(w1_n); w2_n = mat_sub(w2,w2_p,w2_n); e[1][0] = norm(w2_n); w3_n = mat_sub(w3,w3_p,w3_n); e[2][0] = norm(w3_n); } C_ortho[0][0] = w1[0][0]; C_ortho[0][1] = w2[0][0]; C_ortho[0][2] = w3[0][0]; C_ortho[1][0] = w1[1][0]; C_ortho[1][1] = w2[1][0]; C_ortho[1][2] = w3[1][0]; C_ortho[2][0] = w1[2][0]; C_ortho[2][1] = w2[2][0]; C_ortho[2][2] = w3[2][0]; return C_ortho; }
MATRIX mat_bsxfun(MATRIX A, MATRIX B, mtype (*func)(mtype, mtype), MATRIX result) { int m, n, o, p, i, j; m = MatRow(A); n = MatCol(A); o = MatRow(B); p = MatCol(B); if(m<o && n==p && m==1) { if(result== NULL) if((result = mat_creat(o, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<o; ++i) { for(j=0; j<n; ++j) { result[i][j] = (*func)(A[0][j], B[i][j]); } } } else if(m>o && n==p && o==1) { if(result== NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) { for(j=0; j<n; ++j) { result[i][j] = (*func)(A[i][j], B[0][j]); } } } else if(m==o && n<p && n==1) { if(result== NULL) if((result = mat_creat(m, p, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) { for(j=0; j<p; ++j) { result[i][j] = (*func)(A[i][0], B[i][j]); } } } else if(m==o && n>p && p==1) { if(result== NULL) if((result = mat_creat(m, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) { for(j=0; j<n; ++j) { result[i][j] = (*func)(A[i][j], B[i][0]); } } } else if(m==1 && p==1) { if(result== NULL) if((result = mat_creat(o, n, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<o; ++i) { for(j=0; j<n; ++j) { result[i][j] = (*func)(A[0][j], B[i][0]); } } } else if(n==1 && o==1) { if(result== NULL) if((result = mat_creat(m, p, UNDEFINED))==NULL) return mat_error(MAT_MALLOC); for(i=0; i<m; ++i) { for(j=0; j<p; ++j) { result[i][j] = (*func)(A[i][0], B[0][j]); } } } else mat_error(MAT_SIZEMISMATCH); return result; }
extern void get_guidance(double time, struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr, struct mission *missionData_ptr){ if (time>0.05){ #ifdef AIRCRAFT_THOR controlData_ptr->ias_cmd = 17; //Trim airspeed (m/s) #endif #ifdef AIRCRAFT_TYR controlData_ptr->ias_cmd = 17; //Trim airspeed (m/s) #endif #ifdef AIRCRAFT_FASER controlData_ptr->ias_cmd = 23; #endif #ifdef AIRCRAFT_IBIS controlData_ptr->ias_cmd = 23; #endif #ifdef AIRCRAFT_BALDR controlData_ptr->ias_cmd = 23; #endif // Initialization of algorithm and variables if (guide_init==0) // init variables { pos_lla = mat_creat(3,1,ZERO_MATRIX); // LAT, LON, alt vector pos_ecef = mat_creat(3,1,ZERO_MATRIX); // ECEF position vector pos_ecef0 = mat_creat(3,1,ZERO_MATRIX); // ECEF position vector of initial point v_ned = mat_creat(3,1,ZERO_MATRIX); // vector of NED velocity pos_ned = mat_creat(3,1,ZERO_MATRIX); // vector of NED position tmp31 = mat_creat(3,1,ZERO_MATRIX); // temporary 3x1 vector T_ecef2ned = mat_creat(3,3,ZERO_MATRIX); // transformation matrix from ECEF to NED WP_goal = mat_creat(2,1,ZERO_MATRIX); // Vector of North / East goal coordinates (actaul waypoint) guide_init=1; // guidance initialization finished } if (guide_start==0) { pos_lla[0][0]=navData_ptr->lat; pos_lla[1][0]=navData_ptr->lon; pos_lla[2][0]=navData_ptr->alt; // transform starting point from LLA to ECEF LatLonAltToEcef2(pos_ecef0, pos_lla); // transformation matrix for LaunchPoint ECEF |--> NED for initial point T_ecef2ned[0][0] = -sin(pos_lla[0][0])*cos(pos_lla[1][0]); T_ecef2ned[0][1] = -sin(pos_lla[0][0])*sin(pos_lla[1][0]); T_ecef2ned[0][2] = cos(pos_lla[0][0]); T_ecef2ned[1][0] = -sin(pos_lla[1][0]); T_ecef2ned[1][1] = cos(pos_lla[1][0]); T_ecef2ned[1][2] = 0.0; T_ecef2ned[2][0] = -cos(pos_lla[0][0])*cos(pos_lla[1][0]); T_ecef2ned[2][1] = -cos(pos_lla[0][0])*sin(pos_lla[1][0]); T_ecef2ned[2][2] = -sin(pos_lla[0][0]); //save next waypoint coordinates WP_goal[0][0]=waypoints[nextwaypoint][0]; WP_goal[1][0]=waypoints[nextwaypoint][1]; guide_start=1; } else cpsi=0; //to handle SIL initial zero data // current vehicle position/vel (computed at every iteration) pos_lla[0][0]=navData_ptr->lat; pos_lla[1][0]=navData_ptr->lon; pos_lla[2][0]=navData_ptr->alt; v_ned[0][0] = navData_ptr->vn; v_ned[1][0] = navData_ptr->ve; v_ned[2][0] = navData_ptr->vd; // azimuth angle from ground speed psi=atan2(v_ned[1][0], v_ned[0][0]); // Transfrom the difference of actual coordinates from the starting point into NED frame LatLonAltToEcef2(pos_ecef, pos_lla); mat_sub(pos_ecef, pos_ecef0, tmp31); mat_mul(T_ecef2ned, tmp31, pos_ned); // Calculate waypoint distance from actual point d2WP=distance2waypoint(WP_goal, pos_ned); // change of target waypoint: check if vehicle is within x meters of the target waypoint if ((d2WP < WPTOL) && (guide_start==1)) { if(nextwaypoint == numofwaypoints-1) nextwaypoint = 0; // change back to first waypoint else nextwaypoint++; // select new waypoint parameters WP_goal[0][0] = waypoints[nextwaypoint][0]; WP_goal[1][0] = waypoints[nextwaypoint][1]; pinit=0; } // actual aircraft position xA=pos_ned[0][0]; // North yA=pos_ned[1][0]; // East // Target coordinates xT=WP_goal[0][0]; yT=WP_goal[1][0]; if (pinit==0) // decide about tracking method { if (sensorData_ptr->adData_ptr->ias_filt>10) // only for SIL because nonzero initial conditions { psiT=atan2(yT-yA, xT-xA); // Aircraft target azimuth angle // azimuth angle correction if required: if (fabs(psiT-wraparound(psi))>PI) psiT=psiT+mysign(wraparound(psi))*PI2; Rt=pow(controlData_ptr->ias_cmd, 2)/g/tan(PHI0); // possible turn radius // center of circle: dpsi=mysign(psiT-wraparound(psi))*90*D2R; xC=xA+Rt*cos(psi+dpsi); yC=yA+Rt*sin(psi+dpsi); // check feasibility R2=sqrt(pow(WP_goal[0][0]-xC, 2)+pow(WP_goal[1][0]-yC, 2)); // distance of waypoint from circle center if (R2<Rt+FTOL) // infeasible problem turn into opposite direction { cpsi=-dpsi*4; lc=8; pinit=1; } else // feasible problem, go to waypoint { cpsi=psiT-wraparound(psi); lc=0; pinit=1; } } // end if nonzero velocity } // end decide about tracking method if (lc==8) // check if the algorithm can change to circular segment tracking { // calculate actual circle parameters Rt=pow(controlData_ptr->ias_cmd, 2)/g/tan(PHI0); // turn radius // center of circle: psiT=atan2(yT-yA, xT-xA); // azimuth angle correction if required: if (fabs(psiT-wraparound(psi))>PI) psiT=psiT+mysign(wraparound(psi))*PI2; // center of circle: dpsi=mysign(psiT-wraparound(psi))*90*D2R; xC=xA+Rt*cos(psi+dpsi); yC=yA+Rt*sin(psi+dpsi); // check feasibility R2=sqrt(pow(WP_goal[0][0]-xC, 2)+pow(WP_goal[1][0]-yC, 2)); if (R2>=Rt+FTOL) // fasible problem, track circle { cpsi=psiT-wraparound(psi); lc=0; } else cpsi=-dpsi*4; } else if (lc==0) { psiT=atan2(yT-yA, xT-xA); // azimuth angle correction if required: if (fabs(psiT-wraparound(psi))>PI) psiT=psiT+mysign(wraparound(psi))*PI2; cpsi=psiT-wraparound(psi); } // send out control command controlData_ptr->psi_cmd=cpsi; // send out diagnostic variable controlData_ptr->r_cmd=((nextwaypoint+1)*10+lc); } }
// make sure C is 3 x 3 MATRIX mat_T321 (double pitch, double roll, double yaw, MATRIX C) { MATRIX TI2,T23,T3B,t1; double cphi,sphi,ctheta,stheta,cpsi,spsi; // if dimensions of C is wrong if ( MatCol(C) != 3 || MatRow(C) != 3 ) { printf("mat_T321 error: incompatible output matrix size\n"); _exit(-1); // if dimensions of C is correct } else { if ((TI2 = mat_creat( 3, 3, UNDEFINED )) == NULL) return (NULL); if ((T23 = mat_creat( 3, 3, UNDEFINED )) == NULL) return (NULL); if ((T3B = mat_creat( 3, 3, UNDEFINED )) == NULL) return (NULL); cphi = cos(roll); sphi = sin(roll); ctheta = cos(pitch); stheta = sin(pitch); cpsi = cos(yaw); spsi = sin(yaw); TI2[0][0] = spsi; TI2[0][1] = cpsi; TI2[0][2] = 0; TI2[1][0] = cpsi; TI2[1][1] = -spsi; TI2[1][2] = 0; TI2[2][0] = 0; TI2[2][1] = 0; TI2[2][2] = -1; T23[0][0] = ctheta; T23[0][1] = 0; T23[0][2] = -stheta; T23[1][0] = 0; T23[1][1] = 1; T23[1][2] = 0; T23[2][0] = stheta; T23[2][1] = 0; T23[2][2] = ctheta; T3B[0][0] = 1; T3B[0][1] = 0; T3B[0][2] = 0; T3B[1][0] = 0; T3B[1][1] = cphi; T3B[1][2] = sphi; T3B[2][0] = 0; T3B[2][1] = -sphi; T3B[2][2] = cphi; // C = T3B*T23*TI2; if ( ( t1 = mat_creat(3,3,UNDEFINED) ) == NULL ) return (NULL); mat_mul(T23,TI2,t1); mat_mul(T3B,t1, C); } mat_free(T3B); mat_free(T23); mat_free(TI2); mat_free(t1); return(C); }
void init_nav(struct imu *imuData_ptr, struct gps *gpsData_ptr, struct nav *navData_ptr){ /*++++++++++++++++++++++++++++++++++++++++++++++++ *matrix creation for navigation computation *++++++++++++++++++++++++++++++++++++++++++++++++*/ F = mat_creat(15,15,ZERO_MATRIX); // State Matrix PHI = mat_creat(15,15,ZERO_MATRIX); // State transition Matrix P = mat_creat(15,15,ZERO_MATRIX); // Covariance Matrix G = mat_creat(15,12,ZERO_MATRIX); // for Process Noise Transformation Rw = mat_creat(12,12,ZERO_MATRIX); Qw = mat_creat(15,15,ZERO_MATRIX); Q = mat_creat(15,15,ZERO_MATRIX); // Process Noise Matrix R = mat_creat(6,6,ZERO_MATRIX); // GPS Measurement Noise matrix x = mat_creat(15,1,ZERO_MATRIX); y = mat_creat(6,1,ZERO_MATRIX); // GPS Measurement eul = mat_creat(3,1,ZERO_MATRIX); Rbodtonav = mat_creat(3,3,ZERO_MATRIX); grav = mat_creat(3,1,ZERO_MATRIX); // Gravity Model f_b = mat_creat(3,1,ZERO_MATRIX); om_ib = mat_creat(3,1,ZERO_MATRIX); nr = mat_creat(3,1,ZERO_MATRIX); // Nav Transport Rate K = mat_creat(15,6,ZERO_MATRIX); // Kalman Gain H = mat_creat(6,15,ZERO_MATRIX); C_N2B = mat_creat(3,3,ZERO_MATRIX); // DCM C_B2N = mat_creat(3,3,ZERO_MATRIX); // DCM Transpose pos_ref = mat_creat(3,1,ZERO_MATRIX); pos_ins_ecef= mat_creat(3,1,ZERO_MATRIX); pos_ins_ned = mat_creat(3,1,ZERO_MATRIX); pos_gps = mat_creat(3,1,ZERO_MATRIX); pos_gps_ecef= mat_creat(3,1,ZERO_MATRIX); pos_gps_ned = mat_creat(3,1,ZERO_MATRIX); I15 = mat_creat(15,15,UNIT_MATRIX); // Identity I3 = mat_creat(3,3,UNIT_MATRIX); // Identity ImKH = mat_creat(15,15,ZERO_MATRIX); KRKt = mat_creat(15,15,ZERO_MATRIX); dx = mat_creat(3,1,ZERO_MATRIX); // Temporary to get dxdt a_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary b_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary temp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary atemp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary temp1515 = mat_creat(15,15,ZERO_MATRIX); temp615 = mat_creat(6,15,ZERO_MATRIX); temp66 = mat_creat(6,6,ZERO_MATRIX); atemp66 = mat_creat(6,6,ZERO_MATRIX); temp156 = mat_creat(15,6,ZERO_MATRIX); temp1512 = mat_creat(15,12,ZERO_MATRIX); // Assemble the matrices // .... gravity, g grav[2][0] = g; // ... H H[0][0] = 1.0; H[1][1] = 1.0; H[2][2] = 1.0; H[3][3] = 1.0; H[4][4] = 1.0; H[5][5] = 1.0; // ... Rw Rw[0][0] = SIG_W_AX*SIG_W_AX; Rw[1][1] = SIG_W_AY*SIG_W_AY; Rw[2][2] = SIG_W_AZ*SIG_W_AZ; Rw[3][3] = SIG_W_GX*SIG_W_GX; Rw[4][4] = SIG_W_GY*SIG_W_GY; Rw[5][5] = SIG_W_GZ*SIG_W_GZ; Rw[6][6] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[7][7] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[8][8] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[9][9] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[10][10] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[11][11] = 2*SIG_G_D*SIG_G_D/TAU_G; // ... P (initial) P[0][0] = P_P_INIT*P_P_INIT; P[1][1] = P_P_INIT*P_P_INIT; P[2][2] = P_P_INIT*P_P_INIT; P[3][3] = P_V_INIT*P_V_INIT; P[4][4] = P_V_INIT*P_V_INIT; P[5][5] = P_V_INIT*P_V_INIT; P[6][6] = P_A_INIT*P_A_INIT; P[7][7] = P_A_INIT*P_A_INIT; P[8][8] = P_HDG_INIT*P_HDG_INIT; P[9][9] = P_AB_INIT*P_AB_INIT; P[10][10] = P_AB_INIT*P_AB_INIT; P[11][11] = P_AB_INIT*P_AB_INIT; P[12][12] = P_GB_INIT*P_GB_INIT; P[13][13] = P_GB_INIT*P_GB_INIT; P[14][14] = P_GB_INIT*P_GB_INIT; // ... update P in get_nav navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2]; navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5]; navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8]; navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11]; navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14]; // ... R R[0][0] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[1][1] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[2][2] = SIG_GPS_P_D*SIG_GPS_P_D; R[3][3] = SIG_GPS_V*SIG_GPS_V; R[4][4] = SIG_GPS_V*SIG_GPS_V; R[5][5] = SIG_GPS_V*SIG_GPS_V; // .. then initialize states with GPS Data navData_ptr->lat = gpsData_ptr->lat*D2R; navData_ptr->lon = gpsData_ptr->lon*D2R; navData_ptr->alt = gpsData_ptr->alt; navData_ptr->vn = gpsData_ptr->vn; navData_ptr->ve = gpsData_ptr->ve; navData_ptr->vd = gpsData_ptr->vd; // ... and initialize states with IMU Data //navData_ptr->the = asin(imuData_ptr->ax/g); // theta from Ax, aircraft at rest //navData_ptr->phi = asin(-imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest navData_ptr->the = 8*D2R; navData_ptr->phi = 0*D2R; navData_ptr->psi = 90.0*D2R; eul2quat(navData_ptr->quat,(navData_ptr->phi),(navData_ptr->the),(navData_ptr->psi)); navData_ptr->ab[0] = 0.0; navData_ptr->ab[1] = 0.0; navData_ptr->ab[2] = 0.0; navData_ptr->gb[0] = imuData_ptr->p; navData_ptr->gb[1] = imuData_ptr->q; navData_ptr->gb[2] = imuData_ptr->r; // Specific forces and Rotation Rate f_b[0][0] = imuData_ptr->ax - navData_ptr->ab[0]; f_b[1][0] = imuData_ptr->ay - navData_ptr->ab[1]; f_b[2][0] = imuData_ptr->az - navData_ptr->ab[2]; om_ib[0][0] = imuData_ptr->p - navData_ptr->gb[0]; om_ib[1][0] = imuData_ptr->q - navData_ptr->gb[1]; om_ib[2][0] = imuData_ptr->r - navData_ptr->gb[2]; // Time during initialization tprev = imuData_ptr->time; navData_ptr->err_type = data_valid; //send_status("NAV filter initialized"); //fprintf(stderr,"NAV Data Initialization Completed\n"); }
//--------------------------------------------------------------------------- // Compute determinant of the matrix NaReal NaMatrix::det () const { #ifdef __matrix_h if(dim_rows() != dim_cols()){ throw(na_size_mismatch); } NaReal vDet = 0; MATRIX mat = mat_creat(dim_rows(), dim_cols(), UNDEFINED); unsigned i, j, n = dim_rows(); for(i = 0; i < n; ++i){ for(j = 0; j < n; ++j){ mat[i][j] = get(i, j); } } vDet = mat_det(mat); mat_free(mat); return vDet; #else throw(na_not_implemented); #endif/* __matrix_h */ #if 0 unsigned i, j, k, n = dim_rows(); // Predefined det() switch(n){ case 1: vDet = get(0, 0); break; case 2: vDet = get(0, 0) * get (1, 1) - get(0, 1) * get(1, 0); break; default:{ // Allocate space for minor matrix NaMatrix mOfMinor(n - 1, n - 1); for(i = 0; i < n; ++i){ // Compose minor for(j = 0; j < n - 1; ++j){ for(k = 0; k < n - 1; ++k){ if(k < i){ mOfMinor[j][k] = get(1 + j, k); }else if(k >= i){ mOfMinor[j][k] = get(1 + j, 1 + k); } } }// minor composition //NaPrintLog("Minor(%d,%d):\n", i, n - 1); //mOfMinor.print_contents(); // Summarize determinant if(i % 2){ vDet -= get(0, i) * mOfMinor.det(); }else{ vDet += get(0, i) * mOfMinor.det(); } } }break; }// end of switch for different cases //NaPrintLog("det=%g\n", vDet); return vDet; #endif }
MATSTACK mat_pca(MATRIX data, int pca_type) { int i, j, k, k2, m, n; MATRIX evals, im; MATSTACK tmmps0 = NULL; MATRIX symmat, symmat2; m = MatCol(data); n = MatRow(data); switch(pca_type) { case MAT_PCA_CORRELATION: tmmps0 = mat_corcol(data); symmat = tmmps0[1]; break; case MAT_PCA_COVARIANCE: tmmps0 = mat_covcol(data); symmat = tmmps0[1]; break; case MAT_PCA_SUMOFSQUARES: symmat = mat_scpcol(data); break; default: tmmps0 = mat_covcol(data); symmat = tmmps0[1]; break; } evals = mat_creat(m, 1, UNDEFINED); im = mat_creat(m, 1, UNDEFINED); symmat2 = mat_copy(symmat, NULL); mat_tred2(symmat, evals, im); mat_tqli(evals, im, symmat); for(i=0; i<n; ++i) { for(j=0; j<m; ++j) { im[j][0] = tmmps0[2][i][j]; } for(k=0; k<3; ++k) { tmmps0[2][i][k] = 0.0; for(k2=0; k2<m; ++k2) { tmmps0[2][i][k] += im[k2][0] * symmat[k2][m-k-1]; } } } for(j=0; j<m; ++j) { for(k=0; k<m; ++k) { im[k][0] = symmat2[j][k]; } for(i=0; i<3; ++i) { symmat2[j][i] = 0.0; for (k2=0; k2<m; ++k2) { symmat2[j][i] += im[k2][0] * symmat[k2][m-i-1]; } if(evals[m-i-1][0]>0.0005) symmat2[j][i] /= (mtype)sqrt(evals[m-i-1][0]); else symmat2[j][i] = 0.0; } } mat_free(evals); mat_free(im); return tmmps0; }
void EcefToLatLonAlt(MATRIX vector) { int i; double x, y, z, q, p, sinlat, sinlat2; double a, b, d, radius, lat, alt, dummy; double E_WGS84, E2_WGS84, ONE_MIN_E2, A_WGS84; MATRIX lla; lla = mat_creat(3,1,ZERO_MATRIX); E_WGS84 = ECCENTRICITY; /* Earth ellipse ecc - unitless */ E2_WGS84 = E_WGS84*E_WGS84; /* Earth's ellipse ecc^2 - unitless */ ONE_MIN_E2 = 1.0 - E2_WGS84; A_WGS84 = EARTH_RADIUS; /* Earth's ellipse semi-major axis - meters */ x = vector[0][0]; y = vector[1][0]; z = vector[2][0]; lla[1][0] = atan2(y, x); /* Longitude */ p = sqrt((x * x) + (y * y)); /* Latitude and Altitude */ if (p < 0.1) { p = 0.1; } q = z / p; alt = 0.0; lat = atan(q * (1.0 / ONE_MIN_E2)); a = 1.0; i = 0; while ((a > 0.2) && (i < 20)) { sinlat = sin(lat); sinlat2 = sinlat * sinlat; dummy =sqrt((1.0 - (E2_WGS84 * sinlat2))*(1.0 - (E2_WGS84 * sinlat2))); radius = A_WGS84 / sqrt(dummy); d = alt; alt = (p / cos(lat)) - radius; a = q * (radius + alt); b = (ONE_MIN_E2 * radius) + alt; lat = atan2(a, b); a = sqrt((alt - d)*(alt - d)); i = i + 1; } lla[0][0] = lat; lla[2][0] = alt; for (i = 0; i < 3; i++) { vector[i][0] = lla[i][0]; } mat_free(lla); }
void init_nav(struct sensordata *sensorData_ptr, struct nav *navData_ptr, struct control *controlData_ptr){ /*++++++++++++++++++++++++++++++++++++++++++++++++ *matrix creation for navigation computation *++++++++++++++++++++++++++++++++++++++++++++++++*/ F = mat_creat(15,15,ZERO_MATRIX); // State Matrix PHI = mat_creat(15,15,ZERO_MATRIX); // State transition Matrix P = mat_creat(15,15,ZERO_MATRIX); // Covariance Matrix G = mat_creat(15,12,ZERO_MATRIX); // for Process Noise Transformation Rw = mat_creat(12,12,ZERO_MATRIX); Qw = mat_creat(15,15,ZERO_MATRIX); Q = mat_creat(15,15,ZERO_MATRIX); // Process Noise Matrix R = mat_creat(6,6,ZERO_MATRIX); // GPS Measurement Noise matrix x = mat_creat(15,1,ZERO_MATRIX); y = mat_creat(6,1,ZERO_MATRIX); // GPS Measurement eul = mat_creat(3,1,ZERO_MATRIX); Rbodtonav = mat_creat(3,3,ZERO_MATRIX); grav = mat_creat(3,1,ZERO_MATRIX); // Gravity Model f_b = mat_creat(3,1,ZERO_MATRIX); om_ib = mat_creat(3,1,ZERO_MATRIX); nr = mat_creat(3,1,ZERO_MATRIX); // Nav Transport Rate K = mat_creat(15,6,ZERO_MATRIX); // Kalman Gain H = mat_creat(6,15,ZERO_MATRIX); C_N2B = mat_creat(3,3,ZERO_MATRIX); // DCM C_B2N = mat_creat(3,3,ZERO_MATRIX); // DCM Transpose pos_ref = mat_creat(3,1,ZERO_MATRIX); pos_ins_ecef= mat_creat(3,1,ZERO_MATRIX); pos_ins_ned = mat_creat(3,1,ZERO_MATRIX); pos_gps = mat_creat(3,1,ZERO_MATRIX); pos_gps_ecef= mat_creat(3,1,ZERO_MATRIX); pos_gps_ned = mat_creat(3,1,ZERO_MATRIX); I15 = mat_creat(15,15,UNIT_MATRIX); // Identity I3 = mat_creat(3,3,UNIT_MATRIX); // Identity ImKH = mat_creat(15,15,ZERO_MATRIX); KRKt = mat_creat(15,15,ZERO_MATRIX); dx = mat_creat(3,1,ZERO_MATRIX); // Temporary to get dxdt a_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary b_temp31 = mat_creat(3,1,ZERO_MATRIX); // Temporary temp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary atemp33 = mat_creat(3,3,ZERO_MATRIX); // Temporary temp1515 = mat_creat(15,15,ZERO_MATRIX); temp615 = mat_creat(6,15,ZERO_MATRIX); temp66 = mat_creat(6,6,ZERO_MATRIX); atemp66 = mat_creat(6,6,ZERO_MATRIX); temp156 = mat_creat(15,6,ZERO_MATRIX); temp1512 = mat_creat(15,12,ZERO_MATRIX); // Assemble the matrices // .... gravity, g grav[2][0] = g; // ... H H[0][0] = 1.0; H[1][1] = 1.0; H[2][2] = 1.0; H[3][3] = 1.0; H[4][4] = 1.0; H[5][5] = 1.0; // first order correlation + white noise, tau = time constant for correlation // gain on white noise plus gain on correlation // Rw small - trust time update, Rw more - lean on measurement update // split between accels and gyros and / or noise and correlation // ... Rw Rw[0][0] = SIG_W_AX*SIG_W_AX; Rw[1][1] = SIG_W_AY*SIG_W_AY; Rw[2][2] = SIG_W_AZ*SIG_W_AZ; //1 sigma on noise Rw[3][3] = SIG_W_GX*SIG_W_GX; Rw[4][4] = SIG_W_GY*SIG_W_GY; Rw[5][5] = SIG_W_GZ*SIG_W_GZ; Rw[6][6] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[7][7] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[8][8] = 2*SIG_A_D*SIG_A_D/TAU_A; Rw[9][9] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[10][10] = 2*SIG_G_D*SIG_G_D/TAU_G; Rw[11][11] = 2*SIG_G_D*SIG_G_D/TAU_G; // ... P (initial) P[0][0] = P_P_INIT*P_P_INIT; P[1][1] = P_P_INIT*P_P_INIT; P[2][2] = P_P_INIT*P_P_INIT; P[3][3] = P_V_INIT*P_V_INIT; P[4][4] = P_V_INIT*P_V_INIT; P[5][5] = P_V_INIT*P_V_INIT; P[6][6] = P_A_INIT*P_A_INIT; P[7][7] = P_A_INIT*P_A_INIT; P[8][8] = P_HDG_INIT*P_HDG_INIT; P[9][9] = P_AB_INIT*P_AB_INIT; P[10][10] = P_AB_INIT*P_AB_INIT; P[11][11] = P_AB_INIT*P_AB_INIT; P[12][12] = P_GB_INIT*P_GB_INIT; P[13][13] = P_GB_INIT*P_GB_INIT; P[14][14] = P_GB_INIT*P_GB_INIT; // ... update P in get_nav navData_ptr->Pp[0] = P[0][0]; navData_ptr->Pp[1] = P[1][1]; navData_ptr->Pp[2] = P[2][2]; navData_ptr->Pv[0] = P[3][3]; navData_ptr->Pv[1] = P[4][4]; navData_ptr->Pv[2] = P[5][5]; navData_ptr->Pa[0] = P[6][6]; navData_ptr->Pa[1] = P[7][7]; navData_ptr->Pa[2] = P[8][8]; navData_ptr->Pab[0] = P[9][9]; navData_ptr->Pab[1] = P[10][10]; navData_ptr->Pab[2] = P[11][11]; navData_ptr->Pgb[0] = P[12][12]; navData_ptr->Pgb[1] = P[13][13]; navData_ptr->Pgb[2] = P[14][14]; // ... R R[0][0] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[1][1] = SIG_GPS_P_NE*SIG_GPS_P_NE; R[2][2] = SIG_GPS_P_D*SIG_GPS_P_D; R[3][3] = SIG_GPS_V*SIG_GPS_V; R[4][4] = SIG_GPS_V*SIG_GPS_V; R[5][5] = SIG_GPS_V*SIG_GPS_V; // .. then initialize states with GPS Data navData_ptr->lat = sensorData_ptr->gpsData_ptr->lat*D2R; navData_ptr->lon = sensorData_ptr->gpsData_ptr->lon*D2R; navData_ptr->alt = sensorData_ptr->gpsData_ptr->alt; navData_ptr->vn = sensorData_ptr->gpsData_ptr->vn; navData_ptr->ve = sensorData_ptr->gpsData_ptr->ve; navData_ptr->vd = sensorData_ptr->gpsData_ptr->vd; //navData_ptr->the = 0;//asin(sensorData_ptr->imuData_ptr->ax/g); // theta from Ax, aircraft at rest //navData_ptr->phi = 0;//asin(-sensorData_ptr->imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest //navData_ptr->psi = 0.0; // ... and initialize states with IMU Data navData_ptr->the = asin(sensorData_ptr->imuData_ptr->ax/g); // theta from Ax, aircraft at rest navData_ptr->phi = asin(-sensorData_ptr->imuData_ptr->ay/(g*cos(navData_ptr->the))); // phi from Ay, aircraft at rest if((sensorData_ptr->imuData_ptr->hy) > 0){ navData_ptr->psi = 90*D2R - atan(sensorData_ptr->imuData_ptr->hx / sensorData_ptr->imuData_ptr->hy); } else if((sensorData_ptr->imuData_ptr->hy) < 0){ navData_ptr->psi = 270*D2R - atan(sensorData_ptr->imuData_ptr->hx / sensorData_ptr->imuData_ptr->hy) - 360*D2R; } else if((sensorData_ptr->imuData_ptr->hy) == 0){ if((sensorData_ptr->imuData_ptr->hx) < 0){ navData_ptr->psi = 180*D2R; } else{ navData_ptr->psi = 0.0; } } eul2quat(navData_ptr->quat,(navData_ptr->phi),(navData_ptr->the),(navData_ptr->psi)); navData_ptr->ab[0] = 0.0; navData_ptr->ab[1] = 0.0; navData_ptr->ab[2] = 0.0; navData_ptr->gb[0] = sensorData_ptr->imuData_ptr->p; navData_ptr->gb[1] = sensorData_ptr->imuData_ptr->q; navData_ptr->gb[2] = sensorData_ptr->imuData_ptr->r; // Specific forces and Rotation Rate f_b[0][0] = sensorData_ptr->imuData_ptr->ax - navData_ptr->ab[0]; f_b[1][0] = sensorData_ptr->imuData_ptr->ay - navData_ptr->ab[1]; f_b[2][0] = sensorData_ptr->imuData_ptr->az - navData_ptr->ab[2]; om_ib[0][0] = sensorData_ptr->imuData_ptr->p - navData_ptr->gb[0]; om_ib[1][0] = sensorData_ptr->imuData_ptr->q - navData_ptr->gb[1]; om_ib[2][0] = sensorData_ptr->imuData_ptr->r - navData_ptr->gb[2]; // Time during initialization tprev = sensorData_ptr->imuData_ptr->time; navData_ptr->err_type = data_valid; send_status("NAV filter initialized"); //fprintf(stderr,"NAV Data Initialization Completed\n"); }
/* *----------------------------------------------------------------------------- * funct: mat_inv * desct: find inverse of a matrix * given: a = square matrix a, and C, return for inv(a) * retrn: square matrix Inverse(A), C * NULL = fails, singular matrix *----------------------------------------------------------------------------- */ MATRIX mat_inv( MATRIX a , MATRIX C) { MATRIX A, B, P; int i, n; #ifdef CONFORM_CHECK if(MatCol(a)!=MatCol(C)) { error("\nUnconformable matrices in routine mat_inv(): Col(A)!=Col(B)\n"); } if(MatRow(a)!=MatRow(C)) { error("\nUnconformable matrices in routine mat_inv(): Row(A)!=Row(B)\n"); } #endif n = MatCol(a); A = mat_creat(MatRow(a), MatCol(a), UNDEFINED); A = mat_copy(a, A); B = mat_creat( n, 1, UNDEFINED ); P = mat_creat( n, 1, UNDEFINED ); /* * - LU-decomposition - * also check for singular matrix */ if (mat_lu(A, P) == -1) { mat_free(A); mat_free(B); mat_free(P); return (NULL); } else { /* Bug??? was still mat_backsubs1 even when singular??? */ for (i=0; i<n; i++) { mat_fill(B, ZERO_MATRIX); B[i][0] = 1.0; mat_backsubs1( A, B, C, P, i ); } mat_free(A); mat_free(B); mat_free(P); return (C); } }