energy_evaluation_struct_t *reevaluate_energy_level(int64_t battery_state, int64_t battery_capacity) { energy_evaluation_struct_t * ret = (energy_evaluation_struct_t*)malloc(sizeof(energy_evaluation_struct_t)); uint8_t state = 0; //set to max state bool minalive = FALSE; bool maxalive; int64_t minenergy = 0; int64_t maxenergy = 0; while (state <= STATE_BASE && !minalive) { minalive = predict_state(battery_state, battery_capacity, state, 0.0, &minenergy); if (!minalive) { state++; } } //we have the right state...now get the grade if (!minalive) { state = STATE_BASE; ret->state_grade = 0.0; } else { maxalive = predict_state(battery_state, battery_capacity, state, 1.0, &maxenergy); if (maxalive) { ret->state_grade = 1.0; } else { double mingrade = 0.0; double maxgrade = 1.0; double midgrade; while ((maxgrade - mingrade) > .001) { midgrade = (mingrade + maxgrade)/2; if (predict_state(battery_state, battery_capacity, state, midgrade,NULL)) { mingrade = midgrade; } else { maxgrade = midgrade; } } ret->state_grade = midgrade; } } ret->energy_state = state; ret->cost_of_reevaluation = 0; printf("state = %d(%lf)\n",state, ret->state_grade); return ret; }
uint8_t kalman_update( kalman_robot_handle_t * handle, const position_t * measurement, float delta_t, robot_pos_t * dest) { if(handle == NULL || dest == NULL || delta_t < 0.0f) { return 0; } os_mutex_take(&(handle->_mutex)); // predict new state predict_state(&(handle->_state), delta_t, &(handle->_state)); // predict new covariance covariance_t proc_noise_cov; process_noise_covariance(handle, delta_t, &proc_noise_cov); predict_covariance( &(handle->_state_covariance), &proc_noise_cov, delta_t, &(handle->_state_covariance)); // if there is a measurement make kalman update if(measurement != NULL) { // compute kalman gain kalman_gain_t gain; kalman_gain( &(handle->_state_covariance), &(handle->_measurement_covariance), &gain); // compute difference between prediction and measurement vec2d_t residual; residual._x = measurement->x - handle->_state._x; residual._y = measurement->y - handle->_state._y; // estimate new state considering measurement update_state(&(handle->_state), &gain, &residual, &(handle->_state)); update_covariance( &(handle->_state_covariance), &gain, &(handle->_state_covariance)); } // write resulting position (and associated variances) to dest dest->x = handle->_state._x; dest->y = handle->_state._y; dest->var_x = handle->_state_covariance._cov_a._a; dest->var_y = handle->_state_covariance._cov_a._d; dest->cov_xy = handle->_state_covariance._cov_a._b; os_mutex_release(&(handle->_mutex)); return 1; }
/*!***************************************************************************** ******************************************************************************* \note process_blobs \date Feb 1999 \remarks filtering and differentiation of the sensor readings ******************************************************************************* Function Parameters: [in]=input,[out]=output \param[in] raw : raw blobs as input \param[out] blobs : the processed blob information ******************************************************************************/ void process_blobs(Blob2D raw[][2+1]) { int i,j,n; double pos,vel,acc; int rfID; static Blob3D *traw; static int firsttime = TRUE; double x[2+2+1], x1[2+1], x2[2+1]; if (firsttime) { firsttime = FALSE; traw = (Blob3D *) my_calloc(max_blobs+1,sizeof(Blob3D),MY_STOP); } for (i=1; i<=max_blobs; ++i) { if (!raw_blob_overwrite_flag) { /* the status of a blob is the AND of the two components */ traw[i].status = raw[i][1].status && raw[i][2].status; switch (stereo_mode) { case VISION_3D_MODE: /* coordinate transformation */ if (traw[i].status) { x[1] = raw[i][1].x[1]; x[2] = raw[i][1].x[2]; x[3] = raw[i][2].x[1]; x[4] = raw[i][2].x[2]; rfID = 0; if (predictLWPROutput(BLOB2ROBOT, x, x, 0.0, TRUE, traw[i].x, &rfID) == 0) { traw[i].status = FALSE; } } break; case VISION_2D_MODE: if (traw[i].status) { x1[1] = raw[i][1].x[1]; x1[2] = raw[i][1].x[2]; x2[1] = raw[i][2].x[1]; x2[2] = raw[i][2].x[2]; #if 1 /* stereo calculation based on the calibration */ stereo (x1, x2, traw[i].x); #endif } break; } } else { /* use the learning model for the coordinate transformation (but not in simulation) */ traw[i] = raw_blobs[i]; /* CGA: should be updated to use 2D blobs, not 3D blobs. */ } /* post processing */ /* if the status changed from FALSE to TRUE, it is important to re-initialize the filtering */ if (traw[i].status && !blobs[i].status) { for (j= _X_; j<= _Z_; ++j) { for (n=0; n<=FILTER_ORDER; ++n) { blobpp[i].fblob[_X_][j].filt[n] = traw[i].x[j]; blobpp[i].fblob[_Y_][j].filt[n] = 0; blobpp[i].fblob[_Z_][j].filt[n] = 0; blobpp[i].fblob[_X_][j].raw[n] = traw[i].x[j]; blobpp[i].fblob[_Y_][j].raw[n] = 0; blobpp[i].fblob[_Z_][j].raw[n] = 0; } } } if (blobpp[i].filter_type == KALMAN) { ; } else { /* default is butterworth filtering */ for (j= _X_; j<= _Z_; ++j) { if (traw[i].status) { blobpp[i].pending = FALSE; blobs[i].status = TRUE; /* filter and differentiate */ pos = filt(traw[i].x[j],&(blobpp[i].fblob[_X_][j])); vel = (pos - blobpp[i].fblob[_X_][j].filt[2])*(double) vision_servo_rate; vel = filt(vel,&(blobpp[i].fblob[_Y_][j])); acc = (vel - blobpp[i].fblob[_Y_][j].filt[2])*(double) vision_servo_rate; acc = filt(acc,&(blobpp[i].fblob[_Z_][j])); if (blobpp[i].acc[j] != NOT_USED) { acc = blobpp[i].acc[j]; } } else { if (++blobpp[i].pending <= MAX_PENDING) { pos = blobpp[i].predicted_state.x[j]; vel = blobpp[i].predicted_state.xd[j]; acc = blobpp[i].predicted_state.xdd[j]; // Note: leave blobs[i].status as is, as we don't want // to set something true which was not true before } else { pos = blobs[i].blob.x[j]; vel = 0.0; acc = 0.0; blobs[i].status = FALSE; } /* feed the filters with the "fake" data to avoid transients when the target status becomes normal */ pos = filt(pos,&(blobpp[i].fblob[_X_][j])); vel = filt(vel,&(blobpp[i].fblob[_Y_][j])); acc = filt(acc,&(blobpp[i].fblob[_Z_][j])); } /* predict ahead */ blobs[i].blob.x[j] = pos; blobs[i].blob.xd[j] = vel; blobs[i].blob.xdd[j] = acc; if (blobs[i].status) { predict_state(&(blobs[i].blob.x[j]),&(blobs[i].blob.xd[j]), &(blobs[i].blob.xdd[j]),predict); /* predict the next state */ blobpp[i].predicted_state.x[j] = pos; blobpp[i].predicted_state.xd[j] = vel; blobpp[i].predicted_state.xdd[j] = acc; predict_state(&(blobpp[i].predicted_state.x[j]), &(blobpp[i].predicted_state.xd[j]), &(blobpp[i].predicted_state.xdd[j]), 1./(double)vision_servo_rate); } } } } }