Difference between revisions of "Acrob navigation in maze using neural network (Kundlová, Jurenka)"

From RoboWiki
Jump to: navigation, search
Line 1: Line 1:
[[Image:Example.jpg]]
+
----
 +
Backpropagation v Matlabe
 +
 
 +
    function [J, grad] = nnCostFunction(nn_params, ...
 +
                                          input_layer_size, ...
 +
                                      hidden_layer_size, ...
 +
                                      num_labels, ...
 +
                                      X, y, lambda)
 +
        Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
 +
                        hidden_layer_size, (input_layer_size + 1));
 +
        Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
 +
                        num_labels, (hidden_layer_size + 1));
 +
        m = size(X, 1);
 +
        J = 0;
 +
        Theta1_grad = zeros(size(Theta1));
 +
        Theta2_grad = zeros(size(Theta2));
 +
        X = [ones(m, 1) X];
 +
        a1 = X;
 +
        z2 = a1*Theta1';
 +
        a2 = sigmoid(z2);
 +
        a2 = [ones(size(a2,1), 1) a2];
 +
        z3 = a2*Theta2';
 +
        a3 = sigmoid(z3);
 +
        h = a3;
 +
        y1 = zeros(size(y,1), num_labels);
 +
        for i=1:size(y)
 +
            y1(i, y(i)) = 1;
 +
        end
 +
        J = 1/m * sum(sum(((-y1).* log(h) - (1-y1).* log(1-h))));
 +
        temp1 = Theta1;
 +
        temp1(:,1) = zeros(size(temp1,1),1);
 +
        temp2 = Theta2;
 +
        temp2(:,1) = zeros(size(temp2,1),1);
 +
        J = J + lambda / (2*m) * ( ...
 +
                sum(sum(temp1 .*temp1)) + sum(sum(temp2.*temp2)) ...
 +
            );
 +
        for t = 1:m
 +
            a1 = X(t,:);
 +
            z2 = a1*Theta1';
 +
            a2 = sigmoid(z2);
 +
            a2 = [ones(size(a2,1), 1) a2];
 +
            z3 = a2*Theta2';
 +
            a3 = sigmoid(z3);
 +
            yk = zeros(num_labels,1);
 +
            yk(y(t)) = 1;
 +
            d3 = a3' - yk;
 +
            d2 = (Theta2'*d3).*[ones(1,1); sigmoidGradient(z2')];
 +
            d2 = d2(2:end);
 +
            Theta1_grad = Theta1_grad + d2*a1;
 +
            Theta2_grad = Theta2_grad + d3*a2;
 +
        end
 +
        Theta1_grad = Theta1_grad / m;
 +
        Theta2_grad = Theta2_grad / m;
 +
        Theta1_grad = Theta1_grad + ( (lambda/m) .* [ zeros( hidden_layer_size, 1 ), Theta1( :, 2:end ) ] );
 +
        Theta2_grad = Theta2_grad + ( (lambda/m) .* [ zeros( num_labels, 1 ), Theta2( :, 2:end ) ] );
 +
        grad = [Theta1_grad(:) ; Theta2_grad(:)];
 +
    end
 +
 
 +
 
 +
----
 +
 
 +
Java
 +
 
 +
 
 +
[http://www.youtube.com/watch?v=RTendHbPHPU Video]
 +
----
 +
 
 +
 
 +
    #include <Servo.h>
 +
    Servo LeftServo; 
 +
    Servo RightServo;
 +
    double THETA_1[5][5] = {
 +
    {  -3.3015,  -3.2262,    8.4318,  -4.8450,  15.3742},
 +
    {  -4.7470,  -0.3573,  -9.7632,  -9.3562,  18.5839},
 +
    {  -13.2009,    8.7361,    8.5763,    7.5276,  10.6005},
 +
    {    7.0469,  -10.8166,  -9.0304,  -4.0693,  13.2435},
 +
    {    2.6464,  -6.6205,  -2.8116,  11.4895,    8.3321}
 +
    };   
 +
    double THETA_2[5][6] = {
 +
    { -12.1840,  25.3169,  27.4735,  -23.5952,  -0.5421,  -2.3738},
 +
    { -14.6229,  -2.5615,  -26.4061,  -0.6488,  27.8756,    1.7607},
 +
    {  12.8366,  -16.8083,  -3.1984,  -14.8165,  -10.5194,  -16.8826},
 +
    { -17.2910,  -4.2473,  -10.6967,    8.6676,  -24.9129,  25.1679},
 +
    { -12.0564,    3.1205,  -3.9859,  21.9216,  -4.5046,  -25.5430}
 +
    };
 +
    char* ACTION_NAMES[]={
 +
      "ERROR",
 +
      "FORWARD",
 +
      "RIGHT",
 +
      "LEFT",
 +
      "FORWARD RIGHT",
 +
      "FORWARD LEFT"
 +
    };
 +
    double LINE_TRESHOLD = 0.5;
 +
    #define FW_SPEED 1.0
 +
    #define BIG_ROT_AMOUNT 1.0
 +
    #define SLOWDOWN 1
 +
    #define SMALL_ROT_AMOUNT 1
 +
    #define LEFT_SENSOR    3
 +
    #define RIGHT_SENSOR    2
 +
    #define CENTER_SENSOR  0
 +
    #define FRONT_SENSOR    1
 +
    #define TRESHOLD 500
 +
    double ACTIONS[6][2] = {
 +
      {0,0},
 +
      {FW_SPEED, 0},
 +
      {0, -BIG_ROT_AMOUNT },
 +
      {0, BIG_ROT_AMOUNT },
 +
      {/*FW_SPEED / SLOWDOWN*/0, -SMALL_ROT_AMOUNT},
 +
      {/*FW_SPEED / SLOWDOWN*/0, SMALL_ROT_AMOUNT}
 +
    } ;
 +
    #define INPUT_SIZE 4
 +
    #define Euler 2.71828182845904523536
 +
    #define THETA_1_ROWS 5
 +
    #define THETA_1_COLUMNS 5
 +
    #define THETA_2_ROWS 5
 +
    #define THETA_2_COLUMNS 6
 +
    int REPEAT_1[INPUT_SIZE] = {0,0,0,0};
 +
    int REPEAT_2[INPUT_SIZE] = {1,1,1,0};
 +
    int last = 1;
 +
    boolean compareArrays(int  a[], int b[]){
 +
      for(int i =0 ; i < INPUT_SIZE; i++){
 +
        if(a[i] != b[i]){
 +
          return false;
 +
        }
 +
      }
 +
      return true;
 +
    }
 +
    void transpose(double * matrix, double * result , int rr, int cc){
 +
        for(int r =0 ; r <  rr; r++){
 +
        for(int c =0 ; c <  cc; c++){
 +
          result[c * rr  + r] = matrix[r * cc + c];
 +
        }
 +
      }
 +
    }
 +
    int feed(int input[]) {
 +
        if (last != 1 && ( compareArrays(input, REPEAT_1) || compareArrays (input, REPEAT_2)  )) {
 +
            return last;
 +
        }
 +
        double x[1][INPUT_SIZE];
 +
        for (int i = 0; i < INPUT_SIZE; i++) {
 +
            x[0][i] = input[i];
 +
        }
 +
        int m = 1;
 +
        double ones[m][1];
 +
        for (int i = 0; i < m; i++) {
 +
            ones[i][0] = 1;
 +
        }
 +
        double t1t[THETA_1_COLUMNS][THETA_1_ROWS];
 +
        transpose((double*)THETA_1, (double*)t1t , THETA_1_ROWS, THETA_1_COLUMNS);
 +
        double t2t[THETA_2_COLUMNS][THETA_2_ROWS];
 +
        transpose((double*)THETA_2, (double*)t2t, THETA_2_ROWS, THETA_2_COLUMNS);
 +
        double a0[m][INPUT_SIZE+1];
 +
        int params[6];
 +
        params[0] = m;
 +
        params[1] = INPUT_SIZE+1;
 +
        params[2] = m;
 +
        params[3] = 1;
 +
        params[4] = 0;
 +
        params[5] = 0;
 +
        combineMatrix((double*)ones, (double*)a0, params);
 +
        params[2] = m;
 +
        params[3] = INPUT_SIZE;
 +
        params[4] = 0;
 +
        params[5] = 1;
 +
        combineMatrix((double*)x, (double*)a0, params);
 +
        double h1[m][THETA_1_ROWS];
 +
        zeroMatrix((double*)h1, m, THETA_1_ROWS);
 +
        int mparams[4];
 +
        mparams[0] = m;
 +
        mparams[1] = INPUT_SIZE + 1;
 +
        mparams[2] = THETA_1_COLUMNS;
 +
        mparams[3] = THETA_1_ROWS;
 +
        multiply((double*)a0, (double*)t1t, (double*)h1, mparams);
 +
        sigmoid((double*)h1,(double*)h1 , m , THETA_1_ROWS);
 +
        double a1[m][THETA_1_ROWS+1];
 +
        params[0] = m;
 +
        params[1] = THETA_1_ROWS+1;
 +
        params[2] = m;
 +
        params[3] = 1;
 +
        params[4] = 0;
 +
        params[5] = 0;
 +
        combineMatrix((double*)ones, (double*)a1, params);
 +
        params[2] = m;
 +
        params[3] = THETA_1_ROWS;
 +
        params[4] = 0;
 +
        params[5] = 1;
 +
        combineMatrix((double*)h1, (double*)a1, params);
 +
        double h2[m][THETA_2_COLUMNS];
 +
        zeroMatrix((double*)h2, m, THETA_2_ROWS);
 +
        mparams[0] = m;
 +
        mparams[1] = THETA_1_ROWS + 1;
 +
        mparams[2] = THETA_2_COLUMNS;
 +
        mparams[3] = THETA_2_ROWS;
 +
        multiply((double*)a1, (double*)t2t, (double*)h2, mparams);
 +
        sigmoid((double*)h2,(double*)h2 , m , THETA_2_ROWS);
 +
        int actionIndex = maxValue((double*)h2, THETA_2_ROWS * m) + 1;
 +
        last = actionIndex;
 +
        return actionIndex;
 +
    }
 +
    double sigmoid(double * matrix, double * result, int rr, int cc){
 +
        for(int r =0 ; r < rr; r++){
 +
        for(int c =0 ; c < cc; c++){
 +
          result[r*cc + c] = 1.0/(1.0+exp(-matrix[r*cc + c]));
 +
        }
 +
      }
 +
    }
 +
    void copyMatrix(double * matrix, double * result, int rr, int cc){
 +
      int param[6];
 +
      param[0] = rr;
 +
      param[1] = cc;
 +
      param[2] = rr;
 +
      param[3] = cc;
 +
      param[4] = 0;
 +
      param[5] = 0;
 +
      combineMatrix(matrix, result, param);
 +
    }
 +
    void combineMatrix(double * matrix, double * result, int param[6]){
 +
      int lr = param[0];
 +
      int lc = param[1];
 +
      int sr = param[2];
 +
      int sc = param[3];
 +
      int ro = param[4];
 +
      int co = param[5];
 +
      for(int r =0 ; r <  sr; r++){
 +
        for(int c =0 ; c < sc; c++){
 +
          result[(r + ro ) * lc + co + c] = matrix[r * sc + c];
 +
        }
 +
      }
 +
    }
 +
    int maxValue(double* A, int length){
 +
      int mValue = 0;
 +
      for(int i=0; i<length; i++){
 +
        if(A[mValue] <= A[i]){
 +
          mValue = i;
 +
        }
 +
      }
 +
      return mValue;
 +
    }
 +
    void multiply(double * A, double * B, double * C, int dim[4]) {
 +
          /*int mA = A.length;
 +
          int nA = A[0].length;
 +
          int mB = B.length;
 +
          int nB = A[0].length;*/
 +
          int mA = dim[0];
 +
          int nA = dim[1];
 +
          int mB = dim[2];
 +
          int nB = dim[3];
 +
          if (nA != mB){
 +
                Serial.print("Multiplication error ");
 +
                Serial.print(nA);
 +
                Serial.print(" ");
 +
                Serial.println(mB);
 +
          }
 +
          for (int i = 0; i < mA; i++){
 +
              for (int j = 0; j < nB; j++){
 +
                  for (int k = 0; k < nA; k++){
 +
                      C[i*nB+j] += (A[i*nA+k] * B[k*nB+j]);
 +
                    }
 +
              }
 +
          }
 +
    }
 +
    void printMatrix(double * matrix, int rr, int cc){
 +
      for(int r =0 ; r <  rr; r++){
 +
        for(int c =0 ; c < cc; c++){
 +
          Serial.print(matrix[r*cc+c]);
 +
          Serial.print(" ");
 +
        }
 +
        Serial.println();
 +
      }
 +
        Serial.println();
 +
    }
 +
    void zeroMatrix(double * matrix, int rr, int cc){
 +
      for(int r =0 ; r <  rr; r++){
 +
        for(int c =0 ; c < cc; c++){
 +
          matrix[r*cc+c] = 0;
 +
        }
 +
      }
 +
    }
 +
    void setup() {
 +
      Serial.begin(9600);
 +
      int input[4];
 +
      input[0] = 0;
 +
      input[1] = 0;
 +
      input[2] = 0;
 +
      input[3] = 0; 
 +
      int actionIndex = feed(input);
 +
      Serial.println(actionIndex);
 +
      LeftServo.attach(9);
 +
      RightServo.attach(10);
 +
    }
 +
    int convert(long val){
 +
      if(val >= TRESHOLD){
 +
        return 1;
 +
      }
 +
      return 0;
 +
    }
 +
    void fw(double left, double right){
 +
      double scale = 1;
 +
      int lAmmount = 90 - round(left*scale);
 +
      int rAmmount = 90 + round(right*scale);
 +
      Serial.print("move ");
 +
      Serial.print(lAmmount);
 +
      Serial.print(" ");
 +
      Serial.println(rAmmount);
 +
      LeftServo.write(lAmmount); 
 +
      RightServo.write(rAmmount);
 +
    }
 +
    void loop() {
 +
      long leftVal = analogRead(LEFT_SENSOR);
 +
      long rightVal = analogRead(RIGHT_SENSOR);
 +
      long centerVal = analogRead(CENTER_SENSOR);
 +
      long frontVal = analogRead(FRONT_SENSOR);
 +
      Serial.print("left: "); 
 +
      Serial.print(leftVal, DEC); 
 +
      Serial.print(" right: "); 
 +
      Serial.print(rightVal, DEC); 
 +
      Serial.print(" center: "); 
 +
      Serial.print(centerVal, DEC); 
 +
      Serial.print(" front: "); 
 +
      Serial.println(frontVal, DEC); 
 +
      int input[4];
 +
      input[0] = convert(leftVal);
 +
      input[1] = convert(centerVal);
 +
      input[2] = convert(rightVal);
 +
      input[3] = convert(frontVal);
 +
      int action = feed(input);
 +
      Serial.println(ACTION_NAMES[action]);
 +
      double * move = ACTIONS[action];
 +
      double left = move[0] + move[1];
 +
      double right = move[0] - move[1];
 +
      fw(left, right);
 +
      delay(1000);
 +
    }
 +
 
 +
 
 +
[http://www.youtube.com/watch?v=mn_QWL0fBuU Video]
 +
 
 +
----

Revision as of 16:14, 12 June 2013


Backpropagation v Matlabe

   function [J, grad] = nnCostFunction(nn_params, ...
                                          input_layer_size, ...
                                      hidden_layer_size, ...
                                      num_labels, ...
                                      X, y, lambda)
       Theta1 = reshape(nn_params(1:hidden_layer_size * (input_layer_size + 1)), ...
                        hidden_layer_size, (input_layer_size + 1));
       Theta2 = reshape(nn_params((1 + (hidden_layer_size * (input_layer_size + 1))):end), ...
                        num_labels, (hidden_layer_size + 1));
       m = size(X, 1);
       J = 0;
       Theta1_grad = zeros(size(Theta1));
       Theta2_grad = zeros(size(Theta2));
       X = [ones(m, 1) X];
       a1 = X;
       z2 = a1*Theta1';
       a2 = sigmoid(z2);
       a2 = [ones(size(a2,1), 1) a2];
       z3 = a2*Theta2';
       a3 = sigmoid(z3);
       h = a3;
       y1 = zeros(size(y,1), num_labels);
       for i=1:size(y)
           y1(i, y(i)) = 1;
       end
       J = 1/m * sum(sum(((-y1).* log(h) - (1-y1).* log(1-h))));
       temp1 = Theta1;
       temp1(:,1) = zeros(size(temp1,1),1); 
       temp2 = Theta2;
       temp2(:,1) = zeros(size(temp2,1),1); 
       J = J + lambda / (2*m) * ( ...
               sum(sum(temp1 .*temp1)) + sum(sum(temp2.*temp2)) ...
           );
       for t = 1:m
           a1 = X(t,:);
           z2 = a1*Theta1';
           a2 = sigmoid(z2);
           a2 = [ones(size(a2,1), 1) a2];
           z3 = a2*Theta2';
           a3 = sigmoid(z3);
           yk = zeros(num_labels,1); 
           yk(y(t)) = 1;
           d3 = a3' - yk;
           d2 = (Theta2'*d3).*[ones(1,1); sigmoidGradient(z2')];
           d2 = d2(2:end);
           Theta1_grad = Theta1_grad + d2*a1;			
           Theta2_grad = Theta2_grad + d3*a2;
       end
       Theta1_grad = Theta1_grad / m;
       Theta2_grad = Theta2_grad / m;
       Theta1_grad = Theta1_grad + ( (lambda/m) .* [ zeros( hidden_layer_size, 1 ), Theta1( :, 2:end ) ] ); 
       Theta2_grad = Theta2_grad + ( (lambda/m) .* [ zeros( num_labels, 1 ), Theta2( :, 2:end ) ] ); 
       grad = [Theta1_grad(:) ; Theta2_grad(:)];
   end



Java


Video



   #include <Servo.h>
   Servo LeftServo;  
   Servo RightServo;
   double THETA_1[5][5] = {
   {   -3.3015,   -3.2262,    8.4318,   -4.8450,   15.3742},
   {   -4.7470,   -0.3573,   -9.7632,   -9.3562,   18.5839},
   {  -13.2009,    8.7361,    8.5763,    7.5276,   10.6005},
   {    7.0469,  -10.8166,   -9.0304,   -4.0693,   13.2435},
   {    2.6464,   -6.6205,   -2.8116,   11.4895,    8.3321}
   };    
   double THETA_2[5][6] = {
   { -12.1840,   25.3169,   27.4735,  -23.5952,   -0.5421,   -2.3738},
   { -14.6229,   -2.5615,  -26.4061,   -0.6488,   27.8756,    1.7607},
   {  12.8366,  -16.8083,   -3.1984,  -14.8165,  -10.5194,  -16.8826},
   { -17.2910,   -4.2473,  -10.6967,    8.6676,  -24.9129,   25.1679},
   { -12.0564,    3.1205,   -3.9859,   21.9216,   -4.5046,  -25.5430}
   };
   char* ACTION_NAMES[]={
     "ERROR",
     "FORWARD",
     "RIGHT",
     "LEFT",
     "FORWARD RIGHT",
     "FORWARD LEFT"
   };
   double LINE_TRESHOLD = 0.5;
   #define FW_SPEED 1.0
   #define BIG_ROT_AMOUNT 1.0
   #define SLOWDOWN 1
   #define SMALL_ROT_AMOUNT 1
   #define LEFT_SENSOR     3
   #define RIGHT_SENSOR    2
   #define CENTER_SENSOR   0
   #define FRONT_SENSOR    1
   #define TRESHOLD 500
   double ACTIONS[6][2] = {
     {0,0},
     {FW_SPEED, 0},
     {0, -BIG_ROT_AMOUNT },
     {0, BIG_ROT_AMOUNT },
     {/*FW_SPEED / SLOWDOWN*/0, -SMALL_ROT_AMOUNT},
     {/*FW_SPEED / SLOWDOWN*/0, SMALL_ROT_AMOUNT}
   } ;
   #define INPUT_SIZE 4
   #define Euler 2.71828182845904523536
   #define THETA_1_ROWS 5
   #define THETA_1_COLUMNS 5
   #define THETA_2_ROWS 5
   #define THETA_2_COLUMNS 6
   int REPEAT_1[INPUT_SIZE] = {0,0,0,0};
   int REPEAT_2[INPUT_SIZE] = {1,1,1,0};
   int last = 1;
   boolean compareArrays(int  a[], int b[]){
     for(int i =0 ; i < INPUT_SIZE; i++){
       if(a[i] != b[i]){
         return false;
       }
     }
     return true;
   }
   void transpose(double * matrix, double * result , int rr, int cc){
       for(int r =0 ; r <  rr; r++){
        for(int c =0 ; c <  cc; c++){
         result[c * rr  + r] = matrix[r * cc + c];
       }
     }
   }
   int feed(int input[]) {
       if (last != 1 && ( compareArrays(input, REPEAT_1) || compareArrays (input, REPEAT_2)  )) {
           return last;
       }
       double x[1][INPUT_SIZE];
       for (int i = 0; i < INPUT_SIZE; i++) {
           x[0][i] = input[i];
       }
       int m = 1;
       double ones[m][1];
       for (int i = 0; i < m; i++) {
           ones[i][0] = 1;
       }
       double t1t[THETA_1_COLUMNS][THETA_1_ROWS];
       transpose((double*)THETA_1, (double*)t1t , THETA_1_ROWS, THETA_1_COLUMNS);
       double t2t[THETA_2_COLUMNS][THETA_2_ROWS];
       transpose((double*)THETA_2, (double*)t2t, THETA_2_ROWS, THETA_2_COLUMNS);
       double a0[m][INPUT_SIZE+1];
       int params[6];
       params[0] = m;
       params[1] = INPUT_SIZE+1;
       params[2] = m;
       params[3] = 1;
       params[4] = 0;
       params[5] = 0;
       combineMatrix((double*)ones, (double*)a0, params);
       params[2] = m;
       params[3] = INPUT_SIZE;
       params[4] = 0;
       params[5] = 1;
       combineMatrix((double*)x, (double*)a0, params);
       double h1[m][THETA_1_ROWS];
       zeroMatrix((double*)h1, m, THETA_1_ROWS);
       int mparams[4];
       mparams[0] = m;
       mparams[1] = INPUT_SIZE + 1;
       mparams[2] = THETA_1_COLUMNS;
       mparams[3] = THETA_1_ROWS;
       multiply((double*)a0, (double*)t1t, (double*)h1, mparams);
       sigmoid((double*)h1,(double*)h1 , m , THETA_1_ROWS);
       double a1[m][THETA_1_ROWS+1];
       params[0] = m;
       params[1] = THETA_1_ROWS+1;
       params[2] = m;
       params[3] = 1;
       params[4] = 0;
       params[5] = 0;
       combineMatrix((double*)ones, (double*)a1, params);
       params[2] = m;
       params[3] = THETA_1_ROWS;
       params[4] = 0;
       params[5] = 1;
       combineMatrix((double*)h1, (double*)a1, params);
       double h2[m][THETA_2_COLUMNS];
       zeroMatrix((double*)h2, m, THETA_2_ROWS);
       mparams[0] = m;
       mparams[1] = THETA_1_ROWS + 1;
       mparams[2] = THETA_2_COLUMNS;
       mparams[3] = THETA_2_ROWS;
       multiply((double*)a1, (double*)t2t, (double*)h2, mparams);
       sigmoid((double*)h2,(double*)h2 , m , THETA_2_ROWS);
       int actionIndex = maxValue((double*)h2, THETA_2_ROWS * m) + 1;
       last = actionIndex;
       return actionIndex;
   }
    double sigmoid(double * matrix, double * result, int rr, int cc){
        for(int r =0 ; r < rr; r++){
        for(int c =0 ; c < cc; c++){
         result[r*cc + c] = 1.0/(1.0+exp(-matrix[r*cc + c]));
       }
     }
   }
   void copyMatrix(double * matrix, double * result, int rr, int cc){
     int param[6];
     param[0] = rr;
     param[1] = cc;
     param[2] = rr;
     param[3] = cc;
     param[4] = 0;
     param[5] = 0;
     combineMatrix(matrix, result, param);
   }
   void combineMatrix(double * matrix, double * result, int param[6]){
     int lr = param[0];
     int lc = param[1];
     int sr = param[2];
     int sc = param[3];
     int ro = param[4];
     int co = param[5];
     for(int r =0 ; r <  sr; r++){
        for(int c =0 ; c < sc; c++){
         result[(r + ro ) * lc + co + c] = matrix[r * sc + c];
       }
     }
   }
   int maxValue(double* A, int length){
     int mValue = 0;
     for(int i=0; i<length; i++){
       if(A[mValue] <= A[i]){
         mValue = i;
       }
     }
     return mValue;
   } 
   void multiply(double * A, double * B, double * C, int dim[4]) {
         /*int mA = A.length;
         int nA = A[0].length;
         int mB = B.length;
         int nB = A[0].length;*/
         int mA = dim[0];
         int nA = dim[1];
         int mB = dim[2];
         int nB = dim[3];
         if (nA != mB){
               Serial.print("Multiplication error ");
               Serial.print(nA);
               Serial.print(" ");
               Serial.println(mB);
         }
         for (int i = 0; i < mA; i++){
             for (int j = 0; j < nB; j++){
                 for (int k = 0; k < nA; k++){
                      C[i*nB+j] += (A[i*nA+k] * B[k*nB+j]);
                   }
             }
         }
   }
   void printMatrix(double * matrix, int rr, int cc){
     for(int r =0 ; r <  rr; r++){
        for(int c =0 ; c < cc; c++){
         Serial.print(matrix[r*cc+c]);
         Serial.print(" ");
       }
       Serial.println();
     }
       Serial.println();
    }
   void zeroMatrix(double * matrix, int rr, int cc){
     for(int r =0 ; r <  rr; r++){
        for(int c =0 ; c < cc; c++){
         matrix[r*cc+c] = 0;
       }
     }
   }
   void setup() {
      Serial.begin(9600);
      int input[4];
      input[0] = 0;
      input[1] = 0;
      input[2] = 0;
      input[3] = 0;   
      int actionIndex = feed(input);
      Serial.println(actionIndex);
      LeftServo.attach(9);
      RightServo.attach(10);
   }
   int convert(long val){
      if(val >= TRESHOLD){
        return 1;
      }
      return 0;
   }
   void fw(double left, double right){
      double scale = 1;
      int lAmmount = 90 - round(left*scale);
      int rAmmount = 90 + round(right*scale);
      Serial.print("move ");
      Serial.print(lAmmount);
      Serial.print(" ");
      Serial.println(rAmmount);
      LeftServo.write(lAmmount);   
      RightServo.write(rAmmount); 
   }
   void loop() {
     long leftVal = analogRead(LEFT_SENSOR);
     long rightVal = analogRead(RIGHT_SENSOR);
     long centerVal = analogRead(CENTER_SENSOR);
     long frontVal = analogRead(FRONT_SENSOR);
     Serial.print("left: ");  
     Serial.print(leftVal, DEC);  
     Serial.print(" right: ");  
     Serial.print(rightVal, DEC);  
     Serial.print(" center: ");  
     Serial.print(centerVal, DEC);  
     Serial.print(" front: ");  
     Serial.println(frontVal, DEC);  
     int input[4];
     input[0] = convert(leftVal); 
     input[1] = convert(centerVal); 
     input[2] = convert(rightVal); 
     input[3] = convert(frontVal); 
     int action = feed(input);
     Serial.println(ACTION_NAMES[action]);
     double * move = ACTIONS[action];
     double left = move[0] + move[1];
     double right = move[0] - move[1];
     fw(left, right);
     delay(1000);
   }


Video