sunnypilot v2026.09.04-4849

version: sunnypilot v2026.003.000 (feature-branch) date: 2026-09-04T22:56:57 master commit: b70920a8c0
sunnypilot v2026.09.04-4849 release
2026-09-06 18:33:42 +08:00 · 2026-09-04 22:56:57 +00:00 · 2026-09-04 22:56:47 +00:00
25 changed files with 3601 additions and 3380 deletions
@@ -0,0 +1,11 @@
+*       @sunnypilot/dev-internal
+/.github/ @devtekve @sunnyhaibin
+/release/ci/ @devtekve @sunnyhaibin
+/tinygrad_repo @devtekve @Discountchubbs
+/tinygrad/ @devtekve @Discountchubbs
+/selfdrive/controls/lib/longitudinal_planner.py @devtekve @Discountchubbs
+/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @devtekve @Discountchubbs
+/selfdrive/modeld/ @devtekve @Discountchubbs
+/sunnypilot/model* @devtekve @Discountchubbs
+/sunnypilot/sunnylink/ @devtekve
+/system/athena/ @devtekve
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
 *                                                                            *
 *                         This file is part of 'ekf'                         *
 ******************************************************************************/
-void err_fun(double *nom_x, double *delta_x, double *out_205475353292099530) {
-   out_205475353292099530[0] = delta_x[0] + nom_x[0];
-   out_205475353292099530[1] = delta_x[1] + nom_x[1];
-   out_205475353292099530[2] = delta_x[2] + nom_x[2];
-   out_205475353292099530[3] = delta_x[3] + nom_x[3];
-   out_205475353292099530[4] = delta_x[4] + nom_x[4];
-   out_205475353292099530[5] = delta_x[5] + nom_x[5];
-   out_205475353292099530[6] = delta_x[6] + nom_x[6];
-   out_205475353292099530[7] = delta_x[7] + nom_x[7];
-   out_205475353292099530[8] = delta_x[8] + nom_x[8];
+void err_fun(double *nom_x, double *delta_x, double *out_5744499419236567507) {
+   out_5744499419236567507[0] = delta_x[0] + nom_x[0];
+   out_5744499419236567507[1] = delta_x[1] + nom_x[1];
+   out_5744499419236567507[2] = delta_x[2] + nom_x[2];
+   out_5744499419236567507[3] = delta_x[3] + nom_x[3];
+   out_5744499419236567507[4] = delta_x[4] + nom_x[4];
+   out_5744499419236567507[5] = delta_x[5] + nom_x[5];
+   out_5744499419236567507[6] = delta_x[6] + nom_x[6];
+   out_5744499419236567507[7] = delta_x[7] + nom_x[7];
+   out_5744499419236567507[8] = delta_x[8] + nom_x[8];
 }
-void inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341) {
-   out_6859088956493527341[0] = -nom_x[0] + true_x[0];
-   out_6859088956493527341[1] = -nom_x[1] + true_x[1];
-   out_6859088956493527341[2] = -nom_x[2] + true_x[2];
-   out_6859088956493527341[3] = -nom_x[3] + true_x[3];
-   out_6859088956493527341[4] = -nom_x[4] + true_x[4];
-   out_6859088956493527341[5] = -nom_x[5] + true_x[5];
-   out_6859088956493527341[6] = -nom_x[6] + true_x[6];
-   out_6859088956493527341[7] = -nom_x[7] + true_x[7];
-   out_6859088956493527341[8] = -nom_x[8] + true_x[8];
+void inv_err_fun(double *nom_x, double *true_x, double *out_1184805137205037163) {
+   out_1184805137205037163[0] = -nom_x[0] + true_x[0];
+   out_1184805137205037163[1] = -nom_x[1] + true_x[1];
+   out_1184805137205037163[2] = -nom_x[2] + true_x[2];
+   out_1184805137205037163[3] = -nom_x[3] + true_x[3];
+   out_1184805137205037163[4] = -nom_x[4] + true_x[4];
+   out_1184805137205037163[5] = -nom_x[5] + true_x[5];
+   out_1184805137205037163[6] = -nom_x[6] + true_x[6];
+   out_1184805137205037163[7] = -nom_x[7] + true_x[7];
+   out_1184805137205037163[8] = -nom_x[8] + true_x[8];
 }
-void H_mod_fun(double *state, double *out_8201686483145055601) {
-   out_8201686483145055601[0] = 1.0;
-   out_8201686483145055601[1] = 0.0;
-   out_8201686483145055601[2] = 0.0;
-   out_8201686483145055601[3] = 0.0;
-   out_8201686483145055601[4] = 0.0;
-   out_8201686483145055601[5] = 0.0;
-   out_8201686483145055601[6] = 0.0;
-   out_8201686483145055601[7] = 0.0;
-   out_8201686483145055601[8] = 0.0;
-   out_8201686483145055601[9] = 0.0;
-   out_8201686483145055601[10] = 1.0;
-   out_8201686483145055601[11] = 0.0;
-   out_8201686483145055601[12] = 0.0;
-   out_8201686483145055601[13] = 0.0;
-   out_8201686483145055601[14] = 0.0;
-   out_8201686483145055601[15] = 0.0;
-   out_8201686483145055601[16] = 0.0;
-   out_8201686483145055601[17] = 0.0;
-   out_8201686483145055601[18] = 0.0;
-   out_8201686483145055601[19] = 0.0;
-   out_8201686483145055601[20] = 1.0;
-   out_8201686483145055601[21] = 0.0;
-   out_8201686483145055601[22] = 0.0;
-   out_8201686483145055601[23] = 0.0;
-   out_8201686483145055601[24] = 0.0;
-   out_8201686483145055601[25] = 0.0;
-   out_8201686483145055601[26] = 0.0;
-   out_8201686483145055601[27] = 0.0;
-   out_8201686483145055601[28] = 0.0;
-   out_8201686483145055601[29] = 0.0;
-   out_8201686483145055601[30] = 1.0;
-   out_8201686483145055601[31] = 0.0;
-   out_8201686483145055601[32] = 0.0;
-   out_8201686483145055601[33] = 0.0;
-   out_8201686483145055601[34] = 0.0;
-   out_8201686483145055601[35] = 0.0;
-   out_8201686483145055601[36] = 0.0;
-   out_8201686483145055601[37] = 0.0;
-   out_8201686483145055601[38] = 0.0;
-   out_8201686483145055601[39] = 0.0;
-   out_8201686483145055601[40] = 1.0;
-   out_8201686483145055601[41] = 0.0;
-   out_8201686483145055601[42] = 0.0;
-   out_8201686483145055601[43] = 0.0;
-   out_8201686483145055601[44] = 0.0;
-   out_8201686483145055601[45] = 0.0;
-   out_8201686483145055601[46] = 0.0;
-   out_8201686483145055601[47] = 0.0;
-   out_8201686483145055601[48] = 0.0;
-   out_8201686483145055601[49] = 0.0;
-   out_8201686483145055601[50] = 1.0;
-   out_8201686483145055601[51] = 0.0;
-   out_8201686483145055601[52] = 0.0;
-   out_8201686483145055601[53] = 0.0;
-   out_8201686483145055601[54] = 0.0;
-   out_8201686483145055601[55] = 0.0;
-   out_8201686483145055601[56] = 0.0;
-   out_8201686483145055601[57] = 0.0;
-   out_8201686483145055601[58] = 0.0;
-   out_8201686483145055601[59] = 0.0;
-   out_8201686483145055601[60] = 1.0;
-   out_8201686483145055601[61] = 0.0;
-   out_8201686483145055601[62] = 0.0;
-   out_8201686483145055601[63] = 0.0;
-   out_8201686483145055601[64] = 0.0;
-   out_8201686483145055601[65] = 0.0;
-   out_8201686483145055601[66] = 0.0;
-   out_8201686483145055601[67] = 0.0;
-   out_8201686483145055601[68] = 0.0;
-   out_8201686483145055601[69] = 0.0;
-   out_8201686483145055601[70] = 1.0;
-   out_8201686483145055601[71] = 0.0;
-   out_8201686483145055601[72] = 0.0;
-   out_8201686483145055601[73] = 0.0;
-   out_8201686483145055601[74] = 0.0;
-   out_8201686483145055601[75] = 0.0;
-   out_8201686483145055601[76] = 0.0;
-   out_8201686483145055601[77] = 0.0;
-   out_8201686483145055601[78] = 0.0;
-   out_8201686483145055601[79] = 0.0;
-   out_8201686483145055601[80] = 1.0;
+void H_mod_fun(double *state, double *out_2752971634021862652) {
+   out_2752971634021862652[0] = 1.0;
+   out_2752971634021862652[1] = 0.0;
+   out_2752971634021862652[2] = 0.0;
+   out_2752971634021862652[3] = 0.0;
+   out_2752971634021862652[4] = 0.0;
+   out_2752971634021862652[5] = 0.0;
+   out_2752971634021862652[6] = 0.0;
+   out_2752971634021862652[7] = 0.0;
+   out_2752971634021862652[8] = 0.0;
+   out_2752971634021862652[9] = 0.0;
+   out_2752971634021862652[10] = 1.0;
+   out_2752971634021862652[11] = 0.0;
+   out_2752971634021862652[12] = 0.0;
+   out_2752971634021862652[13] = 0.0;
+   out_2752971634021862652[14] = 0.0;
+   out_2752971634021862652[15] = 0.0;
+   out_2752971634021862652[16] = 0.0;
+   out_2752971634021862652[17] = 0.0;
+   out_2752971634021862652[18] = 0.0;
+   out_2752971634021862652[19] = 0.0;
+   out_2752971634021862652[20] = 1.0;
+   out_2752971634021862652[21] = 0.0;
+   out_2752971634021862652[22] = 0.0;
+   out_2752971634021862652[23] = 0.0;
+   out_2752971634021862652[24] = 0.0;
+   out_2752971634021862652[25] = 0.0;
+   out_2752971634021862652[26] = 0.0;
+   out_2752971634021862652[27] = 0.0;
+   out_2752971634021862652[28] = 0.0;
+   out_2752971634021862652[29] = 0.0;
+   out_2752971634021862652[30] = 1.0;
+   out_2752971634021862652[31] = 0.0;
+   out_2752971634021862652[32] = 0.0;
+   out_2752971634021862652[33] = 0.0;
+   out_2752971634021862652[34] = 0.0;
+   out_2752971634021862652[35] = 0.0;
+   out_2752971634021862652[36] = 0.0;
+   out_2752971634021862652[37] = 0.0;
+   out_2752971634021862652[38] = 0.0;
+   out_2752971634021862652[39] = 0.0;
+   out_2752971634021862652[40] = 1.0;
+   out_2752971634021862652[41] = 0.0;
+   out_2752971634021862652[42] = 0.0;
+   out_2752971634021862652[43] = 0.0;
+   out_2752971634021862652[44] = 0.0;
+   out_2752971634021862652[45] = 0.0;
+   out_2752971634021862652[46] = 0.0;
+   out_2752971634021862652[47] = 0.0;
+   out_2752971634021862652[48] = 0.0;
+   out_2752971634021862652[49] = 0.0;
+   out_2752971634021862652[50] = 1.0;
+   out_2752971634021862652[51] = 0.0;
+   out_2752971634021862652[52] = 0.0;
+   out_2752971634021862652[53] = 0.0;
+   out_2752971634021862652[54] = 0.0;
+   out_2752971634021862652[55] = 0.0;
+   out_2752971634021862652[56] = 0.0;
+   out_2752971634021862652[57] = 0.0;
+   out_2752971634021862652[58] = 0.0;
+   out_2752971634021862652[59] = 0.0;
+   out_2752971634021862652[60] = 1.0;
+   out_2752971634021862652[61] = 0.0;
+   out_2752971634021862652[62] = 0.0;
+   out_2752971634021862652[63] = 0.0;
+   out_2752971634021862652[64] = 0.0;
+   out_2752971634021862652[65] = 0.0;
+   out_2752971634021862652[66] = 0.0;
+   out_2752971634021862652[67] = 0.0;
+   out_2752971634021862652[68] = 0.0;
+   out_2752971634021862652[69] = 0.0;
+   out_2752971634021862652[70] = 1.0;
+   out_2752971634021862652[71] = 0.0;
+   out_2752971634021862652[72] = 0.0;
+   out_2752971634021862652[73] = 0.0;
+   out_2752971634021862652[74] = 0.0;
+   out_2752971634021862652[75] = 0.0;
+   out_2752971634021862652[76] = 0.0;
+   out_2752971634021862652[77] = 0.0;
+   out_2752971634021862652[78] = 0.0;
+   out_2752971634021862652[79] = 0.0;
+   out_2752971634021862652[80] = 1.0;
 }
-void f_fun(double *state, double dt, double *out_2204957494938772746) {
-   out_2204957494938772746[0] = state[0];
-   out_2204957494938772746[1] = state[1];
-   out_2204957494938772746[2] = state[2];
-   out_2204957494938772746[3] = state[3];
-   out_2204957494938772746[4] = state[4];
-   out_2204957494938772746[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
-   out_2204957494938772746[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
-   out_2204957494938772746[7] = state[7];
-   out_2204957494938772746[8] = state[8];
+void f_fun(double *state, double dt, double *out_7658700760911629364) {
+   out_7658700760911629364[0] = state[0];
+   out_7658700760911629364[1] = state[1];
+   out_7658700760911629364[2] = state[2];
+   out_7658700760911629364[3] = state[3];
+   out_7658700760911629364[4] = state[4];
+   out_7658700760911629364[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
+   out_7658700760911629364[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
+   out_7658700760911629364[7] = state[7];
+   out_7658700760911629364[8] = state[8];
 }
-void F_fun(double *state, double dt, double *out_2327939745960551937) {
-   out_2327939745960551937[0] = 1;
-   out_2327939745960551937[1] = 0;
-   out_2327939745960551937[2] = 0;
-   out_2327939745960551937[3] = 0;
-   out_2327939745960551937[4] = 0;
-   out_2327939745960551937[5] = 0;
-   out_2327939745960551937[6] = 0;
-   out_2327939745960551937[7] = 0;
-   out_2327939745960551937[8] = 0;
-   out_2327939745960551937[9] = 0;
-   out_2327939745960551937[10] = 1;
-   out_2327939745960551937[11] = 0;
-   out_2327939745960551937[12] = 0;
-   out_2327939745960551937[13] = 0;
-   out_2327939745960551937[14] = 0;
-   out_2327939745960551937[15] = 0;
-   out_2327939745960551937[16] = 0;
-   out_2327939745960551937[17] = 0;
-   out_2327939745960551937[18] = 0;
-   out_2327939745960551937[19] = 0;
-   out_2327939745960551937[20] = 1;
-   out_2327939745960551937[21] = 0;
-   out_2327939745960551937[22] = 0;
-   out_2327939745960551937[23] = 0;
-   out_2327939745960551937[24] = 0;
-   out_2327939745960551937[25] = 0;
-   out_2327939745960551937[26] = 0;
-   out_2327939745960551937[27] = 0;
-   out_2327939745960551937[28] = 0;
-   out_2327939745960551937[29] = 0;
-   out_2327939745960551937[30] = 1;
-   out_2327939745960551937[31] = 0;
-   out_2327939745960551937[32] = 0;
-   out_2327939745960551937[33] = 0;
-   out_2327939745960551937[34] = 0;
-   out_2327939745960551937[35] = 0;
-   out_2327939745960551937[36] = 0;
-   out_2327939745960551937[37] = 0;
-   out_2327939745960551937[38] = 0;
-   out_2327939745960551937[39] = 0;
-   out_2327939745960551937[40] = 1;
-   out_2327939745960551937[41] = 0;
-   out_2327939745960551937[42] = 0;
-   out_2327939745960551937[43] = 0;
-   out_2327939745960551937[44] = 0;
-   out_2327939745960551937[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
-   out_2327939745960551937[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
-   out_2327939745960551937[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
-   out_2327939745960551937[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
-   out_2327939745960551937[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
-   out_2327939745960551937[52] = dt*stiffness_front*state[0]/(mass*state[1]);
-   out_2327939745960551937[53] = -9.8100000000000005*dt;
-   out_2327939745960551937[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
-   out_2327939745960551937[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
-   out_2327939745960551937[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
-   out_2327939745960551937[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
-   out_2327939745960551937[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
-   out_2327939745960551937[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
-   out_2327939745960551937[62] = 0;
-   out_2327939745960551937[63] = 0;
-   out_2327939745960551937[64] = 0;
-   out_2327939745960551937[65] = 0;
-   out_2327939745960551937[66] = 0;
-   out_2327939745960551937[67] = 0;
-   out_2327939745960551937[68] = 0;
-   out_2327939745960551937[69] = 0;
-   out_2327939745960551937[70] = 1;
-   out_2327939745960551937[71] = 0;
-   out_2327939745960551937[72] = 0;
-   out_2327939745960551937[73] = 0;
-   out_2327939745960551937[74] = 0;
-   out_2327939745960551937[75] = 0;
-   out_2327939745960551937[76] = 0;
-   out_2327939745960551937[77] = 0;
-   out_2327939745960551937[78] = 0;
-   out_2327939745960551937[79] = 0;
-   out_2327939745960551937[80] = 1;
+void F_fun(double *state, double dt, double *out_6351856658156694456) {
+   out_6351856658156694456[0] = 1;
+   out_6351856658156694456[1] = 0;
+   out_6351856658156694456[2] = 0;
+   out_6351856658156694456[3] = 0;
+   out_6351856658156694456[4] = 0;
+   out_6351856658156694456[5] = 0;
+   out_6351856658156694456[6] = 0;
+   out_6351856658156694456[7] = 0;
+   out_6351856658156694456[8] = 0;
+   out_6351856658156694456[9] = 0;
+   out_6351856658156694456[10] = 1;
+   out_6351856658156694456[11] = 0;
+   out_6351856658156694456[12] = 0;
+   out_6351856658156694456[13] = 0;
+   out_6351856658156694456[14] = 0;
+   out_6351856658156694456[15] = 0;
+   out_6351856658156694456[16] = 0;
+   out_6351856658156694456[17] = 0;
+   out_6351856658156694456[18] = 0;
+   out_6351856658156694456[19] = 0;
+   out_6351856658156694456[20] = 1;
+   out_6351856658156694456[21] = 0;
+   out_6351856658156694456[22] = 0;
+   out_6351856658156694456[23] = 0;
+   out_6351856658156694456[24] = 0;
+   out_6351856658156694456[25] = 0;
+   out_6351856658156694456[26] = 0;
+   out_6351856658156694456[27] = 0;
+   out_6351856658156694456[28] = 0;
+   out_6351856658156694456[29] = 0;
+   out_6351856658156694456[30] = 1;
+   out_6351856658156694456[31] = 0;
+   out_6351856658156694456[32] = 0;
+   out_6351856658156694456[33] = 0;
+   out_6351856658156694456[34] = 0;
+   out_6351856658156694456[35] = 0;
+   out_6351856658156694456[36] = 0;
+   out_6351856658156694456[37] = 0;
+   out_6351856658156694456[38] = 0;
+   out_6351856658156694456[39] = 0;
+   out_6351856658156694456[40] = 1;
+   out_6351856658156694456[41] = 0;
+   out_6351856658156694456[42] = 0;
+   out_6351856658156694456[43] = 0;
+   out_6351856658156694456[44] = 0;
+   out_6351856658156694456[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
+   out_6351856658156694456[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
+   out_6351856658156694456[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6351856658156694456[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6351856658156694456[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
+   out_6351856658156694456[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
+   out_6351856658156694456[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
+   out_6351856658156694456[52] = dt*stiffness_front*state[0]/(mass*state[1]);
+   out_6351856658156694456[53] = -9.8100000000000005*dt;
+   out_6351856658156694456[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
+   out_6351856658156694456[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
+   out_6351856658156694456[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6351856658156694456[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6351856658156694456[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
+   out_6351856658156694456[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
+   out_6351856658156694456[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
+   out_6351856658156694456[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
+   out_6351856658156694456[62] = 0;
+   out_6351856658156694456[63] = 0;
+   out_6351856658156694456[64] = 0;
+   out_6351856658156694456[65] = 0;
+   out_6351856658156694456[66] = 0;
+   out_6351856658156694456[67] = 0;
+   out_6351856658156694456[68] = 0;
+   out_6351856658156694456[69] = 0;
+   out_6351856658156694456[70] = 1;
+   out_6351856658156694456[71] = 0;
+   out_6351856658156694456[72] = 0;
+   out_6351856658156694456[73] = 0;
+   out_6351856658156694456[74] = 0;
+   out_6351856658156694456[75] = 0;
+   out_6351856658156694456[76] = 0;
+   out_6351856658156694456[77] = 0;
+   out_6351856658156694456[78] = 0;
+   out_6351856658156694456[79] = 0;
+   out_6351856658156694456[80] = 1;
 }
-void h_25(double *state, double *unused, double *out_8734450240234959528) {
-   out_8734450240234959528[0] = state[6];
+void h_25(double *state, double *unused, double *out_2115125997200973707) {
+   out_2115125997200973707[0] = state[6];
 }
-void H_25(double *state, double *unused, double *out_6133161197384342845) {
-   out_6133161197384342845[0] = 0;
-   out_6133161197384342845[1] = 0;
-   out_6133161197384342845[2] = 0;
-   out_6133161197384342845[3] = 0;
-   out_6133161197384342845[4] = 0;
-   out_6133161197384342845[5] = 0;
-   out_6133161197384342845[6] = 1;
-   out_6133161197384342845[7] = 0;
-   out_6133161197384342845[8] = 0;
+void H_25(double *state, double *unused, double *out_6275952284011354329) {
+   out_6275952284011354329[0] = 0;
+   out_6275952284011354329[1] = 0;
+   out_6275952284011354329[2] = 0;
+   out_6275952284011354329[3] = 0;
+   out_6275952284011354329[4] = 0;
+   out_6275952284011354329[5] = 0;
+   out_6275952284011354329[6] = 1;
+   out_6275952284011354329[7] = 0;
+   out_6275952284011354329[8] = 0;
 }
-void h_24(double *state, double *unused, double *out_8806581475742121932) {
-   out_8806581475742121932[0] = state[4];
-   out_8806581475742121932[1] = state[5];
+void h_24(double *state, double *unused, double *out_7937179482135297626) {
+   out_7937179482135297626[0] = state[4];
+   out_7937179482135297626[1] = state[5];
 }
-void H_24(double *state, double *unused, double *out_5738011069733690670) {
-   out_5738011069733690670[0] = 0;
-   out_5738011069733690670[1] = 0;
-   out_5738011069733690670[2] = 0;
-   out_5738011069733690670[3] = 0;
-   out_5738011069733690670[4] = 1;
-   out_5738011069733690670[5] = 0;
-   out_5738011069733690670[6] = 0;
-   out_5738011069733690670[7] = 0;
-   out_5738011069733690670[8] = 0;
-   out_5738011069733690670[9] = 0;
-   out_5738011069733690670[10] = 0;
-   out_5738011069733690670[11] = 0;
-   out_5738011069733690670[12] = 0;
-   out_5738011069733690670[13] = 0;
-   out_5738011069733690670[14] = 1;
-   out_5738011069733690670[15] = 0;
-   out_5738011069733690670[16] = 0;
-   out_5738011069733690670[17] = 0;
+void H_24(double *state, double *unused, double *out_8612866190895874151) {
+   out_8612866190895874151[0] = 0;
+   out_8612866190895874151[1] = 0;
+   out_8612866190895874151[2] = 0;
+   out_8612866190895874151[3] = 0;
+   out_8612866190895874151[4] = 1;
+   out_8612866190895874151[5] = 0;
+   out_8612866190895874151[6] = 0;
+   out_8612866190895874151[7] = 0;
+   out_8612866190895874151[8] = 0;
+   out_8612866190895874151[9] = 0;
+   out_8612866190895874151[10] = 0;
+   out_8612866190895874151[11] = 0;
+   out_8612866190895874151[12] = 0;
+   out_8612866190895874151[13] = 0;
+   out_8612866190895874151[14] = 1;
+   out_8612866190895874151[15] = 0;
+   out_8612866190895874151[16] = 0;
+   out_8612866190895874151[17] = 0;
 }
-void h_30(double *state, double *unused, double *out_971184983414287087) {
-   out_971184983414287087[0] = state[4];
+void h_30(double *state, double *unused, double *out_8646825364085210282) {
+   out_8646825364085210282[0] = state[4];
 }
-void H_30(double *state, double *unused, double *out_5396892534833592016) {
-   out_5396892534833592016[0] = 0;
-   out_5396892534833592016[1] = 0;
-   out_5396892534833592016[2] = 0;
-   out_5396892534833592016[3] = 0;
-   out_5396892534833592016[4] = 1;
-   out_5396892534833592016[5] = 0;
-   out_5396892534833592016[6] = 0;
-   out_5396892534833592016[7] = 0;
-   out_5396892534833592016[8] = 0;
+void H_30(double *state, double *unused, double *out_6146613336868114259) {
+   out_6146613336868114259[0] = 0;
+   out_6146613336868114259[1] = 0;
+   out_6146613336868114259[2] = 0;
+   out_6146613336868114259[3] = 0;
+   out_6146613336868114259[4] = 1;
+   out_6146613336868114259[5] = 0;
+   out_6146613336868114259[6] = 0;
+   out_6146613336868114259[7] = 0;
+   out_6146613336868114259[8] = 0;
 }
-void h_26(double *state, double *unused, double *out_1416855028350640224) {
-   out_1416855028350640224[0] = state[7];
+void h_26(double *state, double *unused, double *out_7195513388815643683) {
+   out_7195513388815643683[0] = state[7];
 }
-void H_26(double *state, double *unused, double *out_2391657878510286621) {
-   out_2391657878510286621[0] = 0;
-   out_2391657878510286621[1] = 0;
-   out_2391657878510286621[2] = 0;
-   out_2391657878510286621[3] = 0;
-   out_2391657878510286621[4] = 0;
-   out_2391657878510286621[5] = 0;
-   out_2391657878510286621[6] = 0;
-   out_2391657878510286621[7] = 1;
-   out_2391657878510286621[8] = 0;
+void H_26(double *state, double *unused, double *out_2534448965137298105) {
+   out_2534448965137298105[0] = 0;
+   out_2534448965137298105[1] = 0;
+   out_2534448965137298105[2] = 0;
+   out_2534448965137298105[3] = 0;
+   out_2534448965137298105[4] = 0;
+   out_2534448965137298105[5] = 0;
+   out_2534448965137298105[6] = 0;
+   out_2534448965137298105[7] = 1;
+   out_2534448965137298105[8] = 0;
 }
-void h_27(double *state, double *unused, double *out_5511166866350839857) {
-   out_5511166866350839857[0] = state[3];
+void h_27(double *state, double *unused, double *out_3029764065914673830) {
+   out_3029764065914673830[0] = state[3];
 }
-void H_27(double *state, double *unused, double *out_7571655846634016927) {
-   out_7571655846634016927[0] = 0;
-   out_7571655846634016927[1] = 0;
-   out_7571655846634016927[2] = 0;
-   out_7571655846634016927[3] = 1;
-   out_7571655846634016927[4] = 0;
-   out_7571655846634016927[5] = 0;
-   out_7571655846634016927[6] = 0;
-   out_7571655846634016927[7] = 0;
-   out_7571655846634016927[8] = 0;
+void H_27(double *state, double *unused, double *out_3971850025067689348) {
+   out_3971850025067689348[0] = 0;
+   out_3971850025067689348[1] = 0;
+   out_3971850025067689348[2] = 0;
+   out_3971850025067689348[3] = 1;
+   out_3971850025067689348[4] = 0;
+   out_3971850025067689348[5] = 0;
+   out_3971850025067689348[6] = 0;
+   out_3971850025067689348[7] = 0;
+   out_3971850025067689348[8] = 0;
 }
-void h_29(double *state, double *unused, double *out_5235972804066333968) {
-   out_5235972804066333968[0] = state[1];
+void h_29(double *state, double *unused, double *out_4733501190905998611) {
+   out_4733501190905998611[0] = state[1];
 }
-void H_29(double *state, double *unused, double *out_9161725500205983656) {
-   out_9161725500205983656[0] = 0;
-   out_9161725500205983656[1] = 1;
-   out_9161725500205983656[2] = 0;
-   out_9161725500205983656[3] = 0;
-   out_9161725500205983656[4] = 0;
-   out_9161725500205983656[5] = 0;
-   out_9161725500205983656[6] = 0;
-   out_9161725500205983656[7] = 0;
-   out_9161725500205983656[8] = 0;
+void H_29(double *state, double *unused, double *out_6656844681182506443) {
+   out_6656844681182506443[0] = 0;
+   out_6656844681182506443[1] = 1;
+   out_6656844681182506443[2] = 0;
+   out_6656844681182506443[3] = 0;
+   out_6656844681182506443[4] = 0;
+   out_6656844681182506443[5] = 0;
+   out_6656844681182506443[6] = 0;
+   out_6656844681182506443[7] = 0;
+   out_6656844681182506443[8] = 0;
 }
-void h_28(double *state, double *unused, double *out_7891338238985058206) {
-   out_7891338238985058206[0] = state[0];
+void h_28(double *state, double *unused, double *out_6095421901546968827) {
+   out_6095421901546968827[0] = state[0];
 }
-void H_28(double *state, double *unused, double *out_4079326483136453082) {
-   out_4079326483136453082[0] = 1;
-   out_4079326483136453082[1] = 0;
-   out_4079326483136453082[2] = 0;
-   out_4079326483136453082[3] = 0;
-   out_4079326483136453082[4] = 0;
-   out_4079326483136453082[5] = 0;
-   out_4079326483136453082[6] = 0;
-   out_4079326483136453082[7] = 0;
-   out_4079326483136453082[8] = 0;
+void H_28(double *state, double *unused, double *out_2823911718871392259) {
+   out_2823911718871392259[0] = 1;
+   out_2823911718871392259[1] = 0;
+   out_2823911718871392259[2] = 0;
+   out_2823911718871392259[3] = 0;
+   out_2823911718871392259[4] = 0;
+   out_2823911718871392259[5] = 0;
+   out_2823911718871392259[6] = 0;
+   out_2823911718871392259[7] = 0;
+   out_2823911718871392259[8] = 0;
 }
-void h_31(double *state, double *unused, double *out_8694811794428211892) {
-   out_8694811794428211892[0] = state[8];
+void h_31(double *state, double *unused, double *out_6524751479517568177) {
+   out_6524751479517568177[0] = state[8];
 }
-void H_31(double *state, double *unused, double *out_6163807159261303273) {
-   out_6163807159261303273[0] = 0;
-   out_6163807159261303273[1] = 0;
-   out_6163807159261303273[2] = 0;
-   out_6163807159261303273[3] = 0;
-   out_6163807159261303273[4] = 0;
-   out_6163807159261303273[5] = 0;
-   out_6163807159261303273[6] = 0;
-   out_6163807159261303273[7] = 0;
-   out_6163807159261303273[8] = 1;
+void H_31(double *state, double *unused, double *out_6306598245888314757) {
+   out_6306598245888314757[0] = 0;
+   out_6306598245888314757[1] = 0;
+   out_6306598245888314757[2] = 0;
+   out_6306598245888314757[3] = 0;
+   out_6306598245888314757[4] = 0;
+   out_6306598245888314757[5] = 0;
+   out_6306598245888314757[6] = 0;
+   out_6306598245888314757[7] = 0;
+   out_6306598245888314757[8] = 1;
 }
 #include <eigen3/Eigen/Dense>
 #include <iostream>
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
  update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
 }
-void car_err_fun(double *nom_x, double *delta_x, double *out_205475353292099530) {
-  err_fun(nom_x, delta_x, out_205475353292099530);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5744499419236567507) {
+  err_fun(nom_x, delta_x, out_5744499419236567507);
 }
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341) {
-  inv_err_fun(nom_x, true_x, out_6859088956493527341);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1184805137205037163) {
+  inv_err_fun(nom_x, true_x, out_1184805137205037163);
 }
-void car_H_mod_fun(double *state, double *out_8201686483145055601) {
-  H_mod_fun(state, out_8201686483145055601);
+void car_H_mod_fun(double *state, double *out_2752971634021862652) {
+  H_mod_fun(state, out_2752971634021862652);
 }
-void car_f_fun(double *state, double dt, double *out_2204957494938772746) {
-  f_fun(state,  dt, out_2204957494938772746);
+void car_f_fun(double *state, double dt, double *out_7658700760911629364) {
+  f_fun(state,  dt, out_7658700760911629364);
 }
-void car_F_fun(double *state, double dt, double *out_2327939745960551937) {
-  F_fun(state,  dt, out_2327939745960551937);
+void car_F_fun(double *state, double dt, double *out_6351856658156694456) {
+  F_fun(state,  dt, out_6351856658156694456);
 }
-void car_h_25(double *state, double *unused, double *out_8734450240234959528) {
-  h_25(state, unused, out_8734450240234959528);
+void car_h_25(double *state, double *unused, double *out_2115125997200973707) {
+  h_25(state, unused, out_2115125997200973707);
 }
-void car_H_25(double *state, double *unused, double *out_6133161197384342845) {
-  H_25(state, unused, out_6133161197384342845);
+void car_H_25(double *state, double *unused, double *out_6275952284011354329) {
+  H_25(state, unused, out_6275952284011354329);
 }
-void car_h_24(double *state, double *unused, double *out_8806581475742121932) {
-  h_24(state, unused, out_8806581475742121932);
+void car_h_24(double *state, double *unused, double *out_7937179482135297626) {
+  h_24(state, unused, out_7937179482135297626);
 }
-void car_H_24(double *state, double *unused, double *out_5738011069733690670) {
-  H_24(state, unused, out_5738011069733690670);
+void car_H_24(double *state, double *unused, double *out_8612866190895874151) {
+  H_24(state, unused, out_8612866190895874151);
 }
-void car_h_30(double *state, double *unused, double *out_971184983414287087) {
-  h_30(state, unused, out_971184983414287087);
+void car_h_30(double *state, double *unused, double *out_8646825364085210282) {
+  h_30(state, unused, out_8646825364085210282);
 }
-void car_H_30(double *state, double *unused, double *out_5396892534833592016) {
-  H_30(state, unused, out_5396892534833592016);
+void car_H_30(double *state, double *unused, double *out_6146613336868114259) {
+  H_30(state, unused, out_6146613336868114259);
 }
-void car_h_26(double *state, double *unused, double *out_1416855028350640224) {
-  h_26(state, unused, out_1416855028350640224);
+void car_h_26(double *state, double *unused, double *out_7195513388815643683) {
+  h_26(state, unused, out_7195513388815643683);
 }
-void car_H_26(double *state, double *unused, double *out_2391657878510286621) {
-  H_26(state, unused, out_2391657878510286621);
+void car_H_26(double *state, double *unused, double *out_2534448965137298105) {
+  H_26(state, unused, out_2534448965137298105);
 }
-void car_h_27(double *state, double *unused, double *out_5511166866350839857) {
-  h_27(state, unused, out_5511166866350839857);
+void car_h_27(double *state, double *unused, double *out_3029764065914673830) {
+  h_27(state, unused, out_3029764065914673830);
 }
-void car_H_27(double *state, double *unused, double *out_7571655846634016927) {
-  H_27(state, unused, out_7571655846634016927);
+void car_H_27(double *state, double *unused, double *out_3971850025067689348) {
+  H_27(state, unused, out_3971850025067689348);
 }
-void car_h_29(double *state, double *unused, double *out_5235972804066333968) {
-  h_29(state, unused, out_5235972804066333968);
+void car_h_29(double *state, double *unused, double *out_4733501190905998611) {
+  h_29(state, unused, out_4733501190905998611);
 }
-void car_H_29(double *state, double *unused, double *out_9161725500205983656) {
-  H_29(state, unused, out_9161725500205983656);
+void car_H_29(double *state, double *unused, double *out_6656844681182506443) {
+  H_29(state, unused, out_6656844681182506443);
 }
-void car_h_28(double *state, double *unused, double *out_7891338238985058206) {
-  h_28(state, unused, out_7891338238985058206);
+void car_h_28(double *state, double *unused, double *out_6095421901546968827) {
+  h_28(state, unused, out_6095421901546968827);
 }
-void car_H_28(double *state, double *unused, double *out_4079326483136453082) {
-  H_28(state, unused, out_4079326483136453082);
+void car_H_28(double *state, double *unused, double *out_2823911718871392259) {
+  H_28(state, unused, out_2823911718871392259);
 }
-void car_h_31(double *state, double *unused, double *out_8694811794428211892) {
-  h_31(state, unused, out_8694811794428211892);
+void car_h_31(double *state, double *unused, double *out_6524751479517568177) {
+  h_31(state, unused, out_6524751479517568177);
 }
-void car_H_31(double *state, double *unused, double *out_6163807159261303273) {
-  H_31(state, unused, out_6163807159261303273);
+void car_H_31(double *state, double *unused, double *out_6306598245888314757) {
+  H_31(state, unused, out_6306598245888314757);
 }
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
  predict(in_x, in_P, in_Q, dt);
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void car_err_fun(double *nom_x, double *delta_x, double *out_205475353292099530);
-void car_inv_err_fun(double *nom_x, double *true_x, double *out_6859088956493527341);
-void car_H_mod_fun(double *state, double *out_8201686483145055601);
-void car_f_fun(double *state, double dt, double *out_2204957494938772746);
-void car_F_fun(double *state, double dt, double *out_2327939745960551937);
-void car_h_25(double *state, double *unused, double *out_8734450240234959528);
-void car_H_25(double *state, double *unused, double *out_6133161197384342845);
-void car_h_24(double *state, double *unused, double *out_8806581475742121932);
-void car_H_24(double *state, double *unused, double *out_5738011069733690670);
-void car_h_30(double *state, double *unused, double *out_971184983414287087);
-void car_H_30(double *state, double *unused, double *out_5396892534833592016);
-void car_h_26(double *state, double *unused, double *out_1416855028350640224);
-void car_H_26(double *state, double *unused, double *out_2391657878510286621);
-void car_h_27(double *state, double *unused, double *out_5511166866350839857);
-void car_H_27(double *state, double *unused, double *out_7571655846634016927);
-void car_h_29(double *state, double *unused, double *out_5235972804066333968);
-void car_H_29(double *state, double *unused, double *out_9161725500205983656);
-void car_h_28(double *state, double *unused, double *out_7891338238985058206);
-void car_H_28(double *state, double *unused, double *out_4079326483136453082);
-void car_h_31(double *state, double *unused, double *out_8694811794428211892);
-void car_H_31(double *state, double *unused, double *out_6163807159261303273);
+void car_err_fun(double *nom_x, double *delta_x, double *out_5744499419236567507);
+void car_inv_err_fun(double *nom_x, double *true_x, double *out_1184805137205037163);
+void car_H_mod_fun(double *state, double *out_2752971634021862652);
+void car_f_fun(double *state, double dt, double *out_7658700760911629364);
+void car_F_fun(double *state, double dt, double *out_6351856658156694456);
+void car_h_25(double *state, double *unused, double *out_2115125997200973707);
+void car_H_25(double *state, double *unused, double *out_6275952284011354329);
+void car_h_24(double *state, double *unused, double *out_7937179482135297626);
+void car_H_24(double *state, double *unused, double *out_8612866190895874151);
+void car_h_30(double *state, double *unused, double *out_8646825364085210282);
+void car_H_30(double *state, double *unused, double *out_6146613336868114259);
+void car_h_26(double *state, double *unused, double *out_7195513388815643683);
+void car_H_26(double *state, double *unused, double *out_2534448965137298105);
+void car_h_27(double *state, double *unused, double *out_3029764065914673830);
+void car_H_27(double *state, double *unused, double *out_3971850025067689348);
+void car_h_29(double *state, double *unused, double *out_4733501190905998611);
+void car_H_29(double *state, double *unused, double *out_6656844681182506443);
+void car_h_28(double *state, double *unused, double *out_6095421901546968827);
+void car_H_28(double *state, double *unused, double *out_2823911718871392259);
+void car_h_31(double *state, double *unused, double *out_6524751479517568177);
+void car_H_31(double *state, double *unused, double *out_6306598245888314757);
 void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
 void car_set_mass(double x);
 void car_set_rotational_inertia(double x);
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
 void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void pose_err_fun(double *nom_x, double *delta_x, double *out_7491504487925958906);
-void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8173082431785614969);
-void pose_H_mod_fun(double *state, double *out_3955896825213980969);
-void pose_f_fun(double *state, double dt, double *out_6226431844178190638);
-void pose_F_fun(double *state, double dt, double *out_5590858509180719260);
-void pose_h_4(double *state, double *unused, double *out_8311116516369893334);
-void pose_H_4(double *state, double *unused, double *out_1196621559825099766);
-void pose_h_10(double *state, double *unused, double *out_8024633733467632830);
-void pose_H_10(double *state, double *unused, double *out_9211636060607930666);
-void pose_h_13(double *state, double *unused, double *out_5246243153983473200);
-void pose_H_13(double *state, double *unused, double *out_4408895385157432567);
-void pose_h_14(double *state, double *unused, double *out_2666716766060469740);
-void pose_H_14(double *state, double *unused, double *out_5159862416164584295);
+void pose_err_fun(double *nom_x, double *delta_x, double *out_7899872685628621531);
+void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1215289726053279423);
+void pose_H_mod_fun(double *state, double *out_420741753375595218);
+void pose_f_fun(double *state, double dt, double *out_5996963235034927992);
+void pose_F_fun(double *state, double dt, double *out_78320000076568703);
+void pose_h_4(double *state, double *unused, double *out_2403402202974529453);
+void pose_H_4(double *state, double *unused, double *out_5573260138414675953);
+void pose_h_10(double *state, double *unused, double *out_1415940429779603810);
+void pose_H_10(double *state, double *unused, double *out_5540918162512642087);
+void pose_h_13(double *state, double *unused, double *out_8116494657900557165);
+void pose_H_13(double *state, double *unused, double *out_8785533963747008754);
+void pose_h_14(double *state, double *unused, double *out_2556692697037776409);
+void pose_H_14(double *state, double *unused, double *out_8910243078955391134);
 void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -1 +1 @@
-#define SUNNYPILOT_VERSION "2026.09.06-4860"
+#define SUNNYPILOT_VERSION "2026.09.04-4849"
@@ -0,0 +1,5 @@
+from pathlib import Path
+
+MODEL_PATH = Path(__file__).parent / 'models/supercombo.onnx'
+MODEL_PKL_PATH = Path(__file__).parent / 'models/supercombo_tinygrad.pkl'
+METADATA_PATH = Path(__file__).parent / 'models/supercombo_metadata.pkl'
@@ -32,7 +32,7 @@ def _patch_tinygrad_fetch_fw():
  helpers.fetch_fw = fetch_fw
 _patch_tinygrad_fetch_fw()

-import openpilot.selfdrive.modeld.compile_modeld as stock
+from openpilot.selfdrive.modeld.compile_modeld import NV12Frame, make_frame_prepare, sample_desire, sample_skip, shift_and_sample
 from tinygrad import dtypes
 from tinygrad.device import Device
 from tinygrad.engine.jit import TinyJit
@@ -41,7 +41,8 @@ from tinygrad.tensor import Tensor
 MODEL_TYPES = ('vision_policy', 'supercombo', 'vision_multi_policy')
 WARP_INPUTS = ['tfm', 'big_tfm']
 POLICY_INPUTS = ['img_q', 'big_img_q', 'feat_q', 'desire_q', 'packed_npy_inputs']
-nv12_copy_size = stock.nv12_copy_size
+WARP_DEV = os.getenv('WARP_DEV')
+

 def _detect_desire_key(shapes: dict) -> str | None:
  return next((key for key in shapes if key.startswith('desire')), None)
@@ -138,7 +139,7 @@ def make_supercombo_input_queues(input_shapes: dict, frame_skip: int,
  return generate_queues_and_npy(input_shapes, frame_skip, device, is_supercombo=True)


-def make_random_images(keys, shape, device, rng=None):
+def make_random_images(keys, shape, device):
  return {k: Tensor.randint(shape, low=0, high=256, dtype=dtypes.uint8, device=device).realize() for k in keys}


@@ -151,9 +152,24 @@ def make_warp_queues(device=Device.DEFAULT):
  return queues, npy


+def make_warp(nv12: NV12Frame, model_w: int, model_h: int):
+  frame_prepare = make_frame_prepare(nv12, model_w, model_h)
+  WARP_DEV = os.getenv('WARP_DEV', Device.DEFAULT)
+
+  def warp(tfm, big_tfm, frame, big_frame):
+    tfm = tfm.to(WARP_DEV)
+    big_tfm = big_tfm.to(WARP_DEV)
+    Tensor.realize(tfm, big_tfm)
+
+    warped_frame = frame_prepare(frame, tfm).unsqueeze(0)
+    warped_big_frame = frame_prepare(big_frame, big_tfm).unsqueeze(0)
+    return Tensor.cat(warped_frame, warped_big_frame)
+  return warp
+
+
 def make_run_policy(vision_runner, policy_runners: list, features_slice: slice, frame_skip: int, input_shapes: dict):
-  sample_skip_fn = partial(stock.sample_skip, frame_skip=frame_skip)
-  sample_desire_fn = partial(stock.sample_desire, frame_skip=frame_skip)
+  sample_skip_fn = partial(sample_skip, frame_skip=frame_skip)
+  sample_desire_fn = partial(sample_desire, frame_skip=frame_skip)

  desire_key = _detect_desire_key(input_shapes)
  road_key, wide_key = _detect_vision_keys(input_shapes)
@@ -170,14 +186,14 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
    warped_dev = warped.to(Device.DEFAULT)
    Tensor.realize(packed_npy_inputs_dev, warped_dev)

-    img = stock.shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
-    big_img = stock.shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)
+    img = shift_and_sample(img_q, warped_dev[0:1], sample_skip_fn)
+    big_img = shift_and_sample(big_img_q, warped_dev[1:2], sample_skip_fn)

    unpacked_tensors = [tensor.reshape(shape) for tensor, shape in zip(packed_npy_inputs_dev.split(npy_sizes), npy_shapes.values(), strict=True)]
    unpacked_dict = dict(zip(npy_shapes.keys(), unpacked_tensors, strict=True))

    desire_dev = unpacked_dict['desire']
-    desire_buf = stock.shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)
+    desire_buf = shift_and_sample(desire_q, desire_dev.reshape(1, 1, -1), sample_desire_fn)

    inputs = {desire_key: desire_buf}
    for key, tensor_val in unpacked_dict.items():
@@ -186,13 +202,13 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,

    if 'prev_feat' in unpacked_dict:
      prev_feat_dev = unpacked_dict['prev_feat']
-      inputs['features_buffer'] = stock.shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])
+      inputs['features_buffer'] = shift_and_sample(feat_q, prev_feat_dev.reshape(1, 1, -1), sample_skip_fn).reshape(input_shapes['features_buffer'])

    if vision_runner:
      vision_out_cast = next(iter(vision_runner({road_key: img, wide_key: big_img}).values())).cast('float32').realize()
      if 'features_buffer' not in inputs:
        new_feat = vision_out_cast[:, features_slice].reshape(1, -1).unsqueeze(0)
-        inputs['features_buffer'] = stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+        inputs['features_buffer'] = shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
      policy_outs = [next(iter(pol_runner(inputs).values())).cast('float32').realize() for pol_runner in policy_runners]
      return (vision_out_cast, *policy_outs) if len(policy_outs) > 1 else (vision_out_cast, policy_outs[0])

@@ -203,28 +219,27 @@ def make_run_policy(vision_runner, policy_runners: list, features_slice: slice,
    policy_out = next(iter(policy_runners[0](inputs).values())).cast('float32').realize()
    if 'features_buffer' not in inputs and features_slice is not None:
      new_feat = policy_out[:, features_slice].reshape(1, -1).unsqueeze(0)
-      stock.shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
+      shift_and_sample(feat_q, new_feat, sample_skip_fn).realize()
    return policy_out

  return run_policy


-def compile_jit(jit, input_keys, make_queues, make_random_inputs=None, benchmark_runs: int = 1):
+def compile_jit(jit, make_random_inputs, input_keys, make_queues):
  SEED = 42
-  def random_inputs_run(fn, seed, n_runs, test_val=None, test_buffers=None, expect_match=True):
-    queues_res = make_queues(Device.DEFAULT)
-    input_queues, npy = queues_res[0], queues_res[1]
-    frame_views = queues_res[2] if len(queues_res) > 2 else {}
+  def random_inputs_run(fn, seed, test_val=None, test_buffers=None, expect_match=True):
+    input_queues, npy = make_queues(Device.DEFAULT)
    rng = np.random.default_rng(seed)
    Tensor.manual_seed(seed)

+    testing = test_val is not None or test_buffers is not None
+    n_runs = 1 if testing else 3
+
    for i in range(n_runs):
      for v in npy.values():
        v[:] = rng.standard_normal(v.shape).astype(v.dtype)
-      for v in frame_views.values():
-        v[:] = rng.integers(0, 256, size=v.shape, dtype=np.uint8)
      Device.default.synchronize()
-      random_inputs = make_random_inputs(rng=rng) if make_random_inputs is not None else {}
+      random_inputs = make_random_inputs()
      st = time.perf_counter()
      outs = fn(**{k: input_queues[k] for k in input_keys if k in input_queues}, **random_inputs)
      mt = time.perf_counter()
@@ -245,15 +260,14 @@ def compile_jit(jit, input_keys, make_queues, make_random_inputs=None, benchmark
    return val, buffers

  print('capture + replay')
-  test_val, test_buffers = random_inputs_run(jit, SEED, 3)
-  print(f'pickle round trip ({benchmark_runs} runs per seed)')
+  test_val, test_buffers = random_inputs_run(jit, SEED)
+  print('pickle round trip')
  with tempfile.TemporaryFile(dir=".") as f:
    dump_oob(jit, f)
    f.seek(0)
-    loaded_jit = load_oob(f)
-  random_inputs_run(loaded_jit, SEED, benchmark_runs, test_val, test_buffers, expect_match=True)
-  random_inputs_run(loaded_jit, SEED+1, benchmark_runs, test_val, test_buffers, expect_match=False)
-  return jit
+    deserialized_jit = load_oob(f)
+  random_inputs_run(deserialized_jit, SEED, test_val=test_val, test_buffers=test_buffers)
+  return deserialized_jit


 def _parse_size(size_str: str) -> tuple[int, int]:
@@ -303,7 +317,6 @@ if __name__ == "__main__":
  parser.add_argument('--model-size', type=_parse_size, required=True, help='model input WxH')
  parser.add_argument('--camera-resolutions', type=_parse_size, nargs='+', required=True)
  parser.add_argument('--frame-skip', type=int, default=None, help='frame skip value (auto-derived if not provided)')
-  parser.add_argument('--benchmark-runs', type=int, default=1, help='benchmark runs')
  parser.add_argument('--output', required=True)

  parser.add_argument('--vision-onnx', help='vision ONNX (for split models)')
@@ -322,64 +335,48 @@ if __name__ == "__main__":
  args.on_policy_onnx = read_file_chunked_to_disk(args.on_policy_onnx)
  args.supercombo_onnx = read_file_chunked_to_disk(args.supercombo_onnx)

-  if args.model_type == 'supercombo':
+  vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
+
+  if args.model_type == 'vision_policy':
+    assert vision_runner and args.policy_onnx
+    policy_runners = [OnnxRunner(args.policy_onnx)]
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
+  elif args.model_type == 'supercombo':
    assert args.supercombo_onnx
-    model_metadata = make_metadata_dict(args.supercombo_onnx)
-    output_data['metadata'] = {'model': model_metadata, **model_metadata}
-    output_data['input_devices'] = {'model': Device.DEFAULT}
-    output_data['run_model'] = {}
-    derived_frame_skip = args.frame_skip or derive_frame_skip({}, model_metadata['input_shapes'])
-    model_runner = OnnxRunner(args.supercombo_onnx)
-    run_policy = stock.make_run_policy(model_runner, model_metadata, derived_frame_skip)
-    for cam_w, cam_h in args.camera_resolutions:
-      print(f"Compiling unified run_model JIT for {cam_w}x{cam_h}...")
-      nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-      make_model_queues = partial(stock.make_input_queues, model_metadata['input_shapes'], derived_frame_skip,
-                                  frame_copy_size=frame_copy_size)
-      warp = stock.make_warp(nv12, model_w, model_h)
-      run_model_jit = TinyJit(stock.make_run_model(warp, run_policy, model_metadata, frame_copy_size), prune=True)
-      output_data['run_model'][(cam_w, cam_h)] = compile_jit(run_model_jit, stock.MODELD_INPUTS, make_model_queues, benchmark_runs=args.benchmark_runs)
-  else:
-    vision_runner = OnnxRunner(args.vision_onnx) if args.vision_onnx else None
-    if args.model_type == 'vision_policy':
-      assert vision_runner and args.policy_onnx
-      policy_runners = [OnnxRunner(args.policy_onnx)]
-      output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx), 'policy': make_metadata_dict(args.policy_onnx)}
-    elif args.model_type == 'vision_multi_policy':
-      assert vision_runner
-      policy_runners, policy_names = _load_policy_runners(args)
-      output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
-      for name in policy_names:
-        runner_arg = getattr(args, f"{name}_onnx")
-        output_data['metadata'][name] = make_metadata_dict(runner_arg)
+    policy_runners = [OnnxRunner(args.supercombo_onnx)]
+    output_data['metadata'] = {'model': make_metadata_dict(args.supercombo_onnx)}
+  elif args.model_type == 'vision_multi_policy':
+    assert vision_runner
+    policy_runners, policy_names = _load_policy_runners(args)
+    output_data['metadata'] = {'vision': make_metadata_dict(args.vision_onnx)}
+    for name in policy_names:
+      runner_arg = getattr(args, f"{name}_onnx")
+      output_data['metadata'][name] = make_metadata_dict(runner_arg)

-    policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
-    first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
-    vision_meta = output_data['metadata'].get('vision', {})
+  policy_keys = [key for key in output_data['metadata'].keys() if key != 'vision']
+  first_policy_meta = output_data['metadata'][policy_keys[0]] if policy_keys else {}
+  vision_meta = output_data['metadata'].get('vision', {})

-    derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
-    all_shapes = {key: value for meta in output_data['metadata'].values() for key, value in meta['input_shapes'].items()}
-    feat_meta = output_data['metadata'].get('vision') or output_data['metadata'].get('policy')
-    assert feat_meta is not None
-    features_slice = feat_meta['output_slices']['hidden_state']
+  derived_frame_skip = args.frame_skip or derive_frame_skip(vision_meta.get('input_shapes', {}), first_policy_meta.get('input_shapes', {}))
+  all_shapes = {key: value for meta in output_data['metadata'].values() for key, value in meta['input_shapes'].items()}
+  feat_meta = output_data['metadata'].get('vision') or output_data['metadata'].get('model') or output_data['metadata'].get('policy')
+  assert feat_meta is not None
+  features_slice = feat_meta['output_slices']['hidden_state']
+  is_supercombo = vision_runner is None

-    print(f"Compiling run_policy JIT (model_size={model_w}x{model_h}, frame_skip={derived_frame_skip})...")
-    run_policy_func = make_run_policy(vision_runner, policy_runners, features_slice, derived_frame_skip, all_shapes)
-    run_policy_jit = TinyJit(run_policy_func, prune=True)
-    make_policy_queues = partial(generate_queues_and_npy, all_shapes, derived_frame_skip, is_supercombo=False)
-    make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, model_h // 2, model_w // 2), device=Device.DEFAULT)
-    output_data['run_policy'] = compile_jit(run_policy_jit, POLICY_INPUTS, make_policy_queues, make_random_inputs=make_random_model_inputs)
+  print(f"Compiling run_policy JIT (model_size={model_w}x{model_h}, frame_skip={derived_frame_skip})...")
+  run_policy_func = make_run_policy(vision_runner, policy_runners, features_slice, derived_frame_skip, all_shapes)
+  run_policy_jit = TinyJit(run_policy_func, prune=True)
+  make_policy_queues = partial(generate_queues_and_npy, all_shapes, derived_frame_skip, is_supercombo=is_supercombo)
+  make_random_model_inputs = partial(make_random_images, keys=['warped'], shape=(2, 6, model_h // 2, model_w // 2), device=WARP_DEV)
+  output_data['run_policy'] = compile_jit(run_policy_jit, make_random_model_inputs, POLICY_INPUTS, make_policy_queues)

-    for cam_w, cam_h in args.camera_resolutions:
-      print(f"Compiling warp JIT for {cam_w}x{cam_h}...")
-      nv12 = stock.NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
-      frame_copy_size = stock.nv12_copy_size(nv12.stride, nv12.y_height, nv12.uv_height)
-      make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=frame_copy_size, device=Device.DEFAULT)
-      warp = TinyJit(stock.make_warp(nv12, model_w, model_h), prune=True)
-      output_data[(cam_w, cam_h)] = compile_jit(warp, WARP_INPUTS, make_warp_queues, make_random_inputs=make_random_warp_inputs)
-
-    output_data['metadata']['warp_dev'] = Device.DEFAULT
+  for cam_w, cam_h in args.camera_resolutions:
+    print(f"Compiling warp JIT for {cam_w}x{cam_h}...")
+    nv12 = NV12Frame(cam_w, cam_h, *get_nv12_info(cam_w, cam_h))
+    make_random_warp_inputs = partial(make_random_images, keys=['frame', 'big_frame'], shape=nv12.size, device=WARP_DEV)
+    warp = TinyJit(make_warp(nv12, model_w, model_h), prune=True)
+    output_data[(cam_w, cam_h)] = compile_jit(warp, make_random_warp_inputs, WARP_INPUTS, make_warp_queues)

  with open(args.output, "wb") as file:
    dump_oob(output_data, file)
@@ -14,8 +14,6 @@ class ModelConstants:

  # model inputs constants
  MODEL_FREQ = 20
-  MODEL_RUN_FREQ = 20
-  MODEL_CONTEXT_FREQ = 5
  FEATURE_LEN = 512
  FULL_HISTORY_BUFFER_LEN = 99
  DESIRE_LEN = 8
@@ -37,7 +35,6 @@ class ModelConstants:
  LANE_LINES_WIDTH = 2
  ROAD_EDGES_WIDTH = 2
  PLAN_WIDTH = 15
-  ACTION_WIDTH = 2
  DESIRE_PRED_WIDTH = 8
  LAT_PLANNER_SOLUTION_WIDTH = 4
  DESIRED_CURV_WIDTH = 1
@@ -1,9 +1,26 @@
 from openpilot.sunnypilot.modeld_v2.constants import Meta
+from openpilot.cereal import custom
 from openpilot.sunnypilot.modeld_v2.meta_20hz import Meta20hz
 from openpilot.sunnypilot.models.helpers import get_active_bundle

+ModelBundle = custom.ModelManagerSP.ModelBundle
+

 def load_meta_constants():
+  """
+  Determines and loads the appropriate meta model class based on the metadata provided. The function checks
+  specific keys and conditions within the provided metadata dictionary to identify the corresponding meta
+  model class to return.
+
+  :param model_metadata: Dictionary containing metadata about the model. It includes
+      details such as input shapes, output slices, and other configurations for identifying
+      metadata-dependent meta model classes.
+  :type model_metadata: dict
+  :return: The appropriate meta model class (Meta, MetaSimPose, or MetaTombRaider)
+      based on the conditions and metadata provided.
+  :rtype: type
+  """
  if (bundle := get_active_bundle()) and bundle.is20hz:
    return Meta20hz
-  return Meta
+
+  return Meta  # Default
@@ -6,7 +6,6 @@ This file is part of sunnypilot and is licensed under the MIT License.
 See the LICENSE.md file in the root directory for more details.
 """

-from collections.abc import Callable
 import os
 os.environ['GMMU'] = '0'
 import numpy as np
@@ -38,18 +37,11 @@ from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper
 from openpilot.selfdrive.controls.lib.drive_helpers import get_accel_from_plan, smooth_value
 from openpilot.selfdrive.modeld.modeld import ChestnutState

-from openpilot.selfdrive.modeld.compile_modeld import (
-  MODELD_INPUTS,
-  make_input_queues as make_stock_input_queues,
-)
 from openpilot.sunnypilot.modeld_v2.fill_model_msg import fill_model_msg, fill_pose_msg, PublishState, get_curvature_from_output
-from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
+from openpilot.sunnypilot.modeld_v2.constants import Plan
 from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
 from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
-from openpilot.sunnypilot.modeld_v2.compile_modeld import (derive_frame_skip, make_split_input_queues,
-                                                           make_supercombo_input_queues, nv12_copy_size,
-                                                           WARP_INPUTS, POLICY_INPUTS)
+from openpilot.sunnypilot.modeld_v2.compile_modeld import derive_frame_skip, make_split_input_queues, make_supercombo_input_queues, WARP_INPUTS, POLICY_INPUTS
 from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
 from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
 from openpilot.sunnypilot.models.helpers import get_active_bundle
@@ -118,40 +110,36 @@ class ModelState(ModelStateBase):
    cloudlog.warning(f"loading combined pkl: {pkl_path}")
    jits = load_oob(open_file_chunked(pkl_path))

-    metadata = jits['metadata']
-    self.WARP_DEV = metadata.get('warp_dev', 'QCOM') if COMMA_HARDWARE else 'CPU'
-    self.DEV = ('AMD' if self.chestnut else 'QCOM') if COMMA_HARDWARE else 'CPU'
+    self.WARP_DEV = 'QCOM' if COMMA_HARDWARE else 'CPU'
+    self.DEV = 'AMD' if self.chestnut else self.WARP_DEV
    self.QUEUE_DEV = self.DEV
-    self.is_run_model = 'run_model' in jits
+    metadata = jits['metadata']

-    nv12_info = get_nv12_info(cam_w, cam_h)
-    self.frame_copy_size = nv12_copy_size(*nv12_info[:3])
-    self.full_frames: dict = {}
-    self._blob_cache: dict = {}
-    self.frame_buffers: dict = {}
+    self.is_legacy_model = 'run_policy' not in jits  # remove after next recompile
+    if self.is_legacy_model:
+      self.warp = jits[(cam_w, cam_h)]['warp_enqueue']
+      self.run_policy = jits[(cam_w, cam_h)]['run_policy']
+    else:
+      self.run_policy = jits['run_policy']
+      self.warp = jits[(cam_w, cam_h)]

-    if self.is_run_model or 'model' in metadata:
-      model_metadata = metadata.get('model', metadata)
-      self.input_shapes = model_metadata['input_shapes']
+    if 'model' in metadata:
+      model_metadata = metadata['model']
      self.vision_output_slices = model_metadata['output_slices']
      self.policy_output_slices = {}
      self._policy_slices_list = []
      self._combined_model_type = 'supercombo'
-      self._vision_input_names = [key for key in self.input_shapes if 'img' in key]
-      self.frame_skip = derive_frame_skip({}, self.input_shapes)
-      if self.is_run_model:
-        self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
-          self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
-        self.frame_views, self.npy = self.frame_buffers, self.numpy_inputs
-        self.run_model, self.run_policy, self.warp = jits['run_model'][(cam_w, cam_h)], None, None
-      else:
-        self.input_queues, self.numpy_inputs = make_supercombo_input_queues(self.input_shapes, self.frame_skip, device=self.QUEUE_DEV)
-        self.run_model, self.run_policy, self.warp = None, jits['run_policy'], jits[(cam_w, cam_h)]
+      self._vision_input_names = [key for key in model_metadata['input_shapes'] if 'img' in key]
+      frame_skip = derive_frame_skip({}, model_metadata['input_shapes'])
+      self.input_queues, self.numpy_inputs = make_supercombo_input_queues(model_metadata['input_shapes'],
+                                                                          frame_skip, device=self.QUEUE_DEV)
    else:
-      self.run_model, self.run_policy, self.warp = None, jits['run_policy'], jits[(cam_w, cam_h)]
      vision_metadata = metadata['vision']
-      policy_keys = [k for k in metadata if k not in ('vision', 'warp_dev')]
-      self._combined_model_type = 'split' if policy_keys == ['policy'] else 'multi_policy'
+      policy_keys = [k for k in metadata if k != 'vision']
+      if policy_keys == ['policy']:
+        self._combined_model_type = 'split'
+      else:
+        self._combined_model_type = 'multi_policy'
      self.vision_output_slices = vision_metadata['output_slices']
      self._policy_keys = policy_keys
      self._policy_slices_list = [metadata[k]['output_slices'] for k in policy_keys]
@@ -167,39 +155,54 @@ class ModelState(ModelStateBase):
    self._desire_key = next(key for key in self.numpy_inputs if key.startswith('desire'))
    self._road_key = next(key for key in self._vision_input_names if 'big' not in key)
    self._wide_key = next(key for key in self._vision_input_names if 'big' in key)
-    self.frame_buf_params = dict.fromkeys(self._vision_input_names, nv12_info)

    is_20hz = bundle.is20hz if bundle else self._combined_model_type in ('split', 'multi_policy')
    if is_20hz:
      from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
      self.constants = SplitModelConstants()
    else:
+      from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
      self.constants = ModelConstants()

-    self.parser = Parser()
-    self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
+    if self._combined_model_type != 'supercombo':
+      from openpilot.sunnypilot.modeld_v2.parse_model_outputs_split import Parser as SplitParser
+      self.parser = SplitParser()
+    else:
+      from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser as CombinedParser
+      self.parser = CombinedParser()

-    if self.warp is not None:
-      self.full_frames = {k: Tensor(np.zeros(nv12_info[3], dtype=np.uint8), device=self.WARP_DEV).contiguous().realize() for k in self._vision_input_names}
-      self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[self._road_key], big_frame=self.full_frames[self._wide_key])
+    self.prev_desire = np.zeros(self.constants.DESIRE_LEN, dtype=np.float32)
+    self.full_frames: dict = {}
+    self._blob_cache: dict = {}
+    nv12_info = get_nv12_info(cam_w, cam_h)
+    self.frame_buf_params = dict.fromkeys(self._vision_input_names, nv12_info)
+
+    yuv_size = self.frame_buf_params[self._road_key][3]
+    frame_tensor = Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize()
+    big_frame_tensor = Tensor(np.zeros(yuv_size, dtype=np.uint8), device=self.WARP_DEV).contiguous().realize()
+
+    if self.is_legacy_model: # Remove this conditional hack after recompile
+      self.warp(**self.input_queues, frame=frame_tensor, big_frame=big_frame_tensor)
+    else:
+      self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=frame_tensor, big_frame=big_frame_tensor)

  def warmup(self) -> None:
-    dummy_size = self.frame_copy_size if self.is_run_model else self.frame_buf_params[self._road_key][3]
-    dummy_frames = {k: np.zeros(dummy_size, dtype=np.uint8) for k in self._vision_input_names}
+    dummy_frames = {k: np.zeros(self.frame_buf_params[k][3], dtype=np.uint8) for k in self._vision_input_names}
    transforms = {k: np.eye(3, dtype=np.float32) for k in [self._road_key, self._wide_key] if k}
-    dummy_inputs = {k: np.zeros(v.shape, dtype=v.dtype) for k, v in self.numpy_inputs.items() if k not in ['tfm', 'big_tfm', 'prev_feat']}
-    self.run(dummy_frames, transforms, dummy_inputs)
-    if self.is_run_model:
-      self.input_queues, self.numpy_inputs, self.frame_buffers = make_stock_input_queues(
-        self.input_shapes, self.frame_skip, device=self.DEV, frame_copy_size=self.frame_copy_size)
-      self.frame_views = self.frame_buffers
-      self.npy = self.numpy_inputs
-    else:
-      for v in self.numpy_inputs.values():
-        v[:] = 0
-      self.full_frames.clear()
-      self._blob_cache.clear()
+
+    dummy_inputs = {}
+    for k, v in self.numpy_inputs.items():
+      if k not in ['tfm', 'big_tfm', 'prev_feat']:
+        dummy_inputs[k] = np.zeros(v.shape, dtype=v.dtype)
+
+    self.run(dummy_frames, transforms, dummy_inputs, prepare_only=False)
+
+    for v in self.numpy_inputs.values():
+      v[:] = 0
    self.prev_desire[:] = 0
+    self.full_frames.clear()
+    self._blob_cache.clear()
+

  @property
  def mlsim(self) -> bool:
@@ -214,50 +217,45 @@ class ModelState(ModelStateBase):
    return self._desire_key

  def run(self, bufs: dict[str, VisionBuf], transforms: dict[str, np.ndarray],
-          inputs: dict[str, np.ndarray],
-          after_enqueue: Callable[[], None] | None = None) -> dict[str, np.ndarray] | None:
-    if self.is_run_model:
-      for key, buf in bufs.items():
-        data = buf.data if hasattr(buf, 'data') else buf
-        np.copyto(self.frame_buffers[key], np.frombuffer(data, dtype=np.uint8, count=self.frame_copy_size))
-    else:
-      for key, buf in bufs.items():
-        ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
-        cache_key = (key, ptr)
-        if cache_key not in self._blob_cache:
-          self._blob_cache[cache_key] = Tensor.from_blob(ptr, (self.frame_buf_params[key][3],), dtype='uint8', device=self.WARP_DEV)
-        self.full_frames[key] = self._blob_cache[cache_key]
+                inputs: dict[str, np.ndarray], prepare_only: bool) -> dict[str, np.ndarray] | None:
+    for key in bufs.keys():
+      ptr = np.frombuffer(bufs[key].data, dtype=np.uint8).ctypes.data
+      yuv_size = self.frame_buf_params[key][3]
+      cache_key = (key, ptr)
+      if cache_key not in self._blob_cache:
+        self._blob_cache[cache_key] = Tensor.from_blob(ptr, (yuv_size,), dtype='uint8', device=self.WARP_DEV)
+      self.full_frames[key] = self._blob_cache[cache_key]

    desire_key = self.desire_key
    inputs[desire_key][0] = 0
    self.numpy_inputs[desire_key][:] = np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
    self.prev_desire[:] = inputs[desire_key]
-
    for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
      if key in self.numpy_inputs and key in inputs:
        self.numpy_inputs[key][:] = inputs[key]

-    self.numpy_inputs['tfm'][:, :] = transforms[self._road_key].reshape(3, 3)
-    self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
+    road_key = self._road_key
+    wide_key = self._wide_key
+    self.numpy_inputs['tfm'][:, :] = transforms[road_key].reshape(3, 3)
+    self.numpy_inputs['big_tfm'][:, :] = transforms[wide_key].reshape(3, 3)

-    if self.run_model is not None:
-      outs, = self.run_model(**{k: self.input_queues[k] for k in MODELD_INPUTS})
-      raw_outputs = outs
+    if self.is_legacy_model:  # remove after next recompile
+      if prepare_only:
+        self.warp(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
+        return None
+      raw_outputs = self.run_policy(**self.input_queues, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
    else:
-      assert self.warp is not None and self.run_policy is not None
-      warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[self._road_key], big_frame=self.full_frames[self._wide_key])
+      if prepare_only:
+        self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
+        return None
+      warped = self.warp(**{k: self.input_queues[k] for k in WARP_INPUTS}, frame=self.full_frames[road_key], big_frame=self.full_frames[wide_key])
      raw_outputs = self.run_policy(**{k: self.input_queues[k] for k in POLICY_INPUTS if k in self.input_queues}, warped=warped)

-    if after_enqueue is not None:
-      after_enqueue()
-
    if self._combined_model_type == 'supercombo':
      model_output = raw_outputs.numpy().flatten()
-      if self.chestnut and not np.all(np.isfinite(model_output)):
-        raise RuntimeError("model output not finite")
      sliced = {k: model_output[np.newaxis, v] for k, v in self.vision_output_slices.items()}
      outputs = self.parser.parse_outputs(sliced)
-      if 'prev_feat' in self.numpy_inputs and 'hidden_state' in self.vision_output_slices:
+      if 'prev_feat' in self.numpy_inputs:
        self.numpy_inputs['prev_feat'][:] = model_output[self.vision_output_slices['hidden_state']]
    else:
      vision_output = raw_outputs[0].numpy().flatten()
@@ -287,6 +285,9 @@ class ModelState(ModelStateBase):
      buf[0, :-1] = buf[0, 1:]
      buf[0, -1, :] = outputs['desired_curvature'][0, :] if not self.mlsim else 0

+    if self.chestnut and not np.all(np.isfinite(outputs.get('plan', np.array([0.])))):
+      raise RuntimeError("model output not finite")
+
    return outputs

  def get_action_from_model(self, model_output: dict[str, np.ndarray], prev_action: log.ModelDataV2.Action,
@@ -372,11 +373,7 @@ def main(demo=False):
    loader.start()
    loader.join(BIG_MODEL_TIMEOUT)
    model = big_model
-    if model is None:
-      params.put_bool("ChestnutModelError", True)
    params.put_bool("ChestnutActive", model is not None)
-    if model is not None:
-      params.remove("ChestnutModelError")

  small_model = ModelState(cam_w=vipc_client_main.width, cam_h=vipc_client_main.height, chestnut=False) if model is None or CHESTNUT else None
  if model is None:
@@ -490,6 +487,9 @@ def main(demo=False):
    run_count = run_count + 1

    frame_drop_ratio = frames_dropped / (1 + frames_dropped)
+    prepare_only = vipc_dropped_frames > 0
+    if prepare_only:
+      cloudlog.error(f"skipping model eval. Dropped {vipc_dropped_frames} frames")

    bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
    transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
@@ -512,14 +512,11 @@ def main(demo=False):

    mt1 = time.perf_counter()
    try:
-      send_chestnut = (chestnut_state is not None and
-                       run_count % round(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0)
-      model_output = model.run(bufs, transforms, inputs, chestnut_state.send if send_chestnut else None)
+      model_output = model.run(bufs, transforms, inputs, prepare_only)
    except Exception:
      if not params.get_bool("ChestnutActive"):
        raise
      cloudlog.exception("chestnut failed, falling back to small")
-      params.put_bool("ChestnutModelError", True)
      params.put_bool("ChestnutActive", False)
      assert small_model is not None
      model = small_model
@@ -562,6 +559,9 @@ def main(demo=False):
      pm.send('modelDataV2SP', mdv2sp_send)
    last_vipc_frame_id = meta_main.frame_id

+    if chestnut_state is not None and run_count % round(model.constants.MODEL_FREQ / SERVICE_LIST['chestnutState'].frequency) == 0:
+      chestnut_state.send()
+
 if __name__ == "__main__":
  try:
    import argparse
@@ -115,41 +115,22 @@ class Parser:
    outs[name + '_stds'] = pred_std_final.reshape(final_shape)

  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    if 'plan' in outs:
-      self.parse_mdn('plan', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'planplus' in outs:
-      self.parse_mdn('planplus', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
-    if 'lane_lines' in outs:
-      self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'road_edges' in outs:
-      self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
-    if 'pose' in outs:
-      self.parse_mdn('pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'road_transform' in outs:
-      self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
+    # supercombo (4955 / 102) and newer variants (e.g. 990 / 144).
+    self.parse_mdn('plan',   outs, out_shape=(ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH))
+    self.parse_mdn('lane_lines', outs, out_shape=(ModelConstants.NUM_LANE_LINES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('road_edges', outs, out_shape=(ModelConstants.NUM_ROAD_EDGES, ModelConstants.IDX_N, ModelConstants.LANE_LINES_WIDTH))
+    self.parse_mdn('pose',         outs, out_shape=(ModelConstants.POSE_WIDTH,))
+    self.parse_mdn('road_transform', outs, out_shape=(ModelConstants.POSE_WIDTH,))
    if 'sim_pose' in outs:
      self.parse_mdn('sim_pose', outs, out_shape=(ModelConstants.POSE_WIDTH,))
-    if 'wide_from_device_euler' in outs:
-      self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
-    if 'lead' in outs:
-      self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
+    self.parse_mdn('wide_from_device_euler', outs, out_shape=(ModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('lead', outs, out_shape=(ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH))
    if 'lat_planner_solution' in outs:
      self.parse_mdn('lat_planner_solution', outs, out_shape=(ModelConstants.IDX_N, ModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
    if 'desired_curvature' in outs:
      self.parse_mdn('desired_curvature', outs, out_shape=(ModelConstants.DESIRED_CURV_WIDTH,))
-    if 'action' in outs:
-      self.parse_mdn('action', outs, out_shape=(ModelConstants.ACTION_WIDTH,))
    for k in ['lead_prob', 'lane_lines_prob', 'meta']:
-      if k in outs:
-        self.parse_binary_crossentropy(k, outs)
-    if 'desire_state' in outs:
-      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
-    if 'desire_pred' in outs:
-      self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
+      self.parse_binary_crossentropy(k, outs)
+    self.parse_categorical_crossentropy('desire_state', outs, out_shape=(ModelConstants.DESIRE_PRED_WIDTH,))
+    self.parse_categorical_crossentropy('desire_pred',  outs, out_shape=(ModelConstants.DESIRE_PRED_LEN, ModelConstants.DESIRE_PRED_WIDTH))
    return outs
-
-  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    return self.parse_outputs(outs)
-
-  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
-    return self.parse_outputs(outs)
@@ -0,0 +1,159 @@
+import numpy as np
+from openpilot.sunnypilot.models.split_model_constants import SplitModelConstants
+
+
+def safe_exp(x, out=None):
+  # -11 is around 10**14, more causes float16 overflow
+  return np.exp(np.clip(x, -np.inf, 11), out=out)
+
+
+def sigmoid(x):
+  return 1. / (1. + safe_exp(-x))
+
+
+def softmax(x, axis=-1):
+  x -= np.max(x, axis=axis, keepdims=True)
+  if x.dtype == np.float32 or x.dtype == np.float64:
+    safe_exp(x, out=x)
+  else:
+    x = safe_exp(x)
+  x /= np.sum(x, axis=axis, keepdims=True)
+  return x
+
+
+class Parser:
+  def __init__(self, ignore_missing=False):
+    self.ignore_missing = ignore_missing
+
+  def check_missing(self, outs, name):
+    if name not in outs and not self.ignore_missing:
+      raise ValueError(f"Missing output {name}")
+    return name not in outs
+
+  def parse_categorical_crossentropy(self, name, outs, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    if out_shape is not None:
+      raw = raw.reshape((raw.shape[0],) + out_shape)
+    outs[name] = softmax(raw, axis=-1)
+
+  def parse_binary_crossentropy(self, name, outs):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    outs[name] = sigmoid(raw)
+
+  def parse_mdn(self, name, outs, in_N=0, out_N=1, out_shape=None):
+    if self.check_missing(outs, name):
+      return
+    raw = outs[name]
+    raw = raw.reshape((raw.shape[0], max(in_N, 1), -1))
+
+    n_values = (raw.shape[2] - out_N)//2
+    pred_mu = raw[:,:,:n_values]
+    pred_std = safe_exp(raw[:,:,n_values: 2*n_values])
+
+    if in_N > 1:
+      weights = np.zeros((raw.shape[0], in_N, out_N), dtype=raw.dtype)
+      for i in range(out_N):
+        weights[:,:,i - out_N] = softmax(raw[:,:,i - out_N], axis=-1)
+
+      if out_N == 1:
+        for fidx in range(weights.shape[0]):
+          idxs = np.argsort(weights[fidx][:,0])[::-1]
+          weights[fidx] = weights[fidx][idxs]
+          pred_mu[fidx] = pred_mu[fidx][idxs]
+          pred_std[fidx] = pred_std[fidx][idxs]
+      assert out_shape is not None
+      full_shape = tuple([raw.shape[0], in_N] + list(out_shape))
+      outs[name + '_weights'] = weights
+      outs[name + '_hypotheses'] = pred_mu.reshape(full_shape)
+      outs[name + '_stds_hypotheses'] = pred_std.reshape(full_shape)
+
+      pred_mu_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      pred_std_final = np.zeros((raw.shape[0], out_N, n_values), dtype=raw.dtype)
+      for fidx in range(weights.shape[0]):
+        for hidx in range(out_N):
+          idxs = np.argsort(weights[fidx,:,hidx])[::-1]
+          pred_mu_final[fidx, hidx] = pred_mu[fidx, idxs[0]]
+          pred_std_final[fidx, hidx] = pred_std[fidx, idxs[0]]
+    else:
+      pred_mu_final = pred_mu
+      pred_std_final = pred_std
+
+    if out_N > 1:
+      assert out_shape is not None
+      final_shape = tuple([raw.shape[0], out_N] + list(out_shape))
+    else:
+      assert out_shape is not None
+      final_shape = tuple([raw.shape[0],] + list(out_shape))
+    outs[name] = pred_mu_final.reshape(final_shape)
+    outs[name + '_stds'] = pred_std_final.reshape(final_shape)
+
+  def is_mhp(self, outs, name, shape):
+    if self.check_missing(outs, name):
+      return False
+    if outs[name].shape[1] == 2 * shape:
+      return False
+    return True
+
+  def parse_dynamic_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'lead' in outs:
+      lead_mhp = self.is_mhp(outs, 'lead',
+                             SplitModelConstants.LEAD_MHP_SELECTION * SplitModelConstants.LEAD_TRAJ_LEN * SplitModelConstants.LEAD_WIDTH)
+      lead_in_N, lead_out_N = (SplitModelConstants.LEAD_MHP_N, SplitModelConstants.LEAD_MHP_SELECTION) if lead_mhp else (0, 0)
+      lead_out_shape = (SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH) if lead_mhp else \
+        (SplitModelConstants.LEAD_MHP_SELECTION, SplitModelConstants.LEAD_TRAJ_LEN, SplitModelConstants.LEAD_WIDTH)
+      self.parse_mdn('lead', outs, in_N=lead_in_N, out_N=lead_out_N, out_shape=lead_out_shape)
+    if 'plan' in outs:
+      plan_mhp = self.is_mhp(outs, 'plan', SplitModelConstants.IDX_N * SplitModelConstants.PLAN_WIDTH)
+      plan_in_N, plan_out_N = (SplitModelConstants.PLAN_MHP_N, SplitModelConstants.PLAN_MHP_SELECTION) if plan_mhp else (0, 0)
+      self.parse_mdn('plan', outs, in_N=plan_in_N, out_N=plan_out_N,
+                     out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH))
+    if 'planplus' in outs:
+      self.parse_mdn('planplus', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N, SplitModelConstants.PLAN_WIDTH))
+
+  def split_outputs(self, outs: dict[str, np.ndarray]) -> None:
+    if 'desired_curvature' in outs:
+      self.parse_mdn('desired_curvature', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.DESIRED_CURV_WIDTH,))
+    if 'desire_pred' in outs:
+      self.parse_categorical_crossentropy('desire_pred', outs, out_shape=(SplitModelConstants.DESIRE_PRED_LEN,SplitModelConstants.DESIRE_PRED_WIDTH))
+    if 'desire_state' in outs:
+      self.parse_categorical_crossentropy('desire_state', outs, out_shape=(SplitModelConstants.DESIRE_PRED_WIDTH,))
+    if 'lane_lines' in outs:
+      self.parse_mdn('lane_lines', outs, in_N=0, out_N=0,
+                     out_shape=(SplitModelConstants.NUM_LANE_LINES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+    if 'lane_lines_prob' in outs:
+      self.parse_binary_crossentropy('lane_lines_prob', outs)
+    if 'lead_prob' in outs:
+      self.parse_binary_crossentropy('lead_prob', outs)
+    if 'lat_planner_solution' in outs:
+      self.parse_mdn('lat_planner_solution', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.IDX_N,SplitModelConstants.LAT_PLANNER_SOLUTION_WIDTH))
+    if 'meta' in outs:
+      self.parse_binary_crossentropy('meta', outs)
+    if 'road_edges' in outs:
+      self.parse_mdn('road_edges', outs, in_N=0, out_N=0,
+                     out_shape=(SplitModelConstants.NUM_ROAD_EDGES,SplitModelConstants.IDX_N,SplitModelConstants.LANE_LINES_WIDTH))
+    if 'sim_pose' in outs:
+      self.parse_mdn('sim_pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    if 'action' in outs:
+      self.parse_mdn('action', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.ACTION_WIDTH,))
+
+  def parse_vision_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self.parse_mdn('pose', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    self.parse_mdn('wide_from_device_euler', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.WIDE_FROM_DEVICE_WIDTH,))
+    self.parse_mdn('road_transform', outs, in_N=0, out_N=0, out_shape=(SplitModelConstants.POSE_WIDTH,))
+    self.parse_dynamic_outputs(outs)
+    self.split_outputs(outs)
+    return outs
+
+  def parse_policy_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    self.parse_dynamic_outputs(outs)
+    self.split_outputs(outs)
+    return outs
+
+  def parse_outputs(self, outs: dict[str, np.ndarray]) -> dict[str, np.ndarray]:
+    outs = self.parse_vision_outputs(outs)
+    outs = self.parse_policy_outputs(outs)
+    return outs
@@ -117,7 +117,7 @@ ARCHETYPES = {
    is_20hz=True,
    expected_model_type='split',
    expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
    expected_desire_key='desire',
  ),
  'vision_multi_policy': Archetype(
@@ -130,7 +130,7 @@ ARCHETYPES = {
    is_20hz=True,
    expected_model_type='multi_policy',
    expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
    expected_desire_key='desire',
  ),
  'tri_policy': Archetype(
@@ -144,7 +144,7 @@ ARCHETYPES = {
    is_20hz=True,
    expected_model_type='multi_policy',
    expected_constants_class=SplitModelConstants,
-    expected_parser_module='parse_model_outputs',
+    expected_parser_module='parse_model_outputs_split',
    expected_desire_key='desire',
  ),
  'supercombo_non20hz': Archetype(
@@ -103,23 +103,6 @@ class TestStockEquivalence(OpenpilotTestCase):
    assert state.vision_output_slices == arch.metadata_structure['vision']['output_slices']
    assert state.policy_output_slices == arch.metadata_structure['policy']['output_slices']

-  def test_unified_run_model(self, tmp_path, monkeypatch, patch_modeld):
-    from openpilot.common.hardware import hw
-    from openpilot.selfdrive.modeld.helpers import dump_oob
-    shapes = {'img': (1, 12, 128, 256), 'big_img': (1, 12, 128, 256), 'features_buffer': (1, 24, 32, 512),
-              'desire_pulse': (1, 25, 8), 'traffic_convention': (1, 2), 'action_t': (1, 2)}
-    pkl_data = {'metadata': {'model': {'input_shapes': shapes, 'output_slices': {}}},
-                'run_model': {(CAM_W, CAM_H): tests_helpers._noop_jit}}
-    with open(tmp_path / 'driving_test_tinygrad.pkl', 'wb') as f:
-      dump_oob(pkl_data, f)
-    bundle = DummyBundle(models=[DummyModel('supercombo', 'driving_test_tinygrad.pkl')])
-    patch_modeld(bundle)
-    monkeypatch.setattr(hw.Paths, 'model_root', staticmethod(lambda: str(tmp_path)))
-    state = ModelState(cam_w=CAM_W, cam_h=CAM_H)
-    assert state.is_run_model and state.run_model is not None
-    assert state.run_policy is None and state.warp is None
-    assert 'img' in state.frame_views and 'big_img' in state.frame_views
-

 ARCHETYPE_NAMES = list(ARCHETYPES.keys())

@@ -1,81 +0,0 @@
-import numpy as np
-from openpilot.common.test import OpenpilotTestCase
-from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
-from openpilot.sunnypilot.modeld_v2.parse_model_outputs import Parser, _infer_mhp, sigmoid, softmax
-
-
-class TestParseModelOutputs(OpenpilotTestCase):
-  def test_infer_mhp_lead(self):
-    in_hypotheses, out_selections = _infer_mhp(102, 24)
-    assert in_hypotheses == 2
-    assert out_selections == 3
-
-  def test_infer_mhp_plan(self):
-    in_hypotheses, out_selections = _infer_mhp(4955, 495)
-    assert in_hypotheses == 5
-    assert out_selections == 1
-
-  def test_infer_mhp_non_mdn(self):
-    in_hypotheses, out_selections = _infer_mhp(48, 24)
-    assert in_hypotheses == 1
-    assert out_selections == 0
-
-  def test_check_missing_raises(self):
-    parser = Parser(ignore_missing=False)
-    with self.assertRaises(ValueError):
-      parser.check_missing({}, "missing_key")
-
-  def test_check_missing_ignored(self):
-    parser = Parser(ignore_missing=True)
-    assert parser.check_missing({}, "missing_key") is True
-
-  def test_binary_crossentropy(self):
-    parser = Parser()
-    raw_logits = np.array([[-10.0, 0.0, 10.0]], dtype=np.float32)
-    outs = {"meta": raw_logits.copy()}
-    parser.parse_binary_crossentropy("meta", outs)
-    expected_probabilities = sigmoid(raw_logits)
-    np.testing.assert_allclose(outs["meta"], expected_probabilities, rtol=1e-5, atol=1e-6)
-
-  def test_categorical_crossentropy(self):
-    parser = Parser()
-    raw_logits = np.array([[1.0, 2.0, 3.0]], dtype=np.float32)
-    outs = {"desire_state": raw_logits.copy()}
-    parser.parse_categorical_crossentropy("desire_state", outs)
-    expected_probabilities = softmax(raw_logits)
-    np.testing.assert_allclose(outs["desire_state"], expected_probabilities, rtol=1e-5, atol=1e-6)
-
-  def test_parse_vision_outputs(self):
-    parser = Parser()
-    pose_raw = np.zeros((1, ModelConstants.POSE_WIDTH * 2), dtype=np.float32)
-    road_transform_raw = np.zeros((1, ModelConstants.POSE_WIDTH * 2), dtype=np.float32)
-    lead_raw = np.zeros((1, 102), dtype=np.float32)
-    meta_raw = np.zeros((1, 55), dtype=np.float32)
-    vision_outputs = {"pose": pose_raw, "road_transform": road_transform_raw, "lead": lead_raw, "meta": meta_raw}
-    parsed = parser.parse_vision_outputs(vision_outputs)
-    assert "pose" in parsed
-    assert "road_transform" in parsed
-    assert "lead" in parsed
-    assert "meta" in parsed
-    assert parsed["pose"].shape == (1, ModelConstants.POSE_WIDTH)
-    assert parsed["lead"].shape == (1, ModelConstants.LEAD_MHP_SELECTION, ModelConstants.LEAD_TRAJ_LEN, ModelConstants.LEAD_WIDTH)
-
-  def test_parse_policy_outputs(self):
-    parser = Parser()
-    plan_raw = np.zeros((1, 4955), dtype=np.float32)
-    desire_state_raw = np.zeros((1, ModelConstants.DESIRE_PRED_WIDTH), dtype=np.float32)
-    action_raw = np.zeros((1, ModelConstants.ACTION_WIDTH * 2), dtype=np.float32)
-    policy_outputs = {"plan": plan_raw, "desire_state": desire_state_raw, "action": action_raw}
-    parsed = parser.parse_policy_outputs(policy_outputs)
-    assert parsed["plan"].shape == (1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)
-    assert parsed["action"].shape == (1, ModelConstants.ACTION_WIDTH)
-    assert parsed["desire_state"].shape == (1, ModelConstants.DESIRE_PRED_WIDTH)
-
-  def test_parse_outputs_combined(self):
-    parser = Parser()
-    outputs = {"plan": np.zeros((1, 4955), dtype=np.float32), "pose": np.zeros((1, ModelConstants.POSE_WIDTH * 2),
-               dtype=np.float32), "meta": np.zeros((1, 55), dtype=np.float32)}
-    parsed = parser.parse_outputs(outputs)
-    assert parsed["plan"].shape == (1, ModelConstants.IDX_N, ModelConstants.PLAN_WIDTH)
-    assert parsed["pose"].shape == (1, ModelConstants.POSE_WIDTH)
-    assert parsed["meta"].shape == (1, 55)
@@ -0,0 +1,121 @@
+import numpy as np
+
+def index_function(idx, max_val=192, max_idx=32):
+  return max_val * ((idx/max_idx)**2)
+
+
+class ModelConstants:
+  # time and distance indices
+  IDX_N = 33
+  T_IDXS = [index_function(idx, max_val=10.0) for idx in range(IDX_N)]
+  X_IDXS = [index_function(idx, max_val=192.0) for idx in range(IDX_N)]
+  LEAD_T_IDXS = [0., 2., 4., 6., 8., 10.]
+  LEAD_T_OFFSETS = [0., 2., 4.]
+  META_T_IDXS = [2., 4., 6., 8., 10.]
+
+  # model inputs constants
+  MODEL_FREQ = 20
+  FEATURE_LEN = 512
+  HISTORY_BUFFER_LEN = 99
+  DESIRE_LEN = 8
+  TRAFFIC_CONVENTION_LEN = 2
+  NAV_FEATURE_LEN = 256
+  NAV_INSTRUCTION_LEN = 150
+  LAT_PLANNER_STATE_LEN = 4
+  LATERAL_CONTROL_PARAMS_LEN = 2
+  PREV_DESIRED_CURV_LEN = 1
+
+  # model outputs constants
+  FCW_THRESHOLDS_5MS2 = np.array([.05, .05, .15, .15, .15], dtype=np.float32)
+  FCW_THRESHOLDS_3MS2 = np.array([.7, .7], dtype=np.float32)
+  FCW_5MS2_PROBS_WIDTH = 5
+  FCW_3MS2_PROBS_WIDTH = 2
+
+  DISENGAGE_WIDTH = 5
+  POSE_WIDTH = 6
+  WIDE_FROM_DEVICE_WIDTH = 3
+  SIM_POSE_WIDTH = 6
+  LEAD_WIDTH = 4
+  LANE_LINES_WIDTH = 2
+  ROAD_EDGES_WIDTH = 2
+  PLAN_WIDTH = 15
+  DESIRE_PRED_WIDTH = 8
+  LAT_PLANNER_SOLUTION_WIDTH = 4
+  DESIRED_CURV_WIDTH = 1
+
+  NUM_LANE_LINES = 4
+  NUM_ROAD_EDGES = 2
+
+  LEAD_TRAJ_LEN = 6
+  DESIRE_PRED_LEN = 4
+
+  PLAN_MHP_N = 5
+  LEAD_MHP_N = 2
+  PLAN_MHP_SELECTION = 1
+  LEAD_MHP_SELECTION = 3
+
+  FCW_THRESHOLD_5MS2_HIGH = 0.15
+  FCW_THRESHOLD_5MS2_LOW = 0.05
+  FCW_THRESHOLD_3MS2 = 0.7
+
+  CONFIDENCE_BUFFER_LEN = 5
+  RYG_GREEN = 0.01165
+  RYG_YELLOW = 0.06157
+
+  POLY_PATH_DEGREE = 4
+
+
+# model outputs slices
+class Plan:
+  POSITION = slice(0, 3)
+  VELOCITY = slice(3, 6)
+  ACCELERATION = slice(6, 9)
+  T_FROM_CURRENT_EULER = slice(9, 12)
+  ORIENTATION_RATE = slice(12, 15)
+
+
+class Meta:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 31, 6)
+  BRAKE_DISENGAGE = slice(2, 31, 6)
+  STEER_OVERRIDE = slice(3, 31, 6)
+  HARD_BRAKE_3 = slice(4, 31, 6)
+  HARD_BRAKE_4 = slice(5, 31, 6)
+  HARD_BRAKE_5 = slice(6, 31, 6)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  GAS_PRESS = slice(31, 55, 4)
+  BRAKE_PRESS = slice(32, 55, 4)
+  LEFT_BLINKER = slice(33, 55, 4)
+  RIGHT_BLINKER = slice(34, 55, 4)
+
+
+class MetaTombRaider:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 41, 8)
+  BRAKE_DISENGAGE = slice(2, 41, 8)
+  STEER_OVERRIDE = slice(3, 41, 8)
+  HARD_BRAKE_3 = slice(4, 41, 8)
+  HARD_BRAKE_4 = slice(5, 41, 8)
+  HARD_BRAKE_5 = slice(6, 41, 8)
+  GAS_PRESS = slice(7, 41, 8)
+  BRAKE_PRESS = slice(8, 41, 8)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(41, 53, 2)
+  RIGHT_BLINKER = slice(42, 53, 2)
+
+
+class MetaSimPose:
+  ENGAGED = slice(0, 1)
+  # next 2, 4, 6, 8, 10 seconds
+  GAS_DISENGAGE = slice(1, 36, 7)
+  BRAKE_DISENGAGE = slice(2, 36, 7)
+  STEER_OVERRIDE = slice(3, 36, 7)
+  HARD_BRAKE_3 = slice(4, 36, 7)
+  HARD_BRAKE_4 = slice(5, 36, 7)
+  HARD_BRAKE_5 = slice(6, 36, 7)
+  GAS_PRESS = slice(7, 36, 7)
+  # next 0, 2, 4, 6, 8, 10 seconds
+  LEFT_BLINKER = slice(36, 48, 2)
+  RIGHT_BLINKER = slice(37, 48, 2)
@@ -139,7 +139,7 @@ class ModelCache:
 class ModelFetcher:
  """Handles fetching and caching of model data from remote source"""
  MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v22.json"
-  MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v25.json"
+  MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v23.json"

  MODEL_SOURCES = {
    "qcom": (MODEL_URL, ""),
@@ -7,11 +7,14 @@ See the LICENSE.md file in the root directory for more details.

 import hashlib
 import os
+import pickle
+from pathlib import Path
 import numpy as np

 from openpilot.cereal import custom
 from openpilot.common.params import Params
 from openpilot.common.swaglog import cloudlog
+from openpilot.sunnypilot.models.constants import Meta, MetaSimPose, MetaTombRaider
 from openpilot.common.hardware.hw import Paths
 from openpilot.selfdrive.modeld.helpers import chestnut_present

@@ -19,6 +22,7 @@ from openpilot.selfdrive.modeld.helpers import chestnut_present
 REQUIRED_JSON_VERSION = 19

 CUSTOM_MODEL_PATH = Paths.model_root()
+METADATA_PATH = Path(__file__).parent / '../models/supercombo_metadata.pkl'
 ModelManager = custom.ModelManagerSP

 ACTIVE_BUNDLE_KEYS = {
@@ -197,6 +201,33 @@ def _get_model():
  return None


+def load_metadata():
+  metadata_path = METADATA_PATH
+
+  with open(metadata_path, 'rb') as f:
+    return pickle.load(f)
+
+
+def prepare_inputs(model_metadata: dict) -> dict[str, np.ndarray]:
+  return {
+    key: np.zeros(shape, dtype=np.float32).flatten()
+    for key, shape in model_metadata['input_shapes'].items()
+    if 'img' not in key
+  }
+
+
+def load_meta_constants(model_metadata: dict):
+  """ Loads the appropriate meta model class based on key shapes"""
+  if 'sim_pose' in model_metadata['input_shapes']:
+    return MetaSimPose
+
+  meta_slice = model_metadata['output_slices']['meta']
+  if (meta_slice.start, meta_slice.stop, meta_slice.step) == (5868, 5921, None):
+    return MetaTombRaider
+
+  return Meta
+
+
 # The following method(s) are modeld helper methods
 def plan_x_idxs_helper(constants, plan, model_output) -> list[float]:
  # times at X_IDXS according to plan.
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
 void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
 void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
-void live_H(double *in_vec, double *out_8536236811475430067);
-void live_err_fun(double *nom_x, double *delta_x, double *out_4614868058485758139);
-void live_inv_err_fun(double *nom_x, double *true_x, double *out_8573554928028298681);
-void live_H_mod_fun(double *state, double *out_6128852447133223565);
-void live_f_fun(double *state, double dt, double *out_7195534889995166041);
-void live_F_fun(double *state, double dt, double *out_9012508306389018670);
-void live_h_4(double *state, double *unused, double *out_2657657745771735915);
-void live_H_4(double *state, double *unused, double *out_1457735078816209632);
-void live_h_9(double *state, double *unused, double *out_181525824636861445);
-void live_H_9(double *state, double *unused, double *out_8744954014080657102);
-void live_h_10(double *state, double *unused, double *out_2940864223365932608);
-void live_H_10(double *state, double *unused, double *out_6591157835936482265);
-void live_h_12(double *state, double *unused, double *out_830811219903448800);
-void live_H_12(double *state, double *unused, double *out_4923523298226523364);
-void live_h_35(double *state, double *unused, double *out_1177115113523305926);
-void live_H_35(double *state, double *unused, double *out_2177960265901509655);
-void live_h_32(double *state, double *unused, double *out_8970203849717307468);
-void live_H_32(double *state, double *unused, double *out_5493453269239321848);
-void live_h_13(double *state, double *unused, double *out_1337448462981849436);
-void live_H_13(double *state, double *unused, double *out_2400089242629978824);
-void live_h_14(double *state, double *unused, double *out_181525824636861445);
-void live_H_14(double *state, double *unused, double *out_8744954014080657102);
-void live_h_33(double *state, double *unused, double *out_3078698583527751553);
-void live_H_33(double *state, double *unused, double *out_972596738737347949);
+void live_H(double *in_vec, double *out_6359227913389949712);
+void live_err_fun(double *nom_x, double *delta_x, double *out_5070973522734377332);
+void live_inv_err_fun(double *nom_x, double *true_x, double *out_1504386229041325813);
+void live_H_mod_fun(double *state, double *out_8604409008290842398);
+void live_f_fun(double *state, double dt, double *out_122542228347959485);
+void live_F_fun(double *state, double dt, double *out_4079892232653715769);
+void live_h_4(double *state, double *unused, double *out_5353206138509165465);
+void live_H_4(double *state, double *unused, double *out_3934719090872387589);
+void live_h_9(double *state, double *unused, double *out_8807055816776463374);
+void live_H_9(double *state, double *unused, double *out_7224806047572716557);
+void live_h_10(double *state, double *unused, double *out_2897072850418891434);
+void live_H_10(double *state, double *unused, double *out_2938213750100159528);
+void live_h_12(double *state, double *unused, double *out_6098799151588263013);
+void live_H_12(double *state, double *unused, double *out_2446539286170345407);
+void live_h_35(double *state, double *unused, double *out_1955422699431481948);
+void live_H_35(double *state, double *unused, double *out_299023746154668302);
+void live_h_32(double *state, double *unused, double *out_8406014722897533998);
+void live_H_32(double *state, double *unused, double *out_7432545933185735434);
+void live_h_13(double *state, double *unused, double *out_9104729345260618237);
+void live_H_13(double *state, double *unused, double *out_4844093272925809884);
+void live_h_14(double *state, double *unused, double *out_8807055816776463374);
+void live_H_14(double *state, double *unused, double *out_7224806047572716557);
+void live_h_33(double *state, double *unused, double *out_563456123860700114);
+void live_H_33(double *state, double *unused, double *out_948776632190842222);
 void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
 }
@@ -25845,7 +25845,7 @@ __Pyx_RefNannySetupContext("PyInit_ekf_sym_pyx", 0);
  (void)__Pyx_modinit_variable_import_code(__pyx_mstate);
  (void)__Pyx_modinit_function_import_code(__pyx_mstate);
  /*--- Execution code ---*/
-  __Pyx_TraceStartFunc("PyInit_ekf_sym_pyx", __pyx_f[0], 1, 1, 0, 0, __PYX_ERR(0, 1, __pyx_L1_error));
+  __Pyx_TraceStartFunc("PyInit_ekf_sym_pyx", __pyx_f[0], 1, 0, 0, 0, __PYX_ERR(0, 1, __pyx_L1_error));

  /* "View.MemoryView":108
 * 
@@ -26644,7 +26644,7 @@ __Pyx_RefNannySetupContext("PyInit_ekf_sym_pyx", 0);
  __Pyx_GOTREF(__pyx_t_4);
  if (PyDict_SetItem(__pyx_mstate_global->__pyx_d, __pyx_mstate_global->__pyx_n_u_test, __pyx_t_4) < (0)) __PYX_ERR(0, 1, __pyx_L1_error)
  __Pyx_DECREF(__pyx_t_4); __pyx_t_4 = 0;
-  __Pyx_TraceReturnValue(Py_None, 1, 0, __PYX_ERR(0, 1, __pyx_L1_error));
+  __Pyx_TraceReturnValue(Py_None, 0, 0, __PYX_ERR(0, 1, __pyx_L1_error));
  __Pyx_PyMonitoring_ExitScope(0);

  /*--- Wrapped vars code ---*/
@@ -26654,7 +26654,7 @@ __Pyx_RefNannySetupContext("PyInit_ekf_sym_pyx", 0);
  __Pyx_XDECREF(__pyx_t_4);
  __Pyx_XDECREF(__pyx_t_5);
  __Pyx_TraceException(__pyx_lineno, 0, 0);
-  __Pyx_TraceExceptionUnwind(1, 0);
+  __Pyx_TraceExceptionUnwind(0, 0);
  if (__pyx_m) {
    if (__pyx_mstate->__pyx_d && stringtab_initialized) {
      __Pyx_AddTraceback("init rednose.helpers.ekf_sym_pyx", __pyx_clineno, __pyx_lineno, __pyx_filename);
Author	SHA1	Message	Date
github-actions[bot]	85b17a8252	sunnypilot v2026.09.04-4849 version: sunnypilot v2026.003.000 (feature-branch) date: 2026-09-04T22:56:57 master commit: `b70920a8c0`	2026-09-04 22:56:57 +00:00
github-actions[bot]	833adb27e2	sunnypilot v2026.09.04-4849 release	2026-09-04 22:56:47 +00:00