/* * Eat all of the lines in the input file, attempting to categorize * them by their various flavors */ void eaterrors(int *r_errorc, Eptr **r_errorv) { Errorclass errorclass = C_SYNC; char *line; const char *inbuffer; size_t inbuflen; for (;;) { if ((inbuffer = fgetln(errorfile, &inbuflen)) == NULL) break; line = Calloc(inbuflen + 1, sizeof(char)); memcpy(line, inbuffer, inbuflen); line[inbuflen] = '\0'; wordvbuild(line, &cur_wordc, &cur_wordv); /* * for convenience, convert cur_wordv to be 1 based, instead * of 0 based. */ cur_wordv -= 1; if (cur_wordc > 0 && ((( errorclass = onelong() ) != C_UNKNOWN) || (( errorclass = cpp() ) != C_UNKNOWN) || (( errorclass = gcc45ccom() ) != C_UNKNOWN) || (( errorclass = pccccom() ) != C_UNKNOWN) || (( errorclass = richieccom() ) != C_UNKNOWN) || (( errorclass = lint0() ) != C_UNKNOWN) || (( errorclass = lint1() ) != C_UNKNOWN) || (( errorclass = lint2() ) != C_UNKNOWN) || (( errorclass = lint3() ) != C_UNKNOWN) || (( errorclass = make() ) != C_UNKNOWN) || (( errorclass = f77() ) != C_UNKNOWN) || ((errorclass = pi() ) != C_UNKNOWN) || (( errorclass = ri() )!= C_UNKNOWN) || (( errorclass = mod2() )!= C_UNKNOWN) || (( errorclass = troff() )!= C_UNKNOWN)) ) ; else errorclass = catchall(); if (cur_wordc) erroradd(cur_wordc, cur_wordv+1, errorclass, C_UNKNOWN); } #ifdef FULLDEBUG printf("%d errorentrys\n", nerrors); #endif arrayify(r_errorc, r_errorv, er_head); }
void eaterrors(int *r_errorc, Eptr **r_errorv) { Errorclass errorclass = C_SYNC; for (;;) { if (fgets(inbuffer, BUFSIZ, errorfile) == NULL) break; wordvbuild(inbuffer, &wordc, &wordv); /* * for convenience, convert wordv to be 1 based, instead * of 0 based. */ wordv -= 1; /* * check for sunf77 errors has to be done before * pccccom to be able to distingush between the two */ if ((wordc > 0) && (((errorclass = onelong()) != C_UNKNOWN) || ((errorclass = cpp()) != C_UNKNOWN) || ((errorclass = sunf77()) != C_UNKNOWN) || ((errorclass = pccccom()) != C_UNKNOWN) || ((errorclass = richieccom()) != C_UNKNOWN) || ((errorclass = lint0()) != C_UNKNOWN) || ((errorclass = lint1()) != C_UNKNOWN) || ((errorclass = lint2()) != C_UNKNOWN) || ((errorclass = lint3()) != C_UNKNOWN) || ((errorclass = make()) != C_UNKNOWN) || ((errorclass = f77()) != C_UNKNOWN) || ((errorclass = pi()) != C_UNKNOWN) || ((errorclass = ri()) != C_UNKNOWN) || ((errorclass = troff()) != C_UNKNOWN) || ((errorclass = mod2()) != C_UNKNOWN) || ((errorclass = troff()) != C_UNKNOWN))) { /* EMPTY */ } else { errorclass = catchall(); } if (wordc) erroradd(wordc, wordv+1, errorclass, C_UNKNOWN); } #ifdef FULLDEBUG printf("%d errorentrys\n", nerrors); #endif arrayify(r_errorc, r_errorv, er_head); }
void Ukf(VEC *omega, VEC *mag_vec, VEC *mag_vec_I, VEC *sun_vec, VEC *sun_vec_I, VEC *Torq_ext, double t, double h, int eclipse, VEC *state, VEC *st_error, VEC *residual, int *P_flag, double sim_time) { static VEC *omega_prev = VNULL, *mag_vec_prev = VNULL, *sun_vec_prev = VNULL, *q_s_c = VNULL, *x_prev = VNULL, *Torq_prev, *x_m_o; static MAT *Q = {MNULL}, *R = {MNULL}, *Pprev = {MNULL}; static double alpha, kappa, lambda, sqrt_lambda, w_m_0, w_c_0, w_i, beta; static int n_states, n_sig_pts, n_err_states, iter_num, initialize=0; VEC *x = VNULL, *x_priori = VNULL, *x_err_priori = VNULL, *single_sig_pt = VNULL, *v_temp = VNULL, *q_err_quat = VNULL, *err_vec = VNULL, *v_temp2 = VNULL, *x_ang_vel = VNULL, *meas = VNULL, *meas_priori = VNULL, *v_temp3 = VNULL, *x_posteriori_err = VNULL, *x_b_m = VNULL, *x_b_g = VNULL; MAT *sqrt_P = {MNULL}, *P = {MNULL}, *P_priori = {MNULL}, *sig_pt = {MNULL}, *sig_vec_mat = {MNULL}, *err_sig_pt_mat = {MNULL}, *result = {MNULL}, *result_larger = {MNULL}, *result1 = {MNULL}, *Meas_err_mat = {MNULL}, *P_zz = {MNULL}, *iP_vv = {MNULL}, *P_xz = {MNULL}, *K = {MNULL}, *result2 = {MNULL}, *result3 = {MNULL}, *C = {MNULL}; int update_mag_vec, update_sun_vec, update_omega, i, j; double d_res; if (inertia == MNULL) { inertia = m_get(3,3); m_ident(inertia); inertia->me[0][0] = 0.007; inertia->me[1][1] = 0.014; inertia->me[2][2] = 0.015; } if (initialize == 0){ iter_num = 1; n_states = (7+6); n_err_states = (6+6); n_sig_pts = 2*n_err_states+1; alpha = sqrt(3); kappa = 3 - n_states; lambda = alpha*alpha * (n_err_states+kappa) - n_err_states; beta = -(1-(alpha*alpha)); w_m_0 = (lambda)/(n_err_states + lambda); w_c_0 = (lambda/(n_err_states + lambda)) + (1 - (alpha*alpha) + beta); w_i = 0.5/(n_err_states +lambda); initialize = 1; sqrt_lambda = (lambda+n_err_states); if(q_s_c == VNULL) { q_s_c = v_get(4); q_s_c->ve[0] = -0.020656; q_s_c->ve[1] = 0.71468; q_s_c->ve[2] = -0.007319; q_s_c->ve[3] = 0.6991; } if(Torq_prev == VNULL) { Torq_prev = v_get(3); v_zero(Torq_prev); } quat_normalize(q_s_c); } result = m_get(9,9); m_zero(result); result1 = m_get(n_err_states, n_err_states); m_zero(result1); if(x_m_o == VNULL) { x_m_o = v_get(n_states); v_zero(x_m_o); } x = v_get(n_states); v_zero(x); x_err_priori = v_get(n_err_states); v_zero(x_err_priori); x_ang_vel = v_get(3); v_zero(x_ang_vel); sig_pt = m_get(n_states, n_err_states); m_zero(sig_pt); if (C == MNULL) { C = m_get(9, 12); m_zero(C); } if (P_priori == MNULL) { P_priori = m_get(n_err_states, n_err_states); m_zero(P_priori); } if (Q == MNULL) { Q = m_get(n_err_states, n_err_states); m_ident(Q); // Q->me[0][0] = 0.0001; Q->me[1][1] = 0.0001; Q->me[2][2] = 0.0001; Q->me[3][3] = 0.0001; Q->me[4][4] = 0.0001; Q->me[5][5] = 0.0001; Q->me[6][6] = 0.000001; Q->me[7][7] = 0.000001; Q->me[8][8] = 0.000001; Q->me[9][9] = 0.000001; Q->me[10][10] = 0.000001; Q->me[11][11] = 0.000001; } if( Pprev == MNULL) { Pprev = m_get(n_err_states, n_err_states); m_ident(Pprev); Pprev->me[0][0] = 1e-3; Pprev->me[1][1] = 1e-3; Pprev->me[2][2] = 1e-3; Pprev->me[3][3] = 1e-3; Pprev->me[4][4] = 1e-3; Pprev->me[5][5] = 1e-3; Pprev->me[6][6] = 1e-4; Pprev->me[7][7] = 1e-4; Pprev->me[8][8] = 1e-4; Pprev->me[9][9] = 1e-3; Pprev->me[10][10] = 1e-3; Pprev->me[11][11] = 1e-3; } if (R == MNULL) { R = m_get(9,9); m_ident(R); R->me[0][0] = 0.034; R->me[1][1] = 0.034; R->me[2][2] = 0.034; R->me[3][3] = 0.00027; R->me[4][4] = 0.00027; R->me[5][5] = 0.00027; R->me[6][6] = 0.000012; R->me[7][7] = 0.000012; R->me[8][8] = 0.000012; } if(eclipse==0) { R->me[0][0] = 0.00034; R->me[1][1] = 0.00034; R->me[2][2] = 0.00034; R->me[3][3] = 0.00027; R->me[4][4] = 0.00027; R->me[5][5] = 0.00027; R->me[6][6] = 0.0000012; R->me[7][7] = 0.0000012; R->me[8][8] = 0.0000012; Q->me[0][0] = 0.00001; Q->me[1][1] = 0.00001; Q->me[2][2] = 0.00001; Q->me[3][3] = 0.0001;//0.000012;//0.0175;//1e-3; Q->me[4][4] = 0.0001;//0.0175;//1e-3; Q->me[5][5] = 0.0001;//0.0175;//1e-3; Q->me[6][6] = 0.0000000001;//1e-6; Q->me[7][7] = 0.0000000001; Q->me[8][8] = 0.0000000001; Q->me[9][9] = 0.0000000001; Q->me[10][10] = 0.0000000001; Q->me[11][11] = 0.0000000001; } else { R->me[0][0] = 0.34; R->me[1][1] = 0.34; R->me[2][2] = 0.34; R->me[3][3] = 0.0027; R->me[4][4] = 0.0027; R->me[5][5] = 0.0027; R->me[6][6] = 0.0000012; R->me[7][7] = 0.0000012; R->me[8][8] = 0.0000012; Q->me[0][0] = 0.00001; Q->me[1][1] = 0.00001; Q->me[2][2] = 0.00001; Q->me[3][3] = 0.0001; Q->me[4][4] = 0.0001; Q->me[5][5] = 0.0001; Q->me[6][6] = 0.0000000001; Q->me[7][7] = 0.0000000001; Q->me[8][8] = 0.0000000001; Q->me[9][9] = 0.0000000001; Q->me[10][10] = 0.0000000001; Q->me[11][11] = 0.0000000001; } if(omega_prev == VNULL) { omega_prev = v_get(3); v_zero(omega_prev); } if(mag_vec_prev == VNULL) { mag_vec_prev = v_get(3); v_zero(mag_vec_prev); } if(sun_vec_prev == VNULL) { sun_vec_prev = v_get(3); v_zero(sun_vec_prev); } if (err_sig_pt_mat == MNULL) { err_sig_pt_mat = m_get(n_err_states, n_sig_pts); m_zero(err_sig_pt_mat); } if(q_err_quat == VNULL) { q_err_quat = v_get(4); // q_err_quat = v_resize(q_err_quat,4); v_zero(q_err_quat); } if(err_vec == VNULL) { err_vec = v_get(3); v_zero(err_vec); } v_temp = v_get(9); v_resize(v_temp,3); if(x_prev == VNULL) { x_prev = v_get(n_states); v_zero(x_prev); x_prev->ve[3] = 1; quat_mul(x_prev,q_s_c,x_prev); x_prev->ve[4] = 0.0; x_prev->ve[5] = 0.0; x_prev->ve[6] = 0.0; x_prev->ve[7] = 0.0; x_prev->ve[8] = 0.0; x_prev->ve[9] = 0.0; x_prev->ve[10] = 0.0; x_prev->ve[11] = 0.0; x_prev->ve[12] = 0.0; } sqrt_P = m_get(n_err_states, n_err_states); m_zero(sqrt_P); //result = m_resize(result, n_err_states, n_err_states); result_larger = m_get(n_err_states, n_err_states); int n, m; for(n = 0; n < result->n; n++) { for(m = 0; m < result->m; m++) { result_larger->me[m][n] = result->me[m][n]; } } //v_resize(v_temp, n_err_states); V_FREE(v_temp); v_temp = v_get(n_err_states); symmeig(Pprev, result_larger, v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_copy(Pprev, result1); sm_mlt(sqrt_lambda, result1, result_larger); catchall(CHfactor(result_larger), printerr(sim_time)); for(i=0; i<n_err_states; i++){ for(j=i+1; j<n_err_states; j++){ result_larger->me[i][j] = 0; } } expandstate(result_larger, x_prev, sig_pt); sig_vec_mat = m_get(n_states, n_sig_pts); m_zero(sig_vec_mat); for(j = 0; j<(n_err_states+1); j++) { for(i = 0; i<n_states; i++) { if(j==0) { sig_vec_mat->me[i][j] = x_prev->ve[i]; } else if(j>0) { sig_vec_mat->me[i][j] = sig_pt->me[i][j-1]; } } } sm_mlt(-1,result_larger,result_larger); expandstate(result_larger, x_prev, sig_pt); for(j = (n_err_states+1); j<n_sig_pts; j++) { for(i = 0; i<n_states; i++) { sig_vec_mat->me[i][j] = sig_pt->me[i][j-(n_err_states+1)]; } } single_sig_pt = v_get(n_states); quat_rot_vec(q_s_c, Torq_ext); for(j=0; j<(n_sig_pts); j++) { //v_temp = v_resize(v_temp,n_states); V_FREE(v_temp); v_temp = v_get(n_states); get_col(sig_vec_mat, j, single_sig_pt); v_copy(single_sig_pt, v_temp); rk4(t, v_temp, h, Torq_prev); set_col(sig_vec_mat, j, v_temp); } v_copy(Torq_ext, Torq_prev); x_priori = v_get(n_states); v_zero(x_priori); v_resize(v_temp,n_states); v_zero(v_temp); for(j=0; j<n_sig_pts; j++) { get_col( sig_vec_mat, j, v_temp); if(j == 0) { v_mltadd(x_priori, v_temp, w_m_0, x_priori); } else { v_mltadd(x_priori, v_temp, w_i, x_priori); } } v_copy(x_priori, v_temp); v_resize(v_temp,4); quat_normalize(v_temp);//zaroori hai ye for(i=0; i<4; i++) { x_priori->ve[i] = v_temp->ve[i]; } v_resize(v_temp, n_states); v_copy(x_priori, v_temp); v_resize(v_temp, 4); quat_inv(v_temp, v_temp); for(i=0; i<3; i++) { x_ang_vel->ve[i] = x_priori->ve[i+4]; } x_b_m = v_get(3); v_zero(x_b_m); x_b_g = v_get(3); v_zero(x_b_g); /////////////////////////check it!!!!!!!! checked... doesnt change much the estimate for(i=0; i<3; i++) { x_b_m->ve[i] = x_priori->ve[i+7]; x_b_g->ve[i] = x_priori->ve[i+10]; } v_temp2 = v_get(n_states); v_zero(v_temp2); for(j=0; j<n_sig_pts; j++) { v_resize(v_temp2, n_states); get_col( sig_vec_mat, j, v_temp2); for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+4]; } v_resize(v_temp2, 4); quat_mul(v_temp2, v_temp, q_err_quat); v_resize(q_err_quat, n_err_states); v_sub(err_vec, x_ang_vel, err_vec); for(i=3; i<6; i++) { q_err_quat->ve[i] = err_vec->ve[i-3]; } for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+7]; } v_sub(err_vec, x_b_m, err_vec); for(i=6; i<9; i++) { q_err_quat->ve[i] = err_vec->ve[i-6]; } for(i=0; i<3; i++) { err_vec->ve[i] = v_temp2->ve[i+10]; } v_sub(err_vec, x_b_g, err_vec); for(i=9; i<12; i++) { q_err_quat->ve[i] = err_vec->ve[i-9]; } set_col(err_sig_pt_mat, j, q_err_quat); if(j==0){ v_mltadd(x_err_priori, q_err_quat, w_m_0, x_err_priori); } else{ v_mltadd(x_err_priori, q_err_quat, w_i, x_err_priori); } } v_resize(v_temp,n_err_states); for (j=0;j<13;j++) { get_col(err_sig_pt_mat, j, v_temp); v_sub(v_temp, x_err_priori, v_temp); get_dyad(v_temp, v_temp, result_larger); if(j==0){ sm_mlt(w_c_0, result_larger, result_larger); } else{ sm_mlt(w_i, result_larger, result_larger); } m_add(P_priori, result_larger, P_priori); } symmeig(P_priori, result_larger, v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_add(P_priori, Q, P_priori); v_resize(v_temp,3); meas = v_get(9); if (!(is_vec_equal(sun_vec, sun_vec_prev)) /*&& (eclipse==0)*/ ){ update_sun_vec =1; v_copy(sun_vec, sun_vec_prev); v_copy(sun_vec, v_temp); normalize_vec(v_temp); quat_rot_vec(q_s_c, v_temp); normalize_vec(v_temp); for(i = 0; i<3;i++){ meas->ve[i] = v_temp->ve[i]; } } else{ update_sun_vec =0; for(i = 0; i<3;i++){ meas->ve[i] = 0; } } if (!(is_vec_equal(mag_vec, mag_vec_prev)) ){ update_mag_vec =1; v_copy(mag_vec, mag_vec_prev); v_copy(mag_vec, v_temp); normalize_vec(v_temp); quat_rot_vec(q_s_c, v_temp); normalize_vec(v_temp); for(i=3; i<6; i++){ meas->ve[i] = v_temp->ve[i-3]; } } else{ update_mag_vec =0; for(i=3; i<6; i++){ meas->ve[i] = 0;//mag_vec_prev->ve[i-3]; } } if (!(is_vec_equal(omega, omega_prev) ) ){ update_omega =1; v_copy(omega, omega_prev); v_copy(omega, v_temp); quat_rot_vec(q_s_c, v_temp); for(i=6; i<9; i++){ meas->ve[i] = v_temp->ve[i-6]; } } else{ update_omega =0; for(i=6; i<9; i++){ meas->ve[i] = 0; } } v_resize(v_temp, 9); v_resize(v_temp2, n_states); v_temp3 = v_get(3); Meas_err_mat = m_get(9, n_sig_pts); m_zero(Meas_err_mat); meas_priori = v_get(9); v_zero(meas_priori); for(j=0; j<n_sig_pts; j++) { get_col( sig_vec_mat, j, v_temp2); if(update_omega){ for(i=6;i<9;i++){ v_temp->ve[i] = v_temp2->ve[i-2] + x_b_g->ve[i-6]; } } else{ for(i=6;i<9;i++){ v_temp->ve[i] = 0; } } v_resize(v_temp2, 4); if(update_sun_vec){ for(i=0;i<3;i++){ v_temp3->ve[i] = sun_vec_I->ve[i]; } quat_rot_vec(v_temp2, v_temp3); normalize_vec(v_temp3); for(i=0;i<3;i++){ v_temp->ve[i] = v_temp3->ve[i]; } } else{ for(i=0;i<3;i++){ v_temp->ve[i] = 0; } } if(update_mag_vec){ for(i=0;i<3;i++){ v_temp3->ve[i] = mag_vec_I->ve[i]; } normalize_vec(v_temp3); for(i=0;i<3;i++){ v_temp3->ve[i] = v_temp3->ve[i] + x_b_m->ve[i]; } quat_rot_vec(v_temp2, v_temp3); normalize_vec(v_temp3); for(i=3;i<6;i++){ v_temp->ve[i] = v_temp3->ve[i-3]; } } else{ for(i=3;i<6;i++){ v_temp->ve[i] = 0; } } set_col(Meas_err_mat, j, v_temp); if(j==0){ v_mltadd(meas_priori, v_temp, w_m_0, meas_priori); } else{ v_mltadd(meas_priori, v_temp, w_i, meas_priori); } } v_resize(v_temp, 9); m_resize(result_larger, 9, 9); m_zero(result_larger); P_zz = m_get(9, 9); m_zero(P_zz); iP_vv = m_get(9, 9); m_zero(iP_vv); P_xz = m_get(n_err_states, 9); m_zero(P_xz); v_resize(v_temp2, n_err_states); result1 = m_resize(result1,n_err_states,9); for (j=0; j<n_sig_pts; j++) { get_col( Meas_err_mat, j, v_temp); get_col( err_sig_pt_mat, j, v_temp2); v_sub(v_temp, meas_priori, v_temp); get_dyad(v_temp, v_temp, result_larger); get_dyad(v_temp2, v_temp, result1); if(j==0){ sm_mlt(w_c_0, result_larger, result_larger); sm_mlt(w_c_0, result1, result1); } else{ sm_mlt(w_i, result_larger, result_larger); sm_mlt(w_i, result1, result1); } m_add(P_zz, result_larger, P_zz); m_add(P_xz, result1, P_xz); } symmeig(P_zz, result_larger, v_temp); i = 0; for (j=0; j<9; j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } m_add(P_zz, R, P_zz); m_inverse(P_zz, iP_vv); K = m_get(n_err_states, 9); m_zero(K); m_mlt(P_xz, iP_vv, K); if(x_posteriori_err == VNULL) { x_posteriori_err = v_get(n_err_states); v_zero(x_posteriori_err); } v_resize(v_temp,9); v_sub(meas, meas_priori, v_temp); v_copy(v_temp, residual); mv_mlt(K, v_temp, x_posteriori_err); v_resize(v_temp2,3); for(i=0;i<3;i++){ v_temp2->ve[i] = x_posteriori_err->ve[i]; } for(i=4; i<n_states; i++){ x_prev->ve[i] = (x_posteriori_err->ve[i-1] + x_priori->ve[i]); } d_res = v_norm2(v_temp2); v_resize(v_temp2,4); if(d_res<=1 /*&& d_res!=0*/){ v_temp2->ve[0] = v_temp2->ve[0]; v_temp2->ve[1] = v_temp2->ve[1]; v_temp2->ve[2] = v_temp2->ve[2]; v_temp2->ve[3] = sqrt(1-d_res); } else//baad main daala hai { v_temp2->ve[0] = (v_temp2->ve[0])/(sqrt(1+d_res)); v_temp2->ve[1] = (v_temp2->ve[1])/(sqrt(1+d_res)); v_temp2->ve[2] = (v_temp2->ve[2])/(sqrt(1+d_res)); v_temp2->ve[3] = 1/sqrt(1 + d_res); } v_resize(x_posteriori_err, n_states); for(i=(n_states-1); i>3; i--){ x_posteriori_err->ve[i] = x_posteriori_err->ve[i-1]; } for(i=0; i<4; i++){ x_posteriori_err->ve[i] = v_temp2->ve[i]; } quat_mul(v_temp2, x_priori, v_temp2); for(i=0;i<4;i++){ x_prev->ve[i] = v_temp2->ve[i]; } m_resize(result_larger, n_err_states, 9); m_mlt(K, P_zz, result_larger); result2 = m_get(9, n_err_states); m_transp(K,result2); m_resize(result1, n_err_states, n_err_states); m_mlt(result_larger, result2, result1); v_resize(v_temp, n_err_states); m_sub(P_priori, result1, Pprev); symmeig(Pprev, result1 , v_temp); i = 0; for (j=0;j<n_err_states;j++){ if(v_temp->ve[j]>=0); else{ i = 1; } } v_copy(x_prev, v_temp); v_resize(v_temp,4); v_copy(x_prev, v_temp2); v_resize(v_temp2,4); v_copy(x_prev, x_m_o); //v_resize(x_m_o, 4); v_resize(v_temp,3); quat_inv(q_s_c, v_temp2); v_copy( x_prev, state); quat_mul(state, v_temp2, state); for(i=0; i<3; i++){ v_temp->ve[i] = state->ve[i+4]; } quat_rot_vec(v_temp2, v_temp); for(i=0; i<3; i++){ state->ve[i+4] = v_temp->ve[i]; } v_copy( x_posteriori_err, st_error); iter_num++; V_FREE(x); V_FREE(x_priori); V_FREE(x_err_priori); V_FREE(single_sig_pt); V_FREE(v_temp); V_FREE(q_err_quat); V_FREE(err_vec); V_FREE(v_temp2); V_FREE(x_ang_vel); V_FREE(meas); V_FREE(meas_priori); V_FREE(v_temp3); V_FREE(x_posteriori_err); V_FREE(x_b_m); V_FREE(x_b_g); M_FREE(sqrt_P); M_FREE(P); M_FREE(P_priori); M_FREE(sig_pt); M_FREE(sig_vec_mat); M_FREE(err_sig_pt_mat); M_FREE(result); M_FREE(result_larger); M_FREE(result1); M_FREE(Meas_err_mat); M_FREE(P_zz); M_FREE(iP_vv); M_FREE(P_xz); M_FREE(K); M_FREE(result2); M_FREE(result3); }
void test(const int n, const int lb, const int ub, bool dumpfull) { // Generate a special-band-matrix mfull With lb lower diagonals, ub upper // diagonals and the elements of the highest upper diagonal extended across // each row MAT *mfull = m_get(n, n); //m_rand(mfull); randlist(mfull->base, (mfull->n)*(mfull->n)); double **me = mfull->me; for(int i = 0; i < n; i++) { for(int j = 0; j < i - lb; j++) me[i][j] = 0.0; for(int j = i+ub+1; j < n; j++) me[i][j] = me[i][i+ub]; } // Copy matrix mfull to a compactly stored version // mcmpct // First lb columns padding for later use // Next lb columns for lower diagonals // Next column for diagonal // Next ub columns for upper diagonals // as the highest upper diagonal of the same row const int mm = 2*lb+ub+1; double mcmpct[n][mm]; zero(&mcmpct[0][0], n*mm); for(int i = 0; i < n; i++) for(int j = MAX(i-lb, 0); j < MIN(i+ub+1, n); j++) mcmpct[i][j-i+lb+lb] = me[i][j]; // Replace unused values with NAN to be sure they aren't used for(int k = 0; k < n; k++) for(int i = 0; i < lb; i++) mcmpct[k][i] = NAN; for(int k = 0; k < lb; k++) for(int i = 0; i < lb-k; i++) mcmpct[k][i+lb] = NAN; for(int k=n-1; k >= n-ub; k--) for(int i = n-1-k+1+lb; i < mm; i++) mcmpct[k][i+lb] = NAN; // Generate start vector x1 for test VEC *x1 = v_get(n); randlist(x1->ve, n); // Calculate mfull*x1 = dfull VEC *dfull = v_get(n); mv_mlt(mfull, x1, dfull); // Calculate mcmpct*x1 = dcmpct double dcmpct[n]; bdspecLUmlt(&mcmpct[0][0], n, lb, ub, x1->ve, dcmpct); if(dumpfull) { printf("Vector x (random values)\n"); printf("========================\n"); v_out(x1->ve, n); printf("Matrix A (random values)\n"); printf("========================\n"); printf("Full NxN Meschach Matrix\n"); m_out(mfull->base, n, n); printf("Compact bdspec Array\n"); m_out(&mcmpct[0][0], n, mm); printf("Vector d = A*x\n"); printf("==============\n"); printf("Calculated from Full Meschach Matrix:\n"); v_out(dfull->ve, n); printf("Calculated from Compact bdspec Array:\n"); v_out(dcmpct, n); printf("L2 norm of difference between Meschach and bdspec calculations of d\n"); } double ddiff = v_diff(dfull->ve, dcmpct, n); printf("d diff=%6.0E ", ddiff); if(ddiff*ddiff > DBL_EPSILON*v_normsq(dfull->ve, n)) printf("FAIL,"); else printf("PASS,"); PERM *p = px_get(n); LUfactor(mfull, p); int indx[n]; bdspecLUfactormeschscale(&mcmpct[0][0], n, lb, ub, indx); VEC *yfull = v_get(n); catchall(LUsolve(mfull, p, dfull, yfull), printf("--matrix singular--:\n")); double ycmpct[n]; for(int i = 0; i < n; i++) ycmpct[i] = dcmpct[i]; bdspecLUsolve(&mcmpct[0][0], n, lb, ub, indx, ycmpct); if(dumpfull) { printf("\n\n"); printf("LU Factorization\n"); printf("================\n"); printf("Meschach LU Array\n"); m_out(mfull->base, n, n); printf("Compact bdspec LU Array\n"); m_out(&mcmpct[0][0], n, mm); printf("Permutation\n"); printf("===========\n"); printf("Meschach permutation vector\n"); p_out((int *) p->pe, n); printf("bdspec indx vector\n"); p_out(indx, n); printf("A*y = d Solved for y\n"); printf("====================\n"); printf("Meschach result\n"); v_out(yfull->ve, n); printf("bdspec result\n"); v_out(ycmpct, n); printf("L2 norm of difference between Meschach and bdspec calculations of y:\n"); } double ydiff = v_diff(yfull->ve, ycmpct, n); printf("y diff=%6.0E ", ydiff); if(ydiff*ydiff > DBL_EPSILON*v_normsq(yfull->ve, n)) printf("FAIL,"); else printf("PASS,"); if(dumpfull) { printf("\n\n"); printf("L2 norm of error = y-x\n"); printf("======================\n"); } double x1normsq = v_normsq(x1->ve, n); double mescherr = v_diff(yfull->ve, x1->ve, n); printf("mesch err=%6.0E ", mescherr); if(mescherr*mescherr > DBL_EPSILON*x1normsq) printf("FAIL,"); else printf("PASS,"); double bdspecerr = v_diff(ycmpct, x1->ve, n); printf("bdspec err=%6.0E ", bdspecerr); if(bdspecerr*bdspecerr > DBL_EPSILON*x1normsq) printf("FAIL "); else printf("PASS "); if(dumpfull) { printf("\n\n"); } fflush(stdout); }