Changeset 1422 for applications

Timestamp:

01/26/12 12:23:52 (12 years ago)

Author:

jabu

Message:

BaseTrafficAgentCt? - zaklad agenta pro rizeni delky cyklu
Master, Observer - rizeni centralnim agentem Master, Observer jen posila data

Location:

applications/doprava

Files:

• BaseTrafficAgentCt.cpp (added)
• BaseTrafficAgentCt.h (added)
• QuadraticMinimalizator.h (modified) (8 diffs)
• QueueData.cpp (added)
• QueueData.h (added)
• TrafficAgentLQMaster.cpp (added)
• TrafficAgentLQMaster.h (added)
• TrafficAgentLQObserver.cpp (added)
• TrafficAgentLQObserver.h (added)
• traffic_agent_LQ.h (modified) (4 diffs)

applications/doprava/QuadraticMinimalizator.h

@@ -25 +25 @@
             e << Q << endl;
             it_warning(e.str());
+                        return false;
         }
         if ( !isDiagonal(R) ) {
@@ -31 +32 @@
             e << R << endl;
             it_warning(e.str());
+                        return false;
         }
         if ( !isPositiveDefinite(Q) ) {
@@ -37 +39 @@
             e << Q << endl;
             it_warning(e.str());
+                        return false;
         }
         if ( !isPositiveDefinite(R) ) {
@@ -43 +46 @@
             e << R << endl;
             it_warning(e.str());
+                        return false;
         }
+                return true;
     }
 
@@ -56 +61 @@
     }
 
-
-    /**
-     * x(t+1) = Ax(t) + Bu(t) + C
-     * @param horizont
-     * @param C
-     * @return L: u(t) = L [ x(t) 1 ]
-     */
-    mat L( const int horizont, const mat & C ) {
-        it_warning("Nijak se tam C neprojevuje!!!");
-        int xdim = A.cols();
-        int udim = B.cols();
-        mat sqQ = sqrt(Q);
-        mat sqR = sqrt(R);
-        mat M0, M;
-        mat qrQ;
-        mat Lall, Lc1, Lc2, Lc3, Lx1, Lx2, Lu, L;
-        M0 = concat_vertical(
-                concat_vertical(
-                    concat_horizontal( sqQ * B,   concat_horizontal( sqQ * A,             zeros(xdim, udim) ) ),
-                    concat_horizontal( sqR,       concat_horizontal( zeros( udim, xdim ), zeros(udim, udim) ) )
-                ),
-                concat_horizontal( zeros( xdim, xdim + udim ) , sqQ * C)
-            );
-
-        M = M0;
-        for (int h = horizont; h >= 0; h --) {
-            qr(M, Lall);
-
-            Lu  = Lall(0, udim-1, 0,         udim-1     );
-            Lx1 = Lall(0, udim-1, udim,      udim+xdim-1);
-            Lc1 = Lall(0, udim-1, udim+xdim, udim+xdim  );
-            Lx2 = Lall(udim, udim+xdim-1,  udim, udim+xdim-1);
-            Lc2 = Lall(udim, udim+xdim-1,  udim+xdim, udim+xdim);
-            Lc3 = Lall(udim+xdim, udim+xdim+xdim-1,  udim+xdim, udim+xdim);
-            M = concat_vertical(
-                    M0,
-                    concat_vertical(
-                        concat_horizontal( Lx2 * B, concat_horizontal( Lx2 * A, Lc2 ) ),
-                        concat_horizontal( zeros(xdim, xdim + udim), Lc3 )
-                    )
-                );
-        }
-        L = concat_horizontal( - inv(Lu) * Lx1, - inv(Lu) * Lc1);
-        return L;
-    }
     /**
      * x(t+1) = Ax(t) + Bu(t)
@@ -134 +94 @@
     }
 
+        /*
     double maxDif( const mat & M1, const mat & M2 ) {
         return max(max(abs(M1 - M2), 1));
@@ -150 +111 @@
             return false;
     }
-
+*/
     bool isDiagonal(  const mat M ) {
         mat D = abs(M);
@@ -162 +123 @@
         return true;
     }
+        
 
     bool isPositiveDefinite( const mat M ) {

applications/doprava/traffic_agent_LQ.h

@@ -1 +1 @@
 
+#include "BaseTrafficAgentCt.h"
 #include "QuadraticMinimalizator.h"
 
-class TrafficAgentLQ : public BaseTrafficAgent {
+class TrafficAgentLQ : public BaseTrafficAgentCt {
 protected:
-        mat A, B, Q, R;
-        static const int T = 90;
-        int L;
-        int nOfBroadcast;
         QuadraticMinimalizator * minimizer;
-        //map<string, int> queueIndex;
+        
 public:
         // FUNKCE VOLANE V main_loop NA ZACATKU
         
+        void from_setting(const Setting& set) {
+                BaseTrafficAgentCt::from_setting(set);          
+        }
+
         void validate (){
-                L = 90;
-                BaseTrafficAgent::validate();
-
-                rv_action = RV("Tc",1);
-                rv_action.add( RV( stage_names, ones_i(stage_names.length()) ) );
-
-                for ( int i=0; i<lanehs.length(); i ++ ) {
-                        unsigned int index = find_index( green_names, name + "_" + lanehs(i)->getSG() );
-                        lanehs(i)->green_time_ratio = green_times( index );
-                }
-                
-                Q = diag( ones( queues.length()) );
-                R = "1";
-                A = diag( ones( queues.length()) );
-                B = ones( queues.length(), 1);
-
-                minimizer = new QuadraticMinimalizator(A,B,Q,R);
+                BaseTrafficAgentCt::validate();                                 
                 
                 cout << "HELLO! " << name << " IS HERE!" << endl;
-                /*
-                cout << "inputs " << inputs.length() << endl;           
-                cout << "queues " << queues.length() << endl;
-                cout << "rv_inputs" << rv_inputs.length() << endl << rv_inputs << endl;
-                cout << "rv_queues" << rv_queues.length() << endl << rv_queues << endl;
-                cout << "lanehs " << lanehs.length() << endl << endl;
-                cout << "A B Q R " << endl << A << endl << B << endl << Q << endl << R << endl << endl;
-                */
         }
 
         // FUNKCE VOLANE V main_loop V KAZDEM CYKLU
-        void adapt (const vec &glob_dt) {
-                for ( int i=0; i<lanehs.length(); i ++ ) {
-                        //lanehs(i)->inputs( 0 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) );
-                        //lanehs(i)->inputs( 1 ) = inputs( 2*find_index(rv_inputs, lanehs(i)->rv_inputs.name(0) ) + 1 );
-                        lanehs(i)->queue = queues( find_index( rv_queues, lanehs(i)->getQueueName() ) );
-                        lanehs(i)->countAvgs();
-                }
-                
+        void adapt (const vec &glob_dt) {       
+                BaseTrafficAgentCt::adapt(glob_dt);
+
+                Q = 100 * diag( ones( queues.length()) ) + diag(computedQueues);
+                R = "0.0000001";
+                A = diag( ones( queues.length()) );
+                B = zeros( queues.length(), 1); 
+
                 // jeden clen matice B
                 for ( int i=0; i<queues.length(); i ++ ) {
-                        B(i,0) = 0.5 * lanehs(i)->green_time_ratio;
+                        B(i,0) = - s_flow(i) * lanehs(i)->green_time_ratio;
                 }
 
                 
-                
-                BaseTrafficAgent::adapt(glob_dt);
         }
 
@@ -71 +46 @@
                                                 stringstream coefStr;
                                                 coefStr << "coef" << lanehs(i)->rv_outputs.name(j);
-                                                double val = 0.5 * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
+                                                double val = s_flow(i) * lanehs(i)->green_time_ratio * lanehs(i)->getLane().alpha(j);
                                                 sendToAll<double>(set, coefStr.str(), val);
                                         }
@@ -77 +52 @@
                         }
                 }
+
+
+                if ( nOfBroadcast == 1 ) {
+                        broadcastTc(set);
+                }
+                
                 nOfBroadcast ++;
+        }
+
+        double votingWeight() {
+                //vec computedQueues = getComputedQueues();
+                double qsum = 0;
+                for ( int i = 0; i < computedQueues.length(); i++ ) {
+                        qsum += computedQueues(i);
+                }
+                if ( qsum > 1 )
+                        return qsum;
+                else
+                        return 1;
+        }
+
+        void broadcastTc( Setting & set) {
+                // posli idealni tc
+                Tc_computed = computeTc();
+                stringstream tcstr;
+                tcstr << "timecycle" << name;
+                
+                // posli vahu hlasu (suma front)
+                //vec computedQueues = getComputedQueues();     
+                
+                vec tc_vec = "0.0 0.0";
+                tc_vec(0) = Tc_computed;
+                tc_vec(1) = votingWeight();
+                // posilam vektor [tc, w]
+                sendToAll<vec>(set, tcstr.str(), tc_vec);
         }
 
@@ -88 +97 @@
                         double val;
                         UI::get(val, msg, "value");
-                        B(i,0) = B(i,0) - val;
-                        //cout << name << " received coefs " << inputName << " " << what << endl << val <<endl;
+                        B(i,0) = B(i,0) + val;
                 }
+
+                if ( what.substr(0,9) == "timecycle" ) {
+                        vec val;
+                        UI::get(val, msg, "value");
+                        cout << "receiving Tc " << val << endl;
+                        Tc_sum += val(0) * val(1);
+                        Tc_w_sum += val(1);                     
+                }
+
         }
 
-        void act (vec &glob_ut){
-                //cout <<"A act"<<endl<<A<<endl;
-                B = B * T * L;
-                minimizer = new QuadraticMinimalizator(A, B, Q, R);
-                vec computedQueues = getComputedQueues();
-                vec res = minimizer->minimize(computedQueues, 100);
-                //cout << "minimalization res " << name << endl << res << endl << endl;
-                delete minimizer;
-                int Tc = 80;
-                vec action;
-                action.set_size(rv_action._dsize());
-                action(find_index(rv_action, "Tc")) = Tc;       
-                int st;
-                int stage_time_sum = 0;  // soucet delky fazi
-                action(find_index(rv_action, "Tc")) = Tc;               
-                for ( int i =0; i < stage_names.length(); i ++) {                       
-                        if ( (i+1) < stage_names.length() ) {
-                                st = round(((double)(stage_times(i)*Tc))/80.0);
-                                stage_time_sum += st;
-                                action(find_index(rv_action, stage_names(i))) = st;
-                        }
-                        else { // dopocitani posledni faze - oprava zaokrouhlovaci chyby
-                                action(find_index(rv_action, stage_names(i))) = Tc - stage_time_sum;
-                        }
-                }
-                action2ds.filldown(action,glob_ut);
+        void act (vec &glob_ut){                
+                //vec computedQueues = getComputedQueues();
+                cout << endl << name << " ACT" << endl;
+                cout << "Tc computed = " << Tc_computed << endl;
+                cout << "queues = " << round( computedQueues )<< endl;                  
+                double w = votingWeight();
+                Tc_sum += Tc_computed * w;
+                Tc_w_sum += w;
+                int Tc = (int)round( Tc_sum / Tc_w_sum );               
+                setCycleTime(Tc, glob_ut);
+                looper ++;
+        }
+// VYPOCETNI FUNKCE
+
+        // saturovany tok pruhu i
+        double s_flow(int i) {
+                double min_flow = 0.3;
+                double max_flow = 0.5;
+                double input = 0;
+                if ( 2*i < inputs.length() )
+                        input = inputs(2*i);
+                
+                vec q = getComputedQueues();
+                double flow = q(i);
+                if ( input > 0 )
+                        flow += input;
+                flow = flow/T;
+                if ( flow > max_flow )
+                        return max_flow;
+                if ( flow < min_flow )
+                        return min_flow;
+                else
+                        return flow;
         }
 
-        void setCycleTime( int Tc, vec &glob_ut ) {
-                
+        double computeTc() {
+                B = B * T;
+                //vec computedQueues = getComputedQueues();
+                minimizer = new QuadraticMinimalizator(A, B, Q, R);             
+                mat L_mat = minimizer->L( 100);
+                vec u_vec = L_mat * computedQueues;
+                double u = u_vec(0);
+                double Tc_computed = L / (1 - u);
+                delete minimizer;
+                if ( u >= 1 || Tc_computed > Tc_max ) {
+                        cout << endl << "Tc error : " << Tc_computed << " setting to max" << endl;
+                        Tc_computed = Tc_max;
+                }
+                if ( Tc_computed < Tc_min ) {
+                        cout << endl << "Tc error : " << Tc_computed << " setting to min" << endl;
+                        Tc_computed = Tc_min;   
+                }
+                return Tc_computed;
         }
+
+        
+
 
 
         
-// POMOCNE FCE
-        vec getComputedQueues() {
-                vec q = zeros( queues.length() );
-                for ( int i = 0; i < lanehs.length(); i++ ) {
-                        q(i) = lanehs(i)->queue_avg;
-                }
-                return q;
-        }
 
-        template <class T> void sendToAll( Setting &set, string msgCode, T value) {
-                for ( int i = 0; i < neighbours.length(); i ++  ) {     
-                        Setting & msg = set.add(Setting::TypeGroup);
-                        UI::save( neighbours(i), msg, "to" );
-                        UI::save (name,msg,"from");
-                        UI::save( msgCode, msg, "what" );
-                        UI::save( value, msg, "value" );
-                }
-        }
 
-        void printVector ( RV rv_vector, vec vector, string description ) {
-                cout << endl << description << " " << name << endl;
-                int k = 0;
-                for ( int i = 0; i < rv_vector.length(); i ++ ) {
-                        cout << rv_vector.name(i) << " : ";
-                        for ( int j = 0; j < rv_vector.size(i); j ++ ) {                                
-                                cout << vector(k) << " ";
-                                k ++;
-                        }
-                        cout << endl;
-                }
-                cout << endl;
-        }
-
-        unsigned int find_index ( RV rv_vector, string index_string ) {
-                for ( unsigned int i = 0; i < rv_vector.length(); i ++) {
-                        if ( rv_vector.name(i) == index_string )
-                                return i;
-                }
-                return -1;
-        }
-
-        unsigned int find_index ( Array <string> arr, string index_string ) {
-                for ( unsigned int i = 0; i < arr.length(); i ++) {
-                        if ( arr(i) == index_string )
-                                return i;
-                }
-                return -1;
+// KONEC
+        ~TrafficAgentLQ() {
+                
         }