message: Integrated velocity into state space
diff: @@ -66,7 +66,7 @@ dynamic::Trajectory DynamicBehaviorModel::Plan(
  traj(i, 1) = next_state(6);
  traj(i, 2) = next_state(9);
  traj(i, 3) = atan2(next_state(10), next_state(7));
  - traj(i, 4) = 0; // next_state(7);
  + traj(i, 4) = sqrt(pow(next_state(7), 2) + pow(next_state(10), 2));
  }
  }
  