/* .dll creation when using MATLAB cd e:\matlabjv\lbl\lbl_goats2000\code_goats2000 mex ... lbl_goats2000_v3.c ... lbl_init.c ... matrice.c ... lsq_fix.c ... kalman.c */ /*#define REAL_DATA_POST_PROCESSING*/ /* Defined in the Makefile */ /*#define ODYSSEY */ /*commented out to do post processing by hjt and rob*/ /*#define MATLAB_SIM*/ /*#define DEBUG_INIT*/ /*#define DEBUG_FILTER*/ #ifdef MATLAB_SIM #include "mex.h" #endif #include #include #ifdef REAL_DATA_POST_PROCESSING double mission_time; #endif #include #include #include #include "lbl_init.h" #include "matrice.h" #include "lbl_array.h" #include "lsq_fix.h" #include "kalman.h" #define MAX_REJECT 6 #define MAHALA_THRESH 3 matrice X,P,F,J,JJ,K,H,Cin,Q,Sinv,R,INNOV,D,est,RZ; matrice JCinJt,FPFt,Ft,FP,Jt,JCin,FPFtpJCinJt; matrice covar; matrice KH,ImKH,ImKHP,I; matrice HPHt,Ht,HP; matrice PHt; matrice KK,XpKinnov; matrice Dt,DCinDt,S,HS,HSt,DCin; matrice PHtpSSinv; matrice St,HPpSt,KHPpSt,PmKHPpSt; matrice Y,W,Wt,temp,dy,res,Ninv,WtNinv,dyt,dytNinv; matrice a,x,y,noise; matrice Covariance,Hessien,Derivee,essai,da,sol; static int filter_initialized=0,count_init_cycles=0; static lbl_array lbl; static double dr_dn,dr_de; static int count_reject[MAX_BEACON_NUMBER]; void lbl_filter(int,double,double,double,double,double,double,double*,double*,int*); void init_filter(int,double,double,double*,double*,double*); void pos_back_propagation(int,double,double*,double*,double*,double[],double*); double dist3(double,double,double,double,double,double); void dead_reckoning(double,double,double,double); /*------------------------------------------------------------------------*/ void lbl_filter(int ping_flag,double time,double dt,double pitch,double yaw, double depth,double speed,double *tof,double *good_tof,int *lost) { int i,j; static int index=0,first=1; double Cp,Sp,Cy,Sy,dusp,avhead,pred_tof,d_mahala; double st,a1,b1,c1,a2,b2,c2,den1,den2; double pc[14],rc[14]; double vcn,vce,du,dpsi,epsilon; double x_ping,y_ping,z_ping,x_recept,y_recept,z_recept,tJ; if (first) { first = 0; return; } /*--- reset index of array 'est' and JJ when vehicle pings and store current time */ if (ping_flag) { #ifdef DEBUG_FILTER syslog(LOG_ERR,"t=%.2lf : PING !! ...\n",lbl.ping_time); #endif index = 0; for (i=0; i<=2; i++) for (j=0; j<=2; j++) e(JJ,i,j) = 0.0; } /*--- store dead reckoning data */ e(est,0,index) = yaw; e(est,1,index) = pitch; e(est,2,index) = speed; e(est,3,index) = dt; e(est,4,index) = time; /*--- get current estimates of the underwater current, speed bias and heading bias */ /*--- (they won't be modified by the prediction step) */ vcn = e(X,3,0); vce = e(X,4,0); du = e(X,5,0); dpsi = e(X,6,0); /*--- actual vehicle speed and heading */ dusp = speed-du; avhead = yaw-dpsi; /*--- sine and cosine of present pitch and yaw (including heading bias) */ Cy = cos(avhead); Sy = sin(avhead); Cp = cos(pitch); Sp = sin(pitch); /*--- state prediction */ e(X,0,0) += (Cp*Cy*dusp+vcn)*dt; e(X,1,0) += (Cp*Sy*dusp+vce)*dt; e(X,2,0) -= Sp*dusp*dt; /*--- matrix F */ e(F,0,3) = dt; e(F,1,4) = dt; e(F,0,5) =-Cp*Cy*dt; e(F,1,5) =-Cp*Sy*dt; e(F,2,5) = Sp*dt; e(F,0,6) = Cp*Sy*dusp*dt; e(F,1,6) =-Cp*Cy*dusp*dt; /*--- Jacobian of state wrt inputs psi,theta,speed */ e(J,0,0) =-e(F,0,6); e(J,0,1) =-Sp*Cy*dusp*dt; e(J,0,2) =-e(F,0,5); e(J,1,0) =-e(F,1,6); e(J,1,1) =-Sp*Sy*dusp*dt; e(J,1,2) =-e(F,1,5); e(J,2,0) = 0.0; e(J,2,1) =-Cp*dusp*dt; e(J,2,2) =-e(F,2,5); /*--- add J to JJ */ for (i=0; i<=2; i++) for (j=0; j<=2; j++) e(JJ,i,j) += e(J,i,j); /*--- compute P=F*P*Ft+J*Cin*Jt+Q */ state_covar_prediction(); /*--- estimation phase */ if (lbl.newdata) { /*--- init back propagated position at the ping */ x_ping = e(X,0,0); y_ping = e(X,1,0); z_ping = e(X,2,0); /*--- compute back propagated position at the ping */ pos_back_propagation(index,lbl.ping_time,&x_ping,&y_ping,&z_ping,pc,&epsilon); #ifdef DEBUG_FILTER syslog(LOG_ERR,"Back propagated position at ping : x_ping=%.2lf y_ping=%.2lf z_ping=%.2lf\n",x_ping,y_ping,z_ping); #endif /*--- compute time elapsed since ping (+ prediction) */ st = time-lbl.ping_time; /*--- reconstruct position at the reception of beacon #i and correct predicted state */ for (i=1; i<=lbl.number_of_beacon; i++) { if (tof[i] > 0.0) { #ifdef DEBUG_FILTER printf("PROCESSING BEACON %d .... \n",i); #endif /*--- determine position at reception of beacon #i */ tJ = lbl.ping_time+tof[i]; x_recept = e(X,0,0); y_recept = e(X,1,0); z_recept = e(X,2,0); pos_back_propagation(index,tJ,&x_recept,&y_recept,&z_recept,rc,&epsilon); #ifdef DEBUG_FILTER printf("beacon %d received at t=%lf\n",i,tJ); printf("back propagated position at reception of beacon %d : x_recept=%.2lf y_recept=%.2lf z_recept=%.2lf\n",i,x_recept,y_recept,z_recept); #endif a1 = lbl.nb[i]-x_ping; b1 = lbl.eb[i]-y_ping; c1 = lbl.db[i]-z_ping; a2 = lbl.nb[i]-x_recept; b2 = lbl.eb[i]-y_recept; c2 = lbl.db[i]-z_recept; den1 = sqrt(a1*a1+b1*b1+c1*c1); den2 = sqrt(a2*a2+b2*b2+c2*c2); /*--- vector D */ e(D,0,0) = (a1*pc[6]+b1*pc[7])/den1+(a2*rc[6]+b2*rc[7])/den2; e(D,0,1) = (a1*pc[8]+b1*pc[9]+c1*pc[10])/den1+(a2*rc[8]+b2*rc[9]+c2*rc[10])/den2; e(D,0,2) = (a1*pc[11]+b1*pc[12]+c1*pc[13])/den1+(a2*rc[11]+b2*rc[12]+c2*rc[13])/den2; mat_scal_prod_S(D,1.0/lbl.sos); /*--- predicted time of flight */ pred_tof = (den1+den2)/lbl.sos+lbl.turn_around_time[i]; #ifdef DEBUG_FILTER printf("pred_tof[%d]=%lf tof[%d]=%lf epsilon=%lf\n",i,pred_tof,i,tof[i],epsilon); #endif /*--- vector H */ e(H,0,0) =-(a1/den1+a2/den2); e(H,0,1) =-(b1/den1+b2/den2); e(H,0,2) =-(c1/den1+c2/den2); e(H,0,3) = (a1*st/den1+a2*epsilon/den2); e(H,0,4) = (b1*st/den1+b2*epsilon/den2); e(H,0,5) = ((a1*pc[1]+b1*pc[2]+c1*pc[3])/den1+(a2*rc[1]+b2*rc[2]+c2*rc[3])/den2); e(H,0,6) = ((a1*pc[4]+b1*pc[5])/den1+(a2*rc[4]+b2*rc[5])/den2); mat_scal_prod_S(H,1.0/lbl.sos); /*--- innovation */ e(INNOV,0,0) = tof[i]-pred_tof; denom_input_in_obs(); d_mahala = mahalanobis_distance(); if (d_mahala<0.0) syslog(LOG_ERR,"NEGATIVE d_mahala !!!!!!!!!!\n"); /*--- tof validation */ if (d_mahala < MAHALA_THRESH) { kalman_gain_input_in_obs(); state_correction(); state_covar_input_in_obs(); /*--- save valid tof and reset rejection counter */ *(good_tof+i) = tof[i]; count_reject[i] = 0; } else { count_reject[i]++; syslog(LOG_ERR,"%d ",i); /*--- detect filter divergence */ if (count_reject[i] >= MAX_REJECT) { syslog(LOG_ERR,"WARNING ! %d tof rejected on beacon %d : RE-INIT !!\n",count_reject[i],i); *lost = 1; } } } } } /*--- use depth as an observation */ for (i=0; i= NB_DATA_EST) index = NB_DATA_EST-1; } /*-----------------------------------------------------* | Function init_filter() | *-----------------------------------------------------*/ void init_filter(int ping_flag,double time,double depth,double *tof,double *nfix,double *efix) { static double dnb[MAX_BEACON_NUMBER],deb[MAX_BEACON_NUMBER]; int i,j,nb_nonzero_tofs,new_fix; double north_init,east_init,residual,tt; static double memo_dr[4][NB_DATA_EST]; static int idr; int kk; double min_res,resi; int ibad; *nfix = 0.0; *efix = 0.0; new_fix = 0; /*--- detect vehicle ping to count init cycles and reset dead reckoning since ping */ if (ping_flag) { #ifdef DEBUG_INIT syslog(LOG_ERR,"lbl_goats: init_filter - PING ! at t=%lf\n",lbl.ping_time); #endif count_init_cycles++; idr = 0; } #ifdef DEBUG_INIT syslog(LOG_ERR,"lbl_goats: init_filter - %d calls since the last PING.\n",idr); #endif /*--- keep track of dead reckoned position increments */ memo_dr[1][idr] = dr_dn; memo_dr[2][idr] = dr_de; memo_dr[3][idr] = time; /*--- make sure idr doesn't get too large */ idr++; if (idr >= NB_DATA_EST) { idr = 0; syslog(LOG_ERR," Init_filter: ERROR: idr = NB_DATA_EST.\n"); } /*--- when new LBL data available, do computations */ if (lbl.newdata) { #ifdef DEBUG_INIT syslog(LOG_ERR,"NEWDATA at t=%lf : sr1=%lf sr2=%lf sr3=%lf sr4=%lf\n", time,lbl.sr[1],lbl.sr[2],lbl.sr[3],lbl.sr[4]); #endif /*--- count how many tofs are available */ nb_nonzero_tofs = 0; for (i=1; i<=lbl.number_of_beacon; i++) { if (lbl.sr[i] != 0.0) { nb_nonzero_tofs++; /*--- running fix : DR motion between ping and reception of beacon i */ tt = 2.0/lbl.sos*lbl.sr[i]+lbl.turn_around_time[i]; #ifdef DEBUG_INIT syslog(LOG_ERR," init_filter:\n"); syslog(LOG_ERR," i = %d memo_dr[3][0] = %.2f; tt = %.2f\n", i, memo_dr[3][0], tt); syslog(LOG_ERR," memo_dr[3][0] + tt = %.2f\n", memo_dr[3][0]+tt); #endif kk = 0; dnb[i] = deb[i] = 0.0; while ((memo_dr[3][kk] < memo_dr[3][0]+tt) && (kk < idr) ) { dnb[i] += memo_dr[1][kk]; deb[i] += memo_dr[2][kk]; #ifdef DEBUG_INIT syslog(LOG_ERR," memo_dr[3][%d] = %.2f\n", kk, memo_dr[3][kk]); #endif kk++; } dnb[i] -= (memo_dr[3][kk]-(memo_dr[3][0]+tt))/ (memo_dr[3][kk]-memo_dr[3][kk-1])*memo_dr[1][kk-1]; deb[i] -= (memo_dr[3][kk]-(memo_dr[3][0]+tt))/ (memo_dr[3][kk]-memo_dr[3][kk-1])*memo_dr[2][kk-1]; dnb[i] /= 2.0; deb[i] /= 2.0; #ifdef DEBUG_INIT syslog(LOG_ERR,"kk = %d dnb[%d]=%10.5e deb[%d]=%10.5e\n", kk,i,dnb[i],i,deb[i]); #endif } else { lbl.sr[i] = 0.0; dnb[i] = deb[i] = 0.0; } } /* for */ /*--- only 3-beacon fixes are used during the init */ if (nb_nonzero_tofs < 3) { #ifdef DEBUG_INIT syslog(LOG_ERR,"nb_nonzero_tofs = %d => not enough returns\n", nb_nonzero_tofs); #endif } /*--- there are at least 3 ranges available */ else { while (nb_nonzero_tofs >= 3) { lsq_nbeacon(lbl.north_init,lbl.east_init,depth,&lbl,dnb,deb, &north_init,&east_init,&residual,covar); #ifdef DEBUG_INIT syslog(LOG_ERR,"filter_init: residual = %f\n", residual); #endif /*--- remove range with worst residual */ if (residual > RESIDUAL_THRESH) { min_res = 0.0; for (i=1; i<=lbl.number_of_beacon; i++) { if (lbl.sr[i] != 0.0) { resi = fabs(dist3(north_init,east_init,depth,lbl.nb[i], lbl.eb[i],lbl.db[i])-lbl.sr[i]); if (resi > min_res) { min_res = resi; ibad = i; } } } nb_nonzero_tofs--; #ifdef DEBUG_INIT syslog(LOG_ERR,"ibad=%d\n",ibad); #endif lbl.sr[ibad] = 0.0; if (nb_nonzero_tofs < 3) syslog(LOG_ERR,"init_filter: No New Fix\n"); } /*--- the fix is good */ else { new_fix = 1; syslog(LOG_ERR,"init_filter: Compute a New Fix\n"); syslog(LOG_ERR,"(%d)-beacon LBL fix: n=%.2lf e=%.2lf residual=%lf\n", nb_nonzero_tofs,north_init,east_init,residual); break; } } if (new_fix) { /*--- running fix : DR motion between ping and current time */ kk = 0; dnb[0] = 0.0; deb[0] = 0.0; for (kk = 0; kk < idr; kk ++) { dnb[0] += memo_dr[1][kk]; deb[0] += memo_dr[2][kk]; } #ifdef DEBUG_INIT syslog(LOG_ERR,"dnb[0]=%.2lf deb[0]=%.2lf\n",dnb[0],deb[0]); #endif north_init += dnb[0]; east_init += deb[0]; /*--- copy fix */ *nfix = north_init; *efix = east_init; if (!filter_initialized) { /*--- position is a weighted combination of dead-reck and LBL fix */ e(X,0,0) += lbl.fixgain*(north_init-e(X,0,0)-dr_dn); e(X,1,0) += lbl.fixgain*(east_init-e(X,1,0)-dr_de); /*--- if filter not initialized, check if current fix can be used */ #ifdef DEBUG_INIT syslog(LOG_ERR," Init_filter: init_with_residual = %d " " residual = %10.5e\n", init_with_residual(residual), residual); #endif if (init_with_residual(residual)) { filter_initialized = 1; for (i=0; i<=6; i++) { e(X,i,0) = 0.0; for (j=0; j<=6; j++) { e(P,i,j) = 0.0; } } e(X,0,0) = north_init; e(X,1,0) = east_init; e(X,2,0) = depth; e(P,0,0) = e(covar,0,0); e(P,0,1) = e(covar,0,1); e(P,1,0) = e(covar,1,0); e(P,1,1) = e(covar,1,1); e(P,2,2) = INIT_DEPTH_VAR; e(P,3,3) = e(P,4,4) = INIT_VC_VAR; e(P,5,5) = INIT_DU_VAR; e(P,6,6) = INIT_DPSI_VAR; for (i=1; i<=lbl.number_of_beacon; i++) { tof[i] = 0.0; count_reject[i] = 0; } #ifdef DEBUG_INIT syslog(LOG_ERR,"\nInitialization:\n"); syslog(LOG_ERR," n=%.2lf\te=%.2lf\n",north_init,east_init); syslog(LOG_ERR," %d LBL cycles required (%.1lf s)\n\n",count_init_cycles,time); #endif } } } } } } /*------------------------------------------------------------------------*/ void pos_back_propagation(int index,double tJ,double *x_back,double *y_back, double *z_back,double *coeff,double *epsilon) { int i,j; double vcn,vce,du,dpsi; double Cdpsi,Sdpsi,dusp,Cpitch,Spitch,dtps; *epsilon = 0.0; /*--- init coeff */ for (i=0; i<=13; i++) coeff[i] = 0.0; /*--- get current estimates of disturbances */ vcn = e(X,3,0); vce = e(X,4,0); du = e(X,5,0); dpsi = e(X,6,0); /*--- determine if tJ corresponds to a sampling time. If so, get its index j (else j=-1) */ for (i=0,j=-1; i<=index; i++) if (e(est,4,i) == tJ) j = i; /*--- case where tJ does not correspond to a sampling time */ if (j == -1) { for (i=index; i>=0; i--) if (e(est,4,i) > tJ) j = i; #ifdef DEBUG_FILTER /*printf("tJ=%lf does not correspond to a sampling time (index=%d j=%d)\n",tJ,index,j);*/ #endif for (i=index; i>=j-1; i--) { Cdpsi = cos(e(est,0,i)-dpsi); Sdpsi = sin(e(est,0,i)-dpsi); dusp = e(est,2,i)-du; Cpitch = cos(e(est,1,i)); Spitch = sin(e(est,1,i)); if (i == j-1) { dtps = e(est,4,j)-tJ; *epsilon = dtps; } else dtps = e(est,3,i); *x_back -= (Cpitch*Cdpsi*dusp+vcn)*dtps; *y_back -= (Cpitch*Sdpsi*dusp+vce)*dtps; *z_back += Spitch*dusp*dtps; coeff[1] -= Cpitch*Cdpsi*dtps; coeff[2] -= Cpitch*Sdpsi*dtps; coeff[3] += Spitch*dtps; coeff[4] += Cpitch*Sdpsi*dusp*dtps; coeff[5] -= Cpitch*Cdpsi*dusp*dtps; coeff[6] -= Cpitch*Sdpsi*dusp*dtps; coeff[7] += Cpitch*Cdpsi*dusp*dtps; coeff[8] -= Spitch*Cdpsi*dusp*dtps; coeff[9] -= Spitch*Sdpsi*dusp*dtps; coeff[10] -= Cpitch*dusp*dtps; coeff[11] += Cpitch*Cdpsi*dtps; coeff[12] += Cpitch*Sdpsi*dtps; coeff[13] -= Spitch*dtps; } } /*--- case where tJ corresponds to a sampling time */ else { #ifdef DEBUG_FILTER /*printf("tJ=%lf corresponds to a sampling time (index=%d j=%d)\n",tJ,index,j);*/ #endif for (i=index; i>=j; i--) { Cdpsi = cos(e(est,0,i)-dpsi); Sdpsi = sin(e(est,0,i)-dpsi); dusp = e(est,2,i)-du; Cpitch = cos(e(est,1,i)); Spitch = sin(e(est,1,i)); dtps = e(est,3,i); *x_back -= (Cpitch*Cdpsi*dusp+vcn)*dtps; *y_back -= (Cpitch*Sdpsi*dusp+vce)*dtps; *z_back += Spitch*dusp*dtps; coeff[1] -= Cpitch*Cdpsi*dtps; coeff[2] -= Cpitch*Sdpsi*dtps; coeff[3] += Spitch*dtps; coeff[4] += Cpitch*Sdpsi*dusp*dtps; coeff[5] -= Cpitch*Cdpsi*dusp*dtps; coeff[6] -= Cpitch*Sdpsi*dusp*dtps; coeff[7] += Cpitch*Cdpsi*dusp*dtps; coeff[8] -= Spitch*Cdpsi*dusp*dtps; coeff[9] -= Spitch*Sdpsi*dusp*dtps; coeff[10] -= Cpitch*dusp*dtps; coeff[11] += Cpitch*Cdpsi*dtps; coeff[12] += Cpitch*Sdpsi*dtps; coeff[13] -= Spitch*dtps; } } } /*------------------------------------------------------------------------*/ double dist3(double x1,double y1,double z1,double x2,double y2,double z2) { return(sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1))); } /*------------------------------------------------------------------------*/ void dead_reckoning(double dt,double speed,double pitch,double yaw) { double ds; ds = cos(pitch)*speed*dt; dr_dn = ds*cos(yaw); dr_de = ds*sin(yaw); #ifdef DEBUG_INIT syslog(LOG_ERR,"lbl_goats: dead_reckoning - dn = %f, de = %f",dr_dn,dr_de); #endif } /*------------------------------------------------------------------------*/ void lbl_goats2000(double time,double dt,double ping_time,double yaw, double pitch,double speed,double depth,double *tof, double *nfix,double *efix,double *good_tof) { int i,ping_flag,lost=0; /*--- when vehicle pings, log ping time and reset tof and ranges */ if (ping_time != 0.0) { #ifdef DEBUG_FILTER syslog(LOG_ERR,"called with ping time=%lf\n", ping_time); #endif ping_flag = 1; lbl.ping_time = ping_time; for (i=1; i<=lbl.number_of_beacon; i++) { tof[i] = 0.0; lbl.sr[i] = 0.0; } } else ping_flag = 0; /*--- get travel times */ lbl.newdata = 0; for (i=1; i<=lbl.number_of_beacon; i++) { if (tof[i] != 0.0) { lbl.newdata = 1; lbl.sr[i] = 0.5*lbl.sos*(tof[i]-lbl.turn_around_time[i]); #ifdef DEBUG_FILTER syslog(LOG_ERR,"lbl_goats: lbl_goats2000 - lbl.newdata = 1"); #endif } } /*--- dead reck */ dead_reckoning(dt,speed,pitch,yaw); /*--- init filter + fix computation */ if (!filter_initialized) { e(X,0,0) += dr_dn; e(X,1,0) += dr_de; } /*syslog(LOG_ERR,"Calling init_filter");*/ init_filter(ping_flag,time,depth,tof,nfix,efix); /*syslog(LOG_ERR,"Done calling init_filter");*/ /*Don't use 'else' here because init_filter() can modify 'filter_initialized'*/ if (filter_initialized) { #ifdef DEBUG_FILTER syslog(LOG_ERR," lbl_goats: calling filter"); #endif lbl_filter(ping_flag,time,dt,pitch,yaw,depth,speed,tof,good_tof,&lost); #ifdef DEBUG_FILTER syslog(LOG_ERR,"done calling filter"); #endif } /*--- if lost re-initialize */ if (lost) { filter_initialized = 0; count_init_cycles = 0; } } /*------------------------------------------------------------------------*/ void jerome_position(int reset,lbl_array *lbl_in,double curr_time, double interval,double pitch,double yaw,double depth, double speed,double *nfix,double *efix,double *good_tof, double *filter_north,double *filter_east, double *filter_depth,double *north_current, double *east_current,double *speed_bias,double *heading_bias) { static int first=1; int i; double time,dt,ping_time,tof[MAX_BEACON_NUMBER]; #ifdef REAL_DATA_POST_PROCESSING static double last_time; #endif #ifndef REAL_DATA_POST_PROCESSING dt = interval; time = curr_time; #else time = mission_time; if (first) dt = 0.2; else dt = time-last_time; last_time = time; #endif /*--- initialization */ if (first) { syslog(LOG_ERR, "in first loop\n"); first = 0; //init_matrices(); /* initializes all the matrices as globals */ lbl.number_of_beacon = lbl_in->number_of_beacon; lbl.fixgain = 0.5; lbl.sos = lbl_in->sos; lbl.north_init = lbl_in->north_init; lbl.east_init = lbl_in->east_init; /* load beacon positions and turn around time */ for (i=1; i<=lbl.number_of_beacon; i++) { lbl.nb[i] = lbl_in->nb[i]; lbl.eb[i] = lbl_in->eb[i]; lbl.db[i] = lbl_in->db[i]; lbl.turn_around_time[i] = lbl_in->turn_around_time[i]; } lbl.newdata = 0; lbl.tof_var = TOF_VAR; /*--- init filter */ e(X,0,0) = lbl.north_init; e(X,1,0) = lbl.east_init; e(X,2,0) = depth; } /* reset because we obtained a GPS fix */ if (reset) { filter_initialized = 0; count_init_cycles = 0; lbl.east_init = lbl_in->east_init; lbl.north_init = lbl_in->north_init; e(X,0,0) = lbl.north_init; e(X,1,0) = lbl.east_init; e(X,2,0) = depth; } /* load time variables */ ping_time = lbl_in->ping_time; /* copy TOFs to local variables */ for (i=1; i<=lbl.number_of_beacon; i++) tof[i] = lbl_in->tof[i]; #ifdef DEBUG_FILTER syslog(LOG_ERR,"calling goats 2000:,time = %f, dt = %f,ping_time = %f," "yaw = %f, pitch = %f, speed = %f, depth = %f", time,dt,ping_time,yaw,pitch,speed,depth); syslog(LOG_ERR,"tof[1] = %f, tof[2] = %f, tof[3] = %f tof[4] = %f", tof[1],tof[2],tof[3],tof[4]); #endif /*--- do lbl nav calculations */ lbl_goats2000(time,dt,ping_time,yaw,pitch,speed,depth,tof,nfix,efix,good_tof); /*--- copy state vector (nfix, efix and good_tof loaded by lbl_goats2000() */ *filter_north = e(X,0,0); *filter_east = e(X,1,0); *filter_depth = e(X,2,0); *north_current = e(X,3,0); *east_current = e(X,4,0); *speed_bias = e(X,5,0); *heading_bias = e(X,6,0); } /*------------------------------------------------------------------------*/ #ifdef MATLAB_SIM void mexFunction(int nlhs,mxArray *plhs[],int nrhs,const mxArray *prhs[]) { static int first=1; int i; double *ekf_state,*nfix,*efix,*valid_tof,good_tof[MAX_BEACON_NUMBER]; double yaw,pitch,speed,depth; static lbl_array lbl_in; double filter_north,filter_east,filter_depth; double north_current,east_current,speed_bias,heading_bias; /*--- check inputs and outputs */ if(nrhs != 4) mexErrMsgTxt("lbl_goats2000_v2() : 4 inputs !\n"); if(nlhs != 4) mexErrMsgTxt("lbl_goats2000_v2() : 4 outputs !\n"); if(!mxIsStruct(prhs[0])) mexErrMsgTxt("lbl_goats2000_v3() : 1st input is the structure lbl\n"); /*--- outputs */ plhs[0] = mxCreateDoubleMatrix(NB_STATE,1,mxREAL); plhs[1] = mxCreateDoubleMatrix(1,1,mxREAL); plhs[2] = mxCreateDoubleMatrix(1,1,mxREAL); plhs[3] = mxCreateDoubleMatrix((int)mxGetScalar(mxGetField(prhs[0],0,"number_of_beacon")),1,mxREAL); ekf_state = mxGetPr(plhs[0]); nfix = mxGetPr(plhs[1]); efix = mxGetPr(plhs[2]); valid_tof = mxGetPr(plhs[3]); /*--- get timing data */ mission_time = mxGetScalar(prhs[1]); lbl_in.ping_time = mxGetScalar(mxGetField(prhs[0],0,"ping_time")); /*--- get sensor readings */ yaw = (*(mxGetPr(prhs[2])+9)); /* has to be already compensated for magnetic variation */ pitch = (*(mxGetPr(prhs[2])+10)); speed = (*(mxGetPr(prhs[2])+0)); depth = (*(mxGetPr(prhs[2])+8)); /*--- get travel times */ for (i=1; i<=(int)mxGetScalar(mxGetField(prhs[0],0,"number_of_beacon")); i++) lbl_in.tof[i] = (*(mxGetPr(prhs[3])+i-1)); /*--- initialization */ if (first) { first = 0; lbl_in.number_of_beacon = (int)mxGetScalar(mxGetField(prhs[0],0,"number_of_beacon")); lbl_in.sos = (double)mxGetScalar(mxGetField(prhs[0],0,"sos")); lbl_in.north_init = (double)mxGetScalar(mxGetField(prhs[0],0,"north_init")); lbl_in.east_init = (double)mxGetScalar(mxGetField(prhs[0],0,"east_init")); for (i=0; i