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);
}
}
}
}
}
