Do multiple task on Arduino Mega

Hi everybody, I'm trying to run multiple task on my Arduino Mega board, in order to accomplish indoor pedestrian localization. I've got an Adafruit 10-DOF IMU useful to acquire acceleration and gyroscope data. I would like to read data at 100Hz frequency (100 sample/sec) and the processing data all inside my board. Here's my application logic:

/*... some init variables ...*/

void processingAlgorithm(v_accx, v_accy, v_accz, v_gyrox, v_gyroy, v_gyroz, timer){
  /* some code here */
}

void setup() {
  Serial.begin(9600);
    /* Sensors initialisation */
  if (!accel.begin()){
    Serial.print("Error during accelerometer initialisation");
    while (1);
  }
  if (!mag.begin()){
    Serial.print("Error during magnetometer initialisation");
    while (1);
  }
  if (!bmp.begin()){
    Serial.print("Error during bmp initialisation");
    while (1);
  }
  if (!gyro.begin()){
    Serial.print("Error during gyroscope initialisation");
    while (1);
  }
}

void loop(){
 /* Get a new sensor event */
  sensors_event_t event;
  /* Display the results (acceleration is measured in m/s^2) */
  accel.getEvent(&event);
  /* Display the results (gyroscope values in rad/s) */
  gyro.getEvent(&event);

  v_accx(index) = event.acceleration.x;
  v_accy(index) = event.acceleration.y;
  v_accz(index) = event.acceleration.z;
  v_gyrox(index) = event.gyro.x;
  v_gyroy(index) = event.gyro.y;
  v_gyroz(index) = event.gyro.z;
  timer(index) =  millis()*0.001; 
  
  if(indice == 100){
    processingAlgorithm(v_accx, v_accy, v_accz, v_gyrox, v_gyroy, v_gyroz, timer);
  }else{
    index = index+1;
  }
}

I think my logic is wrong, because I'm not able to acquire at 100Hz and I don't know why. Hope someone will help. Thank you. Giacomo.

// some answer here in the event of seeing more details

Here my full processing algorithm, in order to give you so much more details.

void processingAlgorithm(const Eigen::RowVectorXd& v_accx, const Eigen::RowVectorXd& v_accy, const Eigen::RowVectorXd& v_accz, const Eigen::RowVectorXd& v_gyrox, const Eigen::RowVectorXd& v_gyroy, const Eigen::RowVectorXd& v_gyroz,const Eigen::RowVectorXd& timer){
  /* Gyroscope bias, to be determined for each sensor */
  Vector3d gyro_bias;
  gyro_bias << -0.0156,
               -0.0101,
               -0.0020;  

  /* gravity */             
  double gravity = 9.8000;

  /* gravity vector */
  Vector3d g;
  g << 0,
       0,
       gravity; 

  /* gyroscope stance phase detection threshold */
  double gyro_threshold = 0.6;
  
  /* Orientation from accelerometers. Sensor is assumed to be stationary */
  pitch = -asin(v_accx(0)/gravity);
  roll = atan(v_accy(0) / v_accz(0));
  yaw = 0;
  
  //Orientation matrix C initialization
  Matrix3d C;
  C << cos(pitch)*cos(yaw), (sin(roll)*sin(pitch)*cos(yaw))-(cos(roll)*sin(yaw)), ((cos(roll)*sin(pitch)*cos(yaw)) + (sin(roll)*sin(yaw))),
       cos(pitch)*sin(yaw), (sin(roll)*sin(pitch)*sin(yaw)) + (cos(roll)*cos(yaw)), (cos(roll)*sin(pitch)*sin(yaw)) - (sin(roll)*cos(yaw)),
      -sin(pitch), sin(roll)*cos(pitch), cos(roll)*cos(pitch);
  
  Matrix3d C_prev(3,3);
  C_prev = C;

  /* Accelerations in sensor frame */
  MatrixXd acc_s(3,data_size); 
  acc_s = MatrixXd::Zero(3,data_size);
  acc_s << v_accx,
           v_accy,
           v_accz;

  /* Rates of turn in sensor frame */
  MatrixXd gyros(3, data_size);
  gyros << v_gyrox, 
           v_gyroy, 
           v_gyroz;

  /* Preallocate storage for accelerations in navigation frame */
  MatrixXd acc_n(3,data_size);
  acc_n = MatrixXd::Zero(3,data_size);
  acc_n.col(0) = C*acc_s.col(0);

  /* Error covariance matrix */
  MatrixXd P(9,9);
  P = MatrixXd::Zero(9,9);

  /* Process noise parameter, gyroscope and accelerometer noise */
  double sigma_omega = 1 * pow(10, -2);
  double sigma_a = 1 * pow(10, -2);

  /* ZUPT measurement matrix */
  MatrixXd H(3,9);
  H << 0, 0, 0, 0, 0, 0, 1, 0, 0,
       0, 0, 0, 0, 0, 0, 0, 1, 0,
       0, 0, 0, 0, 0, 0, 0, 0, 1;

  /* ZUPT measurement noise covariance matrix */
  double sigma_v = 1 * pow(10, -2);
  
  Matrix3d R(3, 3);
  R << sigma_v, 0, 0,
       0, sigma_v, 0,
       0, 0, sigma_v;
  
  Matrix3d R1(3, 3);
  R1 = R * R;
  R = R1;
  
  /************* MAIN LOOP *************/
  for (int t = 1; t < data_size; t++) {
    /******* Starts INS (trasformation, double integration) *******/
    float dt = timer(t) - timer(t-1);
    Serial.println("Il dt è:"+String(dt));

    /* Remove bias from gyro measurements */
    VectorXd gyro_s1(3);
    gyro_s1 << gyros.col(t) - gyro_bias; 

    /* Skew-symmetric matrix for angular rates */
    Matrix3d ang_rate_matrix(3, 3);
    ang_rate_matrix << 0, -gyro_s1(2), gyro_s1(1),
                       gyro_s1(2), 0, -gyro_s1(0),
                       -gyro_s1(1), gyro_s1(0), 0; 

    /* Identity Matrix (3x3) - (9x9) */
    Matrix3d identity_3;
    identity_3 = MatrixXd::Identity(3,3);
    Matrix3d identity_9;
    identity_9 = MatrixXd::Identity(9,9);
               
    /* Orientation estimation */
    Matrix3d numeratore;
    numeratore = C_prev*((2*identity_3)+(ang_rate_matrix*dt)); 
    Matrix3d denominatore;
    denominatore = ((2*identity_3)-(ang_rate_matrix*dt)); 
       
    C = (numeratore) * (denominatore.inverse());

    /* Traforming the acceleration from sensor frame to navigation frame */
    acc_n.col(t) = 0.5 *(C+C_prev) * acc_s.col(t);

    /* Velocity and position estimation using trapeze integration */
    vel_n.col(t) = vel_n.col(t-1) + (((acc_n.col(t) - g) + (acc_n.col(t-1) - g))*(dt/2));
    pos_n.col(t) = pos_n.col(t-1) + (vel_n.col(t)+vel_n.col(t-1))*dt/2;

    /* State transition matrix */
    MatrixXd Fi(9, 9);
    Fi << 1, 0, 0, 0, 0, 0, 0, 0, 0,
          0, 1, 0, 0, 0, 0, 0, 0, 0,
          0, 0, 1, 0, 0, 0, 0, 0, 0,
          0, 0, 0, 1, 0, 0, dt, 0, 0,
          0, 0, 0, 0, 1, 0, 0, dt, 0,
          0, 0, 0, 0, 0, 1, 0, 0, dt,
          0, -dt*(-acc_n(2,t)), -dt*(acc_n(1,t)), 0, 0, 0, 1, 0, 0,
          -dt*(acc_n(2,t)), 0, -dt*(-acc_n(0,t)), 0, 0, 0, 0, 1, 0,
          -dt*(-acc_n(1,t)), -dt*(acc_n(0,t)), 0, 0, 0, 0, 0, 0, 1; 
    
    MatrixXd FT(9,9);
    FT = Fi.transpose();

    /* Compute the process noise covariance Q */
    MatrixXd Q(9,9);
    Q << pow(sigma_omega*dt,2),0,0,0,0,0,0,0,0,
         0,pow(sigma_omega*dt,2),0,0,0,0,0,0,0,
         0,0,pow(sigma_omega*dt,2),0,0,0,0,0,0,
         0,0,0,0,0,0,0,0,0,
         0,0,0,0,0,0,0,0,0,
         0,0,0,0,0,0,0,0,0,
         0,0,0,0,0,0,pow(sigma_a*dt,2),0,0,
         0,0,0,0,0,0,0,pow(sigma_a*dt,2),0,
         0,0,0,0,0,0,0,0,pow(sigma_a*dt,2);

    /* Propagate the error covariance matrix */
    MatrixXd P_temp1(9,9);
    P_temp1 = multiplication(Fi,P);
    MatrixXd P_temp2(9,9);
    P_temp2 = multiplication(P_temp1,FT);
    P = P_temp2 + Q;
    /******* END INS *******/
    /* Stance phase detection and zero velocity updates */
    float norm = gyros.col(t).norm();
    
    if (norm < gyro_threshold) {
      
      /* Start Kalman filter zero-velocity update */
      /* Kalman gain */
      MatrixXd K_temp1(9,3);
      K_temp1 = multiplication(P,H.transpose());
      MatrixXd K_temp2(3,9);
      K_temp2 = multiplication(H,P);   
      Matrix3d K_temp3;
      K_temp3 = multiplication(K_temp2,H.transpose()); 
      Matrix3d K_temp4;
      K_temp4 = K_temp3 + R; 
      Matrix3d K_temp5;
      K_temp5 = K_temp4.inverse();
      MatrixXd K(9,3);
      K = multiplication(K_temp1,K_temp5);

      /* Update the filter state */
      VectorXd delta_x(9);
      delta_x = multiplication(K,vel_n.col(t));

      /* Update the error covariance matrix */
      MatrixXd P2_temp1(9,9);
      P2_temp1 = multiplication(K,H);
      MatrixXd P2_temp2(9,9);
      P2_temp2 = identity_9 - P2_temp1;
      P = multiplication(P2_temp2,P);

      /* Extract errors from KF state */
      Vector3d attitude_error;
      attitude_error << delta_x(0),
                        delta_x(1),
                        delta_x(2);

      Vector3d pos_error;
      pos_error << delta_x(3),
                   delta_x(4),
                   delta_x(5);

      Vector3d vel_error;
      vel_error  << delta_x(6),
                    delta_x(7),
                    delta_x(8);
      /* End Kalman filter zero-velocity update */
      
      /* Apply corrections to INS estimates */
      /* Skew-symmetric matrix for small angles to correct orientation */
      Matrix3d ang_matrix;
      ang_matrix <<  0, attitude_error(2), -attitude_error(1),
                    -attitude_error(2), 0, attitude_error(0),
                     attitude_error(1), -attitude_error(0), 0;

      /* Correct orientation */
      Matrix3d C_part1;
      C_part1 = (2 * identity_3) + ang_matrix;
      Matrix3d C_part2;
      C_part2 = (2 * identity_3) - ang_matrix;
      Matrix3d C_part3;
      C_part3 = C_part1 * C_part2.inverse();
      C = C_part3 * C;

      /* Correct position and velocity base on Kalman error estimates */
      vel_n.col(t) = vel_n.col(t) - vel_error;
      pos_n.col(t) = pos_n.col(t) - pos_error;
    }
    
    C_prev = C; /* Save orientation estimate, required at start of main loop */

    /* Compute horizontal distance */
    float dis_temp1 = (pos_n(0,t)-pos_n(0,t-1));
    float dis_temp2 = (pos_n(1,t)-pos_n(1,t-1));
    distance(0,t) = distance(0,t-1) + sqrt(pow(dis_temp1,2)+pow(dis_temp2,2));
  }

  Serial.println("Pos_n:");
  print_mat(pos_n);
  Serial.println("Distance:");
  print_mat(distance);  
  Serial.println("Finita iterazione");
  
}

Here my full processing algorithm,

I'm pretty sure that won't compile.

Serial.begin(9600);

That could be limiting.

Please post the entire program so we can see everything that you can see.

What multiple tasks are you trying to run ?

...R