diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h
index 32fa2cdb66..7bc0647147 100644
--- a/selfdrive/common/modeldata.h
+++ b/selfdrive/common/modeldata.h
@@ -1,11 +1,23 @@
 #pragma once
+
+#include <array>
+
 const int  TRAJECTORY_SIZE = 33;
 const int LAT_MPC_N = 16;
 const int LON_MPC_N = 32;
 const float MIN_DRAW_DISTANCE = 10.0;
 const float MAX_DRAW_DISTANCE = 100.0;
 
-const double T_IDXS[TRAJECTORY_SIZE] = {
+template<typename T_SRC, typename  T_DST, size_t size>
+const std::array<T_DST, size> convert_array_to_type(const std::array<T_SRC, size> &src) {
+  std::array<T_DST, size> dst = {};
+  for (int i=0; i<size; i++) {
+    dst[i] = src[i];
+  }
+  return dst;
+}
+
+const std::array<double, TRAJECTORY_SIZE> T_IDXS = {
         0.        ,  0.00976562,  0.0390625 ,  0.08789062,  0.15625   ,
         0.24414062,  0.3515625 ,  0.47851562,  0.625     ,  0.79101562,
         0.9765625 ,  1.18164062,  1.40625   ,  1.65039062,  1.9140625 ,
@@ -13,7 +25,9 @@ const double T_IDXS[TRAJECTORY_SIZE] = {
         3.90625   ,  4.30664062,  4.7265625 ,  5.16601562,  5.625     ,
         6.10351562,  6.6015625 ,  7.11914062,  7.65625   ,  8.21289062,
         8.7890625 ,  9.38476562, 10.};
-const double X_IDXS[TRAJECTORY_SIZE] = {
+const auto T_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(T_IDXS);
+
+const std::array<double, TRAJECTORY_SIZE> X_IDXS = {
          0.    ,   0.1875,   0.75  ,   1.6875,   3.    ,   4.6875,
          6.75  ,   9.1875,  12.    ,  15.1875,  18.75  ,  22.6875,
         27.    ,  31.6875,  36.75  ,  42.1875,  48.    ,  54.1875,
diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc
index 707f355cb4..bab7038b32 100644
--- a/selfdrive/modeld/models/driving.cc
+++ b/selfdrive/modeld/models/driving.cc
@@ -17,8 +17,6 @@ constexpr int OTHER_META_SIZE = 48;
 constexpr int NUM_META_INTERVALS = 5;
 constexpr int META_STRIDE = 7;
 
-constexpr int PLAN_MHP_N = 5;
-constexpr int PLAN_MHP_COLUMNS = 15;
 constexpr int PLAN_MHP_VALS = 15*33;
 constexpr int PLAN_MHP_SELECTION = 1;
 constexpr int PLAN_MHP_GROUP_SIZE =  (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
@@ -57,6 +55,11 @@ float prev_brake_3ms2_probs[3] = {0,0,0};
 
 // #define DUMP_YUV
 
+template<class T, size_t size>
+constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
+  return kj::ArrayPtr(arr.data(), arr.size());
+}
+
 void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
   s->frame = new ModelFrame(device_id, context);
 
@@ -111,7 +114,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
 
   // net outputs
   ModelDataRaw net_outputs;
-  net_outputs.plan = &s->output[PLAN_IDX];
+  net_outputs.plan = (ModelDataRawPlan*)&s->output[PLAN_IDX];
   net_outputs.lane_lines = &s->output[LL_IDX];
   net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
   net_outputs.road_edges = &s->output[RE_IDX];
@@ -137,10 +140,6 @@ static const float *get_best_data(const float *data, int size, int group_size, i
   return &data[max_idx * group_size];
 }
 
-static const float *get_plan_data(float *plan) {
-  return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, PLAN_MHP_GROUP_SIZE - 1);
-}
-
 static const float *get_lead_data(const float *lead, int t_offset) {
   return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, LEAD_MHP_GROUP_SIZE - LEAD_MHP_SELECTION + t_offset);
 }
@@ -239,6 +238,25 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
   meta.setHardBrakePredicted(above_fcw_threshold);
 }
 
+template<size_t size>
+void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
+  xyzt.setT(to_kj_array_ptr(t));
+  xyzt.setX(to_kj_array_ptr(x));
+  xyzt.setY(to_kj_array_ptr(y));
+  xyzt.setZ(to_kj_array_ptr(z));
+}
+
+template<size_t size>
+void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
+               const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
+               const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
+  fill_xyzt(xyzt, t, x, y, z);
+  xyzt.setXStd(to_kj_array_ptr(x_std));
+  xyzt.setYStd(to_kj_array_ptr(y_std));
+  xyzt.setZStd(to_kj_array_ptr(z_std));
+}
+
 void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float *data, int columns,
                float *plan_t_arr = nullptr, bool fill_std = false) {
   float x_arr[TRAJECTORY_SIZE] = {};
@@ -275,14 +293,45 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float *data, i
   }
 }
 
+void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPath &plan) {
+  std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
+  std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
+  std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
+  std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
+  std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
+
+  for(int i=0; i<TRAJECTORY_SIZE; i++) {
+    pos_x[i] = plan.mean[i].position.x;
+    pos_y[i] = plan.mean[i].position.y;
+    pos_z[i] = plan.mean[i].position.z;
+    pos_x_std[i] = exp(plan.std[i].position.x);
+    pos_y_std[i] = exp(plan.std[i].position.y);
+    pos_z_std[i] = exp(plan.std[i].position.z);
+    vel_x[i] = plan.mean[i].velocity.x;
+    vel_y[i] = plan.mean[i].velocity.y;
+    vel_z[i] = plan.mean[i].velocity.z;
+    rot_x[i] = plan.mean[i].rotation.x;
+    rot_y[i] = plan.mean[i].rotation.y;
+    rot_z[i] = plan.mean[i].rotation.z;
+    rot_rate_x[i] = plan.mean[i].rotation_rate.x;
+    rot_rate_y[i] = plan.mean[i].rotation_rate.y;
+    rot_rate_z[i] = plan.mean[i].rotation_rate.z;
+  }
+
+  fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
+  fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
+  fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
+  fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
+}
+
 void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
-  const float *best_plan = get_plan_data(net_outputs.plan);
+  auto best_plan = net_outputs.plan->best_plan();
   float plan_t_arr[TRAJECTORY_SIZE];
   std::fill_n(plan_t_arr, TRAJECTORY_SIZE, NAN);
   plan_t_arr[0] = 0.0;
   for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
     // increment tidx until we find an element that's further away than the current xidx
-    for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan[next_tid*PLAN_MHP_COLUMNS] < X_IDXS[xidx]; next_tid++) {
+    for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
       tidx++;
     }
     if (tidx == TRAJECTORY_SIZE - 1) {
@@ -292,16 +341,13 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
     }
 
     // interpolate to find `t` for the current xidx
-    float current_x_val = best_plan[tidx * PLAN_MHP_COLUMNS];
-    float next_x_val = best_plan[(tidx+1) * PLAN_MHP_COLUMNS];
+    float current_x_val = best_plan.mean[tidx].position.x;
+    float next_x_val = best_plan.mean[tidx+1].position.x;
     float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
     plan_t_arr[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
   }
 
-  fill_xyzt(framed.initPosition(), &best_plan[0], PLAN_MHP_COLUMNS, nullptr, true);
-  fill_xyzt(framed.initVelocity(), &best_plan[3], PLAN_MHP_COLUMNS);
-  fill_xyzt(framed.initOrientation(), &best_plan[9], PLAN_MHP_COLUMNS);
-  fill_xyzt(framed.initOrientationRate(), &best_plan[12], PLAN_MHP_COLUMNS);
+  fill_plan(framed, best_plan);
 
   // lane lines
   auto lane_lines = framed.initLaneLines(4);
@@ -355,25 +401,17 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
 
 void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
                      const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
-  float trans_arr[3];
-  float trans_std_arr[3];
-  float rot_arr[3];
-  float rot_std_arr[3];
-
-  for (int i =0; i < 3; i++) {
-    trans_arr[i] = net_outputs.pose->trans_arr[i];
-    trans_std_arr[i] = exp(net_outputs.pose->trans_std_arr[i]);
-
-    rot_arr[i] = net_outputs.pose->rot_arr[i];
-    rot_std_arr[i] = exp(net_outputs.pose->rot_std_arr[i]);
-  }
-
   MessageBuilder msg;
+  auto v_mean = net_outputs.pose->velocity_mean;
+  auto r_mean = net_outputs.pose->rotation_mean;
+  auto v_std = net_outputs.pose->velocity_std;
+  auto r_std = net_outputs.pose->rotation_std;
+
   auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
-  posenetd.setTrans(trans_arr);
-  posenetd.setRot(rot_arr);
-  posenetd.setTransStd(trans_std_arr);
-  posenetd.setRotStd(rot_std_arr);
+  posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
+  posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
+  posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
+  posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
 
   posenetd.setTimestampEof(timestamp_eof);
   posenetd.setFrameId(vipc_frame_id);
diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h
index d5fe5e8e33..14f31fff68 100644
--- a/selfdrive/modeld/models/driving.h
+++ b/selfdrive/modeld/models/driving.h
@@ -5,6 +5,7 @@
 #define DESIRE
 #define TRAFFIC_CONVENTION
 
+#include <array>
 #include <memory>
 
 #include "cereal/messaging/messaging.h"
@@ -14,19 +15,60 @@
 #include "selfdrive/modeld/models/commonmodel.h"
 #include "selfdrive/modeld/runners/run.h"
 
+constexpr int PLAN_MHP_N = 5;
+
 constexpr int DESIRE_LEN = 8;
 constexpr int TRAFFIC_CONVENTION_LEN = 2;
 constexpr int MODEL_FREQ = 20;
 
+struct ModelDataRawXYZ {
+  float x;
+  float y;
+  float z;
+};
+static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
+
+struct ModelDataRawPlanTimeStep {
+  ModelDataRawXYZ position;
+  ModelDataRawXYZ velocity;
+  ModelDataRawXYZ acceleration;
+  ModelDataRawXYZ rotation;
+  ModelDataRawXYZ rotation_rate;
+};
+static_assert(sizeof(ModelDataRawPlanTimeStep) == sizeof(ModelDataRawXYZ)*5);
+
+struct ModelDataRawPlanPath {
+  std::array<ModelDataRawPlanTimeStep, TRAJECTORY_SIZE> mean;
+  std::array<ModelDataRawPlanTimeStep, TRAJECTORY_SIZE> std;
+  float prob;
+};
+static_assert(sizeof(ModelDataRawPlanPath) == (sizeof(ModelDataRawPlanTimeStep)*TRAJECTORY_SIZE*2) + sizeof(float));
+
+struct ModelDataRawPlan {
+  std::array<ModelDataRawPlanPath, PLAN_MHP_N> path;
+
+  constexpr const ModelDataRawPlanPath &best_plan() const {
+    int max_idx = 0;
+    for (int i = 1; i < path.size(); i++) {
+      if (path[i].prob > path[max_idx].prob) {
+        max_idx = i;
+      }
+    }
+    return path[max_idx];
+  }
+};
+static_assert(sizeof(ModelDataRawPlan) == sizeof(ModelDataRawPlanPath)*PLAN_MHP_N);
+
 struct ModelDataRawPose {
-  float trans_arr[3];
-  float rot_arr[3];
-  float trans_std_arr[3];
-  float rot_std_arr[3];
+  ModelDataRawXYZ velocity_mean;
+  ModelDataRawXYZ rotation_mean;
+  ModelDataRawXYZ velocity_std;
+  ModelDataRawXYZ rotation_std;
 };
+static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
 
 struct ModelDataRaw {
-  float *plan;
+  ModelDataRawPlan *plan;
   float *lane_lines;
   float *lane_lines_prob;
   float *road_edges;