Show
Ignore:
Timestamp:
07/18/10 17:04:54 (14 years ago)
Author:
jabu
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/doprava/traffic_agent.h

    r1137 r1141  
    9595* KOD PRO TrafficAgentCycleTime * 
    9696*********************************/ 
    97         double last_queue; 
    98         double sum_queue_length; 
    99         double last_sum_queue_length; 
    100         double queue_diff_sum; 
    101         double queue_variance_sum; 
    102         int n_of_queues_in_length; 
    103         int last_n_of_queues_in_length; 
    104         double last_variance; 
     97        double queue_last;       
     98        double queue_avg_last; 
    10599        double green_time_ratio; 
    106100        double Tc; 
    … …  
    108102        double saturated_stream; 
    109103        double delta; // ztratovy cas pri prechodu na zelenou 
    110         // udaje opro kalmana 
     104        double delta_w; 
     105        // udaje opro filtr 
    111106        double queue_avg; //  pruner 
    112107        double queue_w;   //  vaha 
    113         double queue_d;   //  rozptyl 
    114         double queue_dd;  //  snizeni verohodnosti odhadu  
    115         double queue_r;   //  neduveryhodnost mereni = 1 
    116108 
    117109        double Ro_avg;  //  prumer 
    118110        double Ro_w;    //  vaha K 
    119         double Ro_d;            //  rozptyl P 
    120         double Ro_dd;           //  snizeni verohodnosti odhadu Q 
    121         double Ro_r;            //  neduveryhodnost mereni = 1 R 
     111 
     112        double cars_in_avg; 
     113        double cars_in_w; 
     114         
    122115 
    123116        double last_queue_avg; 
    … …  
    131124                saturated_stream = 0.5; 
    132125                queue = 0; 
    133                 sum_queue_length = 0; 
    134                 last_sum_queue_length = 0; 
    135                 n_of_queues_in_length = 0; 
    136                 last_n_of_queues_in_length = 0; 
    137                 queue_diff_sum = 0; 
    138                 queue_variance_sum = 0; 
    139                 last_variance = 0; 
    140                 delta = 5; 
    141                 // KALMAN 
     126                queue_last = 0; 
     127                // filtr 
     128                delta = 0; 
     129                delta_w = 0.2; 
     130                 
    142131                queue_avg = 0;  //  prumer 
    143                 queue_w = 1;    //  vaha K 
    144                 queue_d = 0.9;          //  rozptyl P 
    145                 queue_dd =0.1;          //  snizeni verohodnosti odhadu Q 
    146                 queue_r = 9;            //  neduveryhodnost mereni = 1 R 
     132                queue_w = 0.4;  //  vaha                 
    147133 
    148134                Ro_avg = 0;     //  prumer 
    149                 Ro_w = 1;       //  vaha K 
    150                 Ro_d = 0.9;             //  rozptyl P 
    151                 Ro_dd =0.1;             //  snizeni verohodnosti odhadu Q 
    152                 Ro_r = 9;               //  neduveryhodnost mereni = 1 R 
    153  
     135                Ro_w = 0.1;     //  vaha 
     136 
     137                cars_in_avg = 0; 
     138                cars_in_w = 0.4; 
     139                 
    154140                last_queue_avg = 0; 
    155141        } 
    … …  
    161147 
    162148        void echo () { 
    163                 //cout << "n:" << n_of_queues_in_length << " " << getQueueName() << " q:" << getQueue() << " aq:" << getAverageQueueLength() << " diff:" << getAverageQueueGrow() << " D:" << getQueueVariance() << endl; 
    164                 /*cout << "n:" << n_of_queues_in_length  
    165                      << " " << getQueueName()  
    166                          << "LAST aq:" << last_sum_queue_length/last_n_of_queues_in_length  
    167                          << " aq:" << getAverageQueueLength() 
    168                          << " diff" <<  getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length  
    169                          << endl;*/ 
     149                 
    170150                cout << getQueueName(); 
    171                 /*if ( last_n_of_queues_in_length> 0 && n_of_queues_in_length > 0 ) 
    172                         cout << " DIFF: " << getAverageQueueLength() - last_sum_queue_length/last_n_of_queues_in_length; 
    173                 else 
    174                         cout << " no diff available";*/ 
    175                 //cout << "\tn " << n_of_queues_in_length; 
    176                 //cout << " prijizdejicich aut za 90: " << getRo()*90; 
     151                 
    177152                cout << "\tgtr: " << green_time_ratio; 
    178153                cout << "\tRo: " << getRo(); 
    … …  
    188163                        cout << (int)(100*getWT(tc)) << "\t"; 
    189164                } 
    190                 //cout << " diff: " << getRo()*Tc - saturated_stream*getGreenTime(); 
    191                 /*if ( saturated_stream*getGreenTime() >= getAverageQueueLength() ) 
    192                         cout << " projede vsechno "; 
    193                 else 
    194                         cout << " zustanou auta ";*/ 
     165                 
    195166                cout << endl << endl; 
    196167        } 
    … …  
    216187                // pricita pouze namerene udaje 
    217188                if ( ql >= 0 ) { 
    218                         last_queue = queue; 
     189                        queue_last = queue; 
    219190                        queue = ql; 
    220                         queue_diff_sum += queue - last_queue; 
    221                         //queue_variance_sum += abs(queue - last_queue); 
     191                        queue_avg_last = queue_avg; 
     192                         
     193                        // KALMAN                        
     194                        //cout << endl << "pridani fronty\t" << queue_avg << "\t" << ql << endl; 
     195                        queue_avg += queue_w*( ql - queue_avg );                         
     196                        /*cout << getQueueName() << " inputs"; 
     197                        int k = 0; 
     198                        for ( int i = 0; i < rv_inputs.length(); i ++ ) { 
     199                        cout <<"\t"<< rv_inputs.name(i) << ": "; 
     200                        for ( int j = 0; j < rv_inputs.size(i); j ++ ) {                                 
     201                                cout << inputs(k) << " "; 
     202                                k ++; 
     203                        } 
     204                        cout << endl;*/ 
     205                } 
     206                if ( inputs(0) >= 0 ) { 
     207                        cars_in_avg += cars_in_w*( inputs(0) - cars_in_avg ); 
    222208                 
    223                         sum_queue_length += ql; 
    224                         n_of_queues_in_length ++; 
    225                         // KALMAN                        
    226                         cout << endl << "pridani fronty\t" << queue_avg << "\t" << ql << endl; 
    227                         queue_avg += queue_w*( ql - queue_avg ); 
    228                         queue_d += queue_dd; 
    229                         queue_w = queue_d/(queue_d+1); 
    230                         queue_d = (1-queue_w)*queue_d; 
    231          
    232                         double Ro = getActualRo(); 
    233                         Ro_avg += Ro_w*( Ro - Ro_avg ); 
    234                         Ro_d += Ro_dd; 
    235                         Ro_w = Ro_d/(Ro_d+1); 
    236                         Ro_d = (1-Ro_w)*Ro_d; 
    237                 } 
    238         } 
    239  
    240         void resetQueue () { 
    241                 last_Tc = Tc; 
    242                 //last_variance = getQueueVariance(); 
    243                 last_n_of_queues_in_length = n_of_queues_in_length; 
    244                 last_sum_queue_length = sum_queue_length; 
    245                 n_of_queues_in_length = 0; 
    246                 sum_queue_length = 0; 
    247                 queue_variance_sum = 0; 
    248                 queue_diff_sum = 0; 
    249                 // kalman 
    250                 last_queue_avg = getAverageQueueLength(); 
    251         } 
     209                } 
     210                if ( cars_in_avg > 0 && queue_avg_last > saturated_stream*last_Tc*green_time_ratio ) { 
     211                        //double delta_d = queue_avg - queue_avg_last + last_Tc * saturated_stream - cars_in_avg*(double)last_Tc/90;                     
     212                        double delta_d = ( last_Tc/(90*saturated_stream)) * (cars_in_avg - queue_avg + queue_avg_last) - green_time_ratio; 
     213                        delta += delta_w * ( delta_d - delta ); 
     214                        if ( delta < 0 ) 
     215                                delta = 0; 
     216                } 
     217 
     218                cout << getQueueName() << " cars in: " << inputs(0) << " " << cars_in_avg << 
     219                        " q_avd: " << queue_avg << 
     220                        " delta: " << delta << 
     221                        endl; 
     222        } 
     223 
     224 
     225         
    252226 
    253227        double getAverageQueueLength () { 
    254228                // kalmam 
    255229                return queue_avg; 
    256  
    257                 if( n_of_queues_in_length > 0 ) 
    258                         return sum_queue_length / n_of_queues_in_length; 
    259                 else 
    260                         return 0.0; 
    261230        } 
    262231 
    263232        double getLastAverageQueueLength () { 
    264                 if( last_n_of_queues_in_length > 0 ) 
    265                         return last_sum_queue_length / last_n_of_queues_in_length; 
    266                 else 
    267                         return 0.0; 
     233                return queue_avg_last; 
    268234        } 
    269235 
    … …  
    282248        double getRedTime () { return getRedTime( Tc ); }        
    283249 
    284         double getRelativeQueueDiff () { 
    285                 if ( last_n_of_queues_in_length > 0 && n_of_queues_in_length > 0 ) { 
    286                          
    287                         return getAverageQueueLength()/(Tc*(1-green_time_ratio)) - (last_sum_queue_length/last_n_of_queues_in_length)/(last_Tc*(1-green_time_ratio)); 
    288                 } 
    289                 else 
    290                         return 0; 
    291         } 
     250         
    292251 
    293252        // hodnotici funkce - suma cekaciho casu aut za 10h 
    294         double getWT ( double  Tc ) { 
    295           double T = 36000; // celkovy cas 10h 
    296           double Ro = getRo(); 
    297           double Gr = green_time_ratio; 
    298           double ss = saturated_stream; 
    299           double WT = 0; 
    300           double q = 0;  // zacina s nulovou frontou ? 
    301           double ti = 0; 
    302           double sumq = q; 
    303           while ( ti < T ) { 
    304                 // ve fronte stoji vic aut nez je schopno odjet za zelenou 
    305                 if ( q > 0.5*ss*Tc ) { 
    306                   WT += 0.5*ss*(Tc*Gr - delta)*(Tc*Gr - delta); 
    307                 } 
    308                 else { 
    309  
    310                   WT += 0.5*q*(Tc*Gr - delta); 
    311                 } 
    312  
    313                 if ( (Tc*Gr - delta)*ss < q ) 
    314                   q -= (Tc*Gr - delta)*ss; 
    315                 else 
    316                   q = 0; 
    317  
    318                 WT += q * Tc; // zbytek fronty ceka cely cyklus 
    319                 // cekani vozidel, ktera prijela za pocitany cyklus 
    320                 if ( q > 0 ) { 
    321                   //pokud je fronta > 0, auta cekaji prumerne polovinu delky cyklu 
    322                   WT += Ro*Tc*0.5*Tc; 
    323                 } 
    324                 else { 
    325                   // pokud je fronta = 0 cekaji pouze auta, ktera prijela na cervenou (p=(1-Gr)) 0.5Tc(1-Gr) 
    326                   WT += Ro*(1-Gr)*Tc*0.5*(1-Gr)*Tc; 
    327                 } 
    328                 // fronta se zvetsi o Ro*Tc 
    329                 q += Ro*Tc; 
    330                 sumq += Ro*Tc; 
    331                 ti += Tc; 
    332           } 
    333  
    334           //return WT; 
    335           if ( sumq > 0 ) 
    336                 return 100*(WT/(0.5*T*sumq)); 
    337           else 
    338           return 0; 
    339         } 
     253        //double getWT_old ( double  Tc ) { 
     254        //  double T = 36000; // celkovy cas 10h 
     255        //  double Ro = getRo(); 
     256        //  double Gr = green_time_ratio; 
     257        //  double ss = saturated_stream; 
     258        //  double WT = 0; 
     259        //  double q = 0;  // zacina s nulovou frontou ? 
     260        //  double ti = 0; 
     261        //  double sumq = q; 
     262        //  while ( ti < T ) { 
     263        //      // ve fronte stoji vic aut nez je schopno odjet za zelenou 
     264        //      if ( q > 0.5*ss*Tc ) { 
     265        //        WT += 0.5*ss*(Tc*Gr - delta)*(Tc*Gr - delta); 
     266        //      } 
     267        //      else { 
     268 
     269        //        WT += 0.5*q*(Tc*Gr - delta); 
     270        //      } 
     271 
     272        //      if ( (Tc*Gr - delta)*ss < q ) 
     273        //        q -= (Tc*Gr - delta)*ss; 
     274        //      else 
     275        //        q = 0; 
     276 
     277        //      WT += q * Tc; // zbytek fronty ceka cely cyklus 
     278        //      // cekani vozidel, ktera prijela za pocitany cyklus 
     279        //      if ( q > 0 ) { 
     280        //        //pokud je fronta > 0, auta cekaji prumerne polovinu delky cyklu 
     281        //        WT += Ro*Tc*0.5*Tc; 
     282        //      } 
     283        //      else { 
     284        //        // pokud je fronta = 0 cekaji pouze auta, ktera prijela na cervenou (p=(1-Gr)) 0.5Tc(1-Gr) 
     285        //        WT += Ro*(1-Gr)*Tc*0.5*(1-Gr)*Tc; 
     286        //      } 
     287        //      // fronta se zvetsi o Ro*Tc 
     288        //      q += Ro*Tc; 
     289        //      sumq += Ro*Tc; 
     290        //      ti += Tc; 
     291        //  } 
     292 
     293        //  //return WT; 
     294        //  if ( sumq > 0 ) 
     295        //      return 100*(WT/(0.5*T*sumq)); 
     296        //  else 
     297        //  return 0; 
     298        //} 
     299        // stredni doba prujezdu n-teho auta ve fronte 
     300        double getEcarWT ( const double tc, const int n ) { 
     301                double cpg = (tc * green_time_ratio - delta) * saturated_stream; // cars per green time 
     302                double wc = floor( n / cpg ); // number of waiting cycles                
     303                double ET_last_cycle = ( 1/ (2*tc) ) * ( tc*(1-green_time_ratio) + (n-wc*cpg)/saturated_stream )*( tc*(1-green_time_ratio) + (n-wc*cpg)/saturated_stream ); 
     304                double ET = wc*tc + ET_last_cycle; 
     305                return ET; 
     306        } 
     307 
     308        // suma strednich hodnot cekacich casu pres auta ve fronte 
     309        double getWT ( const double tc ) { 
     310                double sumEWT = 0; 
     311                //int n = round(getAverageQueueLength()); 
     312                int n = round(cars_in_avg + getAverageQueueLength()); 
     313                for ( int i = 0; i <= n; i ++ ) { 
     314                        sumEWT += getEcarWT( tc, i ); 
     315                } 
     316                return sumEWT; 
     317        } 
     318 
    340319 
    341320        //double getWT ( double  tc ) {