more fixes for prediction

This commit is contained in:
Johannes Reinhardt 2008-07-14 00:17:24 +02:00
parent 8a8bb9d91a
commit 9ceefe2ec2
2 changed files with 9 additions and 5 deletions

View File

@ -69,6 +69,8 @@ void fit_parameter(telemetry* t1, telemetry* t2, telemetry* t3, map* m){
void radar_dgl(trial* t, timestamp ts){ void radar_dgl(trial* t, timestamp ts){
timestamp step = ts- t->sim.tm.ts; timestamp step = ts- t->sim.tm.ts;
if (step > 0){ if (step > 0){
t->sim.tm.vehicle.turn = t->vehicle.turn;
t->sim.tm.vehicle.accel = t->vehicle.accel;
dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,1,step); dgl(t,&t->sim.tm.vehicle,&t->sim.tm.vehicle,1,step);
t->sim.tm.ts += step; t->sim.tm.ts += step;
t->sim.steps++; t->sim.steps++;
@ -77,7 +79,7 @@ void radar_dgl(trial* t, timestamp ts){
/*updates the simulation state when new telemetry arrives*/ /*updates the simulation state when new telemetry arrives*/
void sim_update(trial* t){ void sim_update(trial* t){
telemetry *tm,*tmp; telemetry *tm,*tmp;
tm = (telemetry*) g_queue_peek_tail(&t->telemetry); tm = (telemetry*) g_queue_peek_tail(&t->telemetry);
fprintf(stderr, "Time diff: %i\n", tm->ts - t->sim.tm.ts); fprintf(stderr, "Time diff: %i\n", tm->ts - t->sim.tm.ts);
@ -94,6 +96,7 @@ void sim_update(trial* t){
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed); fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
/*update simulation vehicle from telemetry: everything and calculate w*/ /*update simulation vehicle from telemetry: everything and calculate w*/
// tm->vehicle.w = t->sim.tm.vehicle.w;
t->sim.tm.vehicle = tm->vehicle; t->sim.tm.vehicle = tm->vehicle;
g_array_set_size(t->sim.tm.objects, 0); g_array_set_size(t->sim.tm.objects, 0);
g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len); g_array_append_vals(t->sim.tm.objects, tm->objects->data, tm->objects->len);
@ -101,6 +104,7 @@ void sim_update(trial* t){
if(t->telemetry.length > 1){ if(t->telemetry.length > 1){
tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data; tmp = (telemetry*) g_queue_peek_tail_link(&t->telemetry)->prev->data;
t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts); t->sim.tm.vehicle.w = (tm->vehicle.dir - tmp->vehicle.dir)/(tm->ts - tmp->ts);
} }
// fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir); // fprintf(stderr, "w = %f, dir = %f\n", t->sim.tm.vehicle.w, t->sim.tm.vehicle.dir);
fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed); fprintf(stderr, "speed = %f\n", t->sim.tm.vehicle.speed);
@ -125,7 +129,7 @@ int dgl(trial* t, vehicle* after, vehicle* before, timestamp h, timestamp deltat
for(i = 0;i<=end;i++){ for(i = 0;i<=end;i++){
if(i == end) dt = deltat - end*dt; if(i == end) dt = deltat - end*dt;
aw = t->map.aw * ((before->w > wsoll) ? -1 : 1); aw = t->map.aw * ((before->w > wsoll) ? 1 : -1);
/*calculate derivative*/ /*calculate derivative*/
diff.x = cos(tmp.dir*M_PI/180)*tmp.speed/1000; diff.x = cos(tmp.dir*M_PI/180)*tmp.speed/1000;

View File

@ -25,12 +25,12 @@ void trial_loop(trial *t) {
vehicle_hard_right(t); break; vehicle_hard_right(t); break;
default: break; default: break;
} }
while (t->telemetry.length < 5) { while (t->telemetry.length < 6) {
if (-1 == trial_wait_for_input(t)) return; if (-1 == trial_wait_for_input(t)) return;
} }
fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,2), fit_parameter( (telemetry*) g_queue_peek_nth(&t->telemetry,3),
(telemetry*) g_queue_peek_nth(&t->telemetry,3),
(telemetry*) g_queue_peek_nth(&t->telemetry,4), (telemetry*) g_queue_peek_nth(&t->telemetry,4),
(telemetry*) g_queue_peek_nth(&t->telemetry,5),
&t->map); &t->map);
fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k); fprintf(stderr,"Fitparameter: a: %f aw: %f k: %f\n",t->map.a,t->map.aw,t->map.k);
} }