Changeset 1422 for applications/doprava/QuadraticMinimalizator.h

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

Files:

• applications/doprava/QuadraticMinimalizator.h (modified) (8 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 ) {