mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-20 05:22:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot 2022-12-23T08:15:11 for EON/C2

Browse Source 
version: dragonpilot v0.9.1 beta for EON/C2
date: 2022-12-23T08:15:11
dp-dev(priv2) master commit: 3cc44ae87d697a5e07bfa58c4e799730bea0b63a
This commit is contained in:
Dragonpilot Team
2022-12-23 07:06:38 +00:00
committed by Comma Device
parent de34fa87ee
commit 398aeb927b
39 changed files with 2592 additions and 2535 deletions
Download Patch File Download Diff File Expand all files Collapse all files
cereal/dp.capnp
+1
View File
@@ -46,4 +46,5 @@ struct DragonConf {
dpE2EConditionalAtSpeedLead @38 :UInt8;
dpE2EConditionalAdaptFp @39 :Bool;
dpE2EConditionalAdaptAp @40 :Bool;
dpE2EConditionalVoacc @41 :Bool;
}
cereal/libcereal_shared.so
BIN
View File
Binary file not shown.
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/dp_conf.py
+1
View File
@@ -142,6 +142,7 @@ confs = [
{'name': 'dp_e2e_conditional_at_speed_lead', 'default': 40, 'type': 'UInt8', 'min': 0, 'max': 80, 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
{'name': 'dp_e2e_conditional_adapt_fp', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
{'name': 'dp_e2e_conditional_adapt_ap', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
{'name': 'dp_e2e_conditional_voacc', 'default': False, 'type': 'Bool', 'depends': [{'name': 'dp_e2e_conditional', 'vals': [True]}], 'conf_type': ['param', 'struct']},
# {'name': 'dp_toyota_no_min_acc_limit', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
# {'name': 'dp_toyota_ldw', 'default': True, 'type': 'Bool', 'conf_type': ['param', 'struct']},
{'name': 'dp_toyota_zss', 'default': False, 'type': 'Bool', 'conf_type': ['param']},
common/params_pyx.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2022.11.30"
#define COMMA_VERSION "2022.12.23"
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
panda/board/obj/bootstub.panda.bin
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.sspoof.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.testing.signed
BIN
View File
Binary file not shown.
panda/board/obj/panda.bin.testing.sspoof.signed
BIN
View File
Binary file not shown.
rednose/helpers/ekf_sym_pyx.so
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd
BIN
View File
Binary file not shown.
selfdrive/boardd/boardd_api_impl.so
BIN
View File
Binary file not shown.
selfdrive/controls/controlsd.py
+3 -1
View File
@@ -399,7 +399,7 @@ class Controls:
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if not REPLAY:
if self.sm['dragonConf'].dpAtl == 0 and not REPLAY:
# Check for mismatch between openpilot and car's PCM
cruise_mismatch = CS.cruiseState.enabled and (not self.enabled or not self.CP.pcmCruise)
self.cruise_mismatch_counter = self.cruise_mismatch_counter + 1 if cruise_mismatch else 0
@@ -560,6 +560,8 @@ class Controls:
# DISABLED
elif self.state == State.disabled:
if CS.cruiseState.available and not CS.cruiseActualEnabled and self.sm['dragonConf'].dpAtl > 0 and not self.events.any(ET.NO_ENTRY):
self.state = State.overriding
if self.events.any(ET.ENABLE):
if self.events.any(ET.NO_ENTRY):
self.current_alert_types.append(ET.NO_ENTRY)
selfdrive/controls/lib/longitudinal_planner.py
+106 -62
View File
@@ -51,12 +51,20 @@ _DP_CRUISE_MAX_V_ECO = [2.5, 1.3, 1.2, 0.7, 0.48, 0.35, 0.25, 0.15, 0.12, 0.06]
_DP_CRUISE_MAX_V_SPORT = [3.5, 3.5, 2.5, 1.5, 2.0, 2.0, 2.0, 1.5, 1.0, 0.5]
_DP_CRUISE_MAX_BP = [0., 3, 6., 8., 11., 15., 20., 25., 30., 55.]
# count n times before we decide a lead is there or not
_DP_E2E_LEAD_COUNT = 50
# lead distance
_DP_E2E_LEAD_DIST = 50
# d-e2e, from modeldata.h
TRAJECTORY_SIZE = 33
_DP_E2E_SNG_COUNT = 250
_DP_E2E_LEAD_COUNT = 5
_DP_E2E_STOP_BP = [0., 10., 20., 30., 40., 50., 55.]
_DP_E2E_STOP_DIST = [10, 30., 50., 70., 80., 90., 120.]
_DP_E2E_STOP_COUNT = 5
_DP_E2E_SNG_COUNT = 5
_DP_E2E_SNG_ACC_COUNT = 5
_DP_E2E_SWAP_COUNT = 10
_DP_E2E_TF_COUNT = 5
def dp_calc_cruise_accel_limits(v_ego, dp_profile):
if dp_profile == DP_ACCEL_ECO:
@@ -95,10 +103,14 @@ class LongitudinalPlanner:
self.dp_e2e_has_lead = False
self.dp_e2e_lead_last = False
self.dp_e2e_lead_count = 0
self.dp_e2e_mode_last = 'acc'
self.dp_e2e_sng = False
self.dp_e2e_sng_count = 0
self.dp_e2e_standstill_last = False
self.dp_e2e_swap_count = 0
self.dp_e2e_stop_count = 0
self.dp_e2e_tf = T_FOLLOW
self.dp_e2e_tf_count = 0
self.CP = CP
self.params = Params()
@@ -129,69 +141,103 @@ class LongitudinalPlanner:
self.events = Events()
self.turn_speed_controller = TurnSpeedController()
self.dp_e2e_adapt_ap = False
self.dp_e2e_adapt_fp = False
def read_param(self):
e2e = self.params.get_bool('ExperimentalMode') and self.CP.openpilotLongitudinalControl
self.mpc.mode = 'blended' if e2e else 'acc'
# dp - conditional e2e
def conditional_e2e(self, standstill, within_speed_condition, e2e_lead, within_speed_condition_lead):
def _set_dp_e2e_mode(self, mode, force=False):
reset_state = False
# lead counter
# to avoid lead comes and go too quickly causing mode switching too fast
# we count _DP_E2E_LEAD_COUNT before we update lead existence.
if e2e_lead != self.dp_e2e_lead_last:
self.dp_e2e_lead_count = 0
if force:
self.dp_e2e_swap_count = 0
if self.mpc.mode != mode:
reset_state = True
self.mpc.mode = mode
return reset_state
# prevent switching in a short period of time.
if self.mpc.mode == mode:
self.dp_e2e_swap_count = 0
else:
self.dp_e2e_lead_count += 1
self.dp_e2e_swap_count += 1
# when lead status count > _DP_E2E_LEAD_COUNT, we update actual lead status
if self.dp_e2e_lead_count >= _DP_E2E_LEAD_COUNT:
self.dp_e2e_has_lead = e2e_lead
if not standstill and self.dp_e2e_standstill_last:
self.dp_e2e_sng = True
if self.dp_e2e_sng:
self.dp_e2e_sng_count += 1
if self.dp_e2e_sng_count >= _DP_E2E_SNG_COUNT:
self.dp_e2e_sng = False
self.dp_e2e_sng = 0
dp_e2e_mode = 'acc'
# standstill uses e2e, to prevent lead suddenly move away.
if standstill:
self.dp_e2e_sng = 0
self.dp_e2e_sng = False
dp_e2e_mode = 'blended'
else:
# lead is driving below x km/h
if self.dp_e2e_has_lead:
if within_speed_condition_lead:
dp_e2e_mode = 'blended'
else:
# within speed condition and does not have a lead, use e2e
if within_speed_condition:
dp_e2e_mode = 'blended'
self.mpc.mode = dp_e2e_mode
if dp_e2e_mode != self.dp_e2e_mode_last:
if self.dp_e2e_swap_count >= _DP_E2E_SWAP_COUNT:
# if self.mpc.mode != mode:
# reset_state = True
self.mpc.mode = mode
reset_state = True
self.dp_e2e_lead_last = e2e_lead
self.dp_e2e_mode_last = dp_e2e_mode
self.dp_e2e_standstill_last = standstill
return reset_state
def conditional_e2e(self, sm):
v_ego_kph = sm['carState'].vEgo * 3.6
standstill = sm['carState'].standstill
# lead detection with buffer
lead = sm['radarState'].leadOne
lead_dist = lead.dRel
# make sure it see lead enough time
if lead.status != self.dp_e2e_lead_last:
self.dp_e2e_lead_count = 0
else:
self.dp_e2e_lead_count += 1
if self.dp_e2e_lead_count >= _DP_E2E_LEAD_COUNT:
self.dp_e2e_has_lead = lead.status
self.dp_e2e_lead_last = lead.status
# when standstill, always e2e
if standstill:
self.dp_e2e_sng_count = 0
self.dp_e2e_sng = False
return self._set_dp_e2e_mode('blended')
if self.dp_e2e_standstill_last and not standstill:
self.dp_e2e_sng = True
# when sng, we e2e for 0.5 secs
if self.dp_e2e_sng:
self.dp_e2e_sng_count += 1
if self.dp_e2e_sng_count > _DP_E2E_SNG_COUNT:
if self.dp_e2e_sng_count > _DP_E2E_SNG_ACC_COUNT:
self.dp_e2e_sng = False
return self._set_dp_e2e_mode('acc')
return self._set_dp_e2e_mode('blended')
# when we see a lead
if sm['dragonConf'].dpE2EConditionalVoacc and self.dp_e2e_has_lead:
# drive above conditional speed and lead is too close
if lead_dist <= v_ego_kph * self.dp_e2e_tf * interp(v_ego_kph, [50., 60., 80.], [1.30, 1.20, 1.10]) / 3.6:
self.dp_e2e_tf_count += 1
else:
self.dp_e2e_tf_count = 0
if self.dp_e2e_tf_count > _DP_E2E_TF_COUNT:
return self._set_dp_e2e_mode('blended')
# stop sign detection
md = sm['modelV2']
if abs(sm['carState'].steeringAngleDeg) <= 60 and len(md.orientation.x) == len(md.position.x) == TRAJECTORY_SIZE:
if md.position.x[TRAJECTORY_SIZE - 1] < interp(v_ego_kph, _DP_E2E_STOP_BP, _DP_E2E_STOP_DIST):
self.dp_e2e_stop_count += 1
else:
self.dp_e2e_stop_count = 0
else:
self.dp_e2e_stop_count = 0
if self.dp_e2e_stop_count >= _DP_E2E_STOP_COUNT:
return self._set_dp_e2e_mode('blended')
return self._set_dp_e2e_mode('acc')
@staticmethod
def parse_model(model_msg, model_error):
def parse_model(model_msg): #, model_error):
if (len(model_msg.position.x) == 33 and
len(model_msg.velocity.x) == 33 and
len(model_msg.acceleration.x) == 33):
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) - model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) - model_error
x = np.interp(T_IDXS_MPC, T_IDXS, model_msg.position.x) #- model_error * T_IDXS_MPC
v = np.interp(T_IDXS_MPC, T_IDXS, model_msg.velocity.x) #- model_error
a = np.interp(T_IDXS_MPC, T_IDXS, model_msg.acceleration.x)
j = np.zeros(len(T_IDXS_MPC))
else:
@@ -203,7 +249,7 @@ class LongitudinalPlanner:
def get_df(self, v_ego):
desired_tf = T_FOLLOW
if not self.dp_e2e_adapt_ap and self.mpc.mode == 'blended':
if not self.dp_e2e_adapt_fp and self.mpc.mode == 'blended':
return desired_tf
if self.dp_following_profile_ctrl:
if self.dp_following_profile == 0:
@@ -237,11 +283,8 @@ class LongitudinalPlanner:
if sm['dragonConf'].dpE2EConditional:
self.dp_e2e_adapt_ap = sm['dragonConf'].dpE2EConditionalAdaptAp
e2e_lead = sm['radarState'].leadOne.status and sm['radarState'].leadOne.dRel <= _DP_E2E_LEAD_DIST
within_speed_condition = sm['controlsState'].vCruise <= sm['dragonConf'].dpE2EConditionalAtSpeed
within_speed_condition_lead = (sm['radarState'].leadOne.vRel + sm['carState'].vEgo)*3.6 <= sm['dragonConf'].dpE2EConditionalAtSpeedLead
if self.conditional_e2e(sm['carState'].standstill, within_speed_condition, e2e_lead, within_speed_condition_lead):
dp_reset_state = True
self.dp_e2e_adapt_fp = sm['dragonConf'].dpE2EConditionalAdaptFp
dp_reset_state = self.conditional_e2e(sm)
else:
if self.param_read_counter % 50 == 0 and read:
self.read_param()
@@ -283,8 +326,8 @@ class LongitudinalPlanner:
# Prevent divergence, smooth in current v_ego
self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
# Compute model v_ego error
if len(sm['modelV2'].temporalPose.trans):
self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
# if len(sm['modelV2'].temporalPose.trans):
# self.v_model_error = sm['modelV2'].temporalPose.trans[0] - v_ego
# Get acceleration and active solutions for custom long mpc.
self.cruise_source, a_min_sol, v_cruise_sol = self.cruise_solutions(not reset_state, self.v_desired_filter.x,
@@ -302,8 +345,9 @@ class LongitudinalPlanner:
# self.mpc.set_weights(prev_accel_constraint)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
x, v, a, j = self.parse_model(sm['modelV2'], self.v_model_error)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint, self.get_df(v_ego))
x, v, a, j = self.parse_model(sm['modelV2']) #, self.v_model_error)
self.dp_e2e_tf = self.get_df(v_ego)
self.mpc.update(sm['carState'], sm['radarState'], v_cruise_sol, x, v, a, j, prev_accel_constraint, self.dp_e2e_tf)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
selfdrive/controls/lib/radar_helpers.py
+12 -3
View File
@@ -5,6 +5,9 @@ from common.kalman.simple_kalman import KF1D
# Default lead acceleration decay set to 50% at 1s
_LEAD_ACCEL_TAU = 1.5
# Hack to maintain vision lead state
_vision_lead_aTau = {0: _LEAD_ACCEL_TAU, 1: _LEAD_ACCEL_TAU}
# radar tracks
SPEED, ACCEL = 0, 1 # Kalman filter states enum
@@ -130,15 +133,21 @@ class Cluster():
"aLeadTau": float(self.aLeadTau)
}
def get_RadarState_from_vision(self, lead_msg, v_ego):
def get_RadarState_from_vision(self, lead_msg, lead_index, v_ego):
# Learn if constant acceleration
if abs(float(lead_msg.a[0])) < 0.5:
_vision_lead_aTau[lead_index] = _LEAD_ACCEL_TAU
else:
_vision_lead_aTau[lead_index] *= 0.9
return {
"dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA),
"yRel": float(-lead_msg.y[0]),
"vRel": float(lead_msg.v[0] - v_ego),
"vLead": float(lead_msg.v[0]),
"vLeadK": float(lead_msg.v[0]),
"aLeadK": float(0),
"aLeadTau": _LEAD_ACCEL_TAU,
"aLeadK": float(lead_msg.a[0]),
"aLeadTau": _vision_lead_aTau[lead_index],
"fcw": False,
"modelProb": float(lead_msg.prob),
"radar": False,
selfdrive/controls/radard.py
+4 -4
View File
@@ -65,7 +65,7 @@ def match_vision_to_cluster(v_ego, lead, clusters):
return None
def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
def get_lead(v_ego, ready, clusters, lead_msg, lead_index, low_speed_override=True):
# Determine leads, this is where the essential logic happens
if len(clusters) > 0 and ready and lead_msg.prob > .5:
cluster = match_vision_to_cluster(v_ego, lead_msg, clusters)
@@ -76,7 +76,7 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True):
if cluster is not None:
lead_dict = cluster.get_RadarState(lead_msg.prob)
elif (cluster is None) and ready and (lead_msg.prob > .5):
lead_dict = Cluster().get_RadarState_from_vision(lead_msg, v_ego)
lead_dict = Cluster().get_RadarState_from_vision(lead_msg, lead_index, v_ego)
if low_speed_override:
low_speed_clusters = [c for c in clusters if c.potential_low_speed_lead(v_ego)]
@@ -172,8 +172,8 @@ class RadarD():
leads_v3 = sm['modelV2'].leadsV3
if len(leads_v3) > 1:
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], low_speed_override=False)
radarState.leadOne = get_lead(self.v_ego, self.ready, clusters, leads_v3[0], 0, low_speed_override=True)
radarState.leadTwo = get_lead(self.v_ego, self.ready, clusters, leads_v3[1], 1, low_speed_override=False)
return dat
selfdrive/locationd/locationd
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/car.cpp
+342 -342
View File
@@ -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_7743502251243671447) {
out_7743502251243671447[0] = delta_x[0] + nom_x[0];
out_7743502251243671447[1] = delta_x[1] + nom_x[1];
out_7743502251243671447[2] = delta_x[2] + nom_x[2];
out_7743502251243671447[3] = delta_x[3] + nom_x[3];
out_7743502251243671447[4] = delta_x[4] + nom_x[4];
out_7743502251243671447[5] = delta_x[5] + nom_x[5];
out_7743502251243671447[6] = delta_x[6] + nom_x[6];
out_7743502251243671447[7] = delta_x[7] + nom_x[7];
out_7743502251243671447[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_8581923876106076715) {
out_8581923876106076715[0] = delta_x[0] + nom_x[0];
out_8581923876106076715[1] = delta_x[1] + nom_x[1];
out_8581923876106076715[2] = delta_x[2] + nom_x[2];
out_8581923876106076715[3] = delta_x[3] + nom_x[3];
out_8581923876106076715[4] = delta_x[4] + nom_x[4];
out_8581923876106076715[5] = delta_x[5] + nom_x[5];
out_8581923876106076715[6] = delta_x[6] + nom_x[6];
out_8581923876106076715[7] = delta_x[7] + nom_x[7];
out_8581923876106076715[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
out_8632432640030731669[0] = -nom_x[0] + true_x[0];
out_8632432640030731669[1] = -nom_x[1] + true_x[1];
out_8632432640030731669[2] = -nom_x[2] + true_x[2];
out_8632432640030731669[3] = -nom_x[3] + true_x[3];
out_8632432640030731669[4] = -nom_x[4] + true_x[4];
out_8632432640030731669[5] = -nom_x[5] + true_x[5];
out_8632432640030731669[6] = -nom_x[6] + true_x[6];
out_8632432640030731669[7] = -nom_x[7] + true_x[7];
out_8632432640030731669[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961) {
out_4891521151268278961[0] = -nom_x[0] + true_x[0];
out_4891521151268278961[1] = -nom_x[1] + true_x[1];
out_4891521151268278961[2] = -nom_x[2] + true_x[2];
out_4891521151268278961[3] = -nom_x[3] + true_x[3];
out_4891521151268278961[4] = -nom_x[4] + true_x[4];
out_4891521151268278961[5] = -nom_x[5] + true_x[5];
out_4891521151268278961[6] = -nom_x[6] + true_x[6];
out_4891521151268278961[7] = -nom_x[7] + true_x[7];
out_4891521151268278961[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_6955983744751080330) {
out_6955983744751080330[0] = 1.0;
out_6955983744751080330[1] = 0;
out_6955983744751080330[2] = 0;
out_6955983744751080330[3] = 0;
out_6955983744751080330[4] = 0;
out_6955983744751080330[5] = 0;
out_6955983744751080330[6] = 0;
out_6955983744751080330[7] = 0;
out_6955983744751080330[8] = 0;
out_6955983744751080330[9] = 0;
out_6955983744751080330[10] = 1.0;
out_6955983744751080330[11] = 0;
out_6955983744751080330[12] = 0;
out_6955983744751080330[13] = 0;
out_6955983744751080330[14] = 0;
out_6955983744751080330[15] = 0;
out_6955983744751080330[16] = 0;
out_6955983744751080330[17] = 0;
out_6955983744751080330[18] = 0;
out_6955983744751080330[19] = 0;
out_6955983744751080330[20] = 1.0;
out_6955983744751080330[21] = 0;
out_6955983744751080330[22] = 0;
out_6955983744751080330[23] = 0;
out_6955983744751080330[24] = 0;
out_6955983744751080330[25] = 0;
out_6955983744751080330[26] = 0;
out_6955983744751080330[27] = 0;
out_6955983744751080330[28] = 0;
out_6955983744751080330[29] = 0;
out_6955983744751080330[30] = 1.0;
out_6955983744751080330[31] = 0;
out_6955983744751080330[32] = 0;
out_6955983744751080330[33] = 0;
out_6955983744751080330[34] = 0;
out_6955983744751080330[35] = 0;
out_6955983744751080330[36] = 0;
out_6955983744751080330[37] = 0;
out_6955983744751080330[38] = 0;
out_6955983744751080330[39] = 0;
out_6955983744751080330[40] = 1.0;
out_6955983744751080330[41] = 0;
out_6955983744751080330[42] = 0;
out_6955983744751080330[43] = 0;
out_6955983744751080330[44] = 0;
out_6955983744751080330[45] = 0;
out_6955983744751080330[46] = 0;
out_6955983744751080330[47] = 0;
out_6955983744751080330[48] = 0;
out_6955983744751080330[49] = 0;
out_6955983744751080330[50] = 1.0;
out_6955983744751080330[51] = 0;
out_6955983744751080330[52] = 0;
out_6955983744751080330[53] = 0;
out_6955983744751080330[54] = 0;
out_6955983744751080330[55] = 0;
out_6955983744751080330[56] = 0;
out_6955983744751080330[57] = 0;
out_6955983744751080330[58] = 0;
out_6955983744751080330[59] = 0;
out_6955983744751080330[60] = 1.0;
out_6955983744751080330[61] = 0;
out_6955983744751080330[62] = 0;
out_6955983744751080330[63] = 0;
out_6955983744751080330[64] = 0;
out_6955983744751080330[65] = 0;
out_6955983744751080330[66] = 0;
out_6955983744751080330[67] = 0;
out_6955983744751080330[68] = 0;
out_6955983744751080330[69] = 0;
out_6955983744751080330[70] = 1.0;
out_6955983744751080330[71] = 0;
out_6955983744751080330[72] = 0;
out_6955983744751080330[73] = 0;
out_6955983744751080330[74] = 0;
out_6955983744751080330[75] = 0;
out_6955983744751080330[76] = 0;
out_6955983744751080330[77] = 0;
out_6955983744751080330[78] = 0;
out_6955983744751080330[79] = 0;
out_6955983744751080330[80] = 1.0;
void H_mod_fun(double *state, double *out_6523549794488946483) {
out_6523549794488946483[0] = 1.0;
out_6523549794488946483[1] = 0;
out_6523549794488946483[2] = 0;
out_6523549794488946483[3] = 0;
out_6523549794488946483[4] = 0;
out_6523549794488946483[5] = 0;
out_6523549794488946483[6] = 0;
out_6523549794488946483[7] = 0;
out_6523549794488946483[8] = 0;
out_6523549794488946483[9] = 0;
out_6523549794488946483[10] = 1.0;
out_6523549794488946483[11] = 0;
out_6523549794488946483[12] = 0;
out_6523549794488946483[13] = 0;
out_6523549794488946483[14] = 0;
out_6523549794488946483[15] = 0;
out_6523549794488946483[16] = 0;
out_6523549794488946483[17] = 0;
out_6523549794488946483[18] = 0;
out_6523549794488946483[19] = 0;
out_6523549794488946483[20] = 1.0;
out_6523549794488946483[21] = 0;
out_6523549794488946483[22] = 0;
out_6523549794488946483[23] = 0;
out_6523549794488946483[24] = 0;
out_6523549794488946483[25] = 0;
out_6523549794488946483[26] = 0;
out_6523549794488946483[27] = 0;
out_6523549794488946483[28] = 0;
out_6523549794488946483[29] = 0;
out_6523549794488946483[30] = 1.0;
out_6523549794488946483[31] = 0;
out_6523549794488946483[32] = 0;
out_6523549794488946483[33] = 0;
out_6523549794488946483[34] = 0;
out_6523549794488946483[35] = 0;
out_6523549794488946483[36] = 0;
out_6523549794488946483[37] = 0;
out_6523549794488946483[38] = 0;
out_6523549794488946483[39] = 0;
out_6523549794488946483[40] = 1.0;
out_6523549794488946483[41] = 0;
out_6523549794488946483[42] = 0;
out_6523549794488946483[43] = 0;
out_6523549794488946483[44] = 0;
out_6523549794488946483[45] = 0;
out_6523549794488946483[46] = 0;
out_6523549794488946483[47] = 0;
out_6523549794488946483[48] = 0;
out_6523549794488946483[49] = 0;
out_6523549794488946483[50] = 1.0;
out_6523549794488946483[51] = 0;
out_6523549794488946483[52] = 0;
out_6523549794488946483[53] = 0;
out_6523549794488946483[54] = 0;
out_6523549794488946483[55] = 0;
out_6523549794488946483[56] = 0;
out_6523549794488946483[57] = 0;
out_6523549794488946483[58] = 0;
out_6523549794488946483[59] = 0;
out_6523549794488946483[60] = 1.0;
out_6523549794488946483[61] = 0;
out_6523549794488946483[62] = 0;
out_6523549794488946483[63] = 0;
out_6523549794488946483[64] = 0;
out_6523549794488946483[65] = 0;
out_6523549794488946483[66] = 0;
out_6523549794488946483[67] = 0;
out_6523549794488946483[68] = 0;
out_6523549794488946483[69] = 0;
out_6523549794488946483[70] = 1.0;
out_6523549794488946483[71] = 0;
out_6523549794488946483[72] = 0;
out_6523549794488946483[73] = 0;
out_6523549794488946483[74] = 0;
out_6523549794488946483[75] = 0;
out_6523549794488946483[76] = 0;
out_6523549794488946483[77] = 0;
out_6523549794488946483[78] = 0;
out_6523549794488946483[79] = 0;
out_6523549794488946483[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_1057256165937700131) {
out_1057256165937700131[0] = state[0];
out_1057256165937700131[1] = state[1];
out_1057256165937700131[2] = state[2];
out_1057256165937700131[3] = state[3];
out_1057256165937700131[4] = state[4];
out_1057256165937700131[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_1057256165937700131[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_1057256165937700131[7] = state[7];
out_1057256165937700131[8] = state[8];
void f_fun(double *state, double dt, double *out_3111820312432597939) {
out_3111820312432597939[0] = state[0];
out_3111820312432597939[1] = state[1];
out_3111820312432597939[2] = state[2];
out_3111820312432597939[3] = state[3];
out_3111820312432597939[4] = state[4];
out_3111820312432597939[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*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_3111820312432597939[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_3111820312432597939[7] = state[7];
out_3111820312432597939[8] = state[8];
}
void F_fun(double *state, double dt, double *out_2042254547319901591) {
out_2042254547319901591[0] = 1;
out_2042254547319901591[1] = 0;
out_2042254547319901591[2] = 0;
out_2042254547319901591[3] = 0;
out_2042254547319901591[4] = 0;
out_2042254547319901591[5] = 0;
out_2042254547319901591[6] = 0;
out_2042254547319901591[7] = 0;
out_2042254547319901591[8] = 0;
out_2042254547319901591[9] = 0;
out_2042254547319901591[10] = 1;
out_2042254547319901591[11] = 0;
out_2042254547319901591[12] = 0;
out_2042254547319901591[13] = 0;
out_2042254547319901591[14] = 0;
out_2042254547319901591[15] = 0;
out_2042254547319901591[16] = 0;
out_2042254547319901591[17] = 0;
out_2042254547319901591[18] = 0;
out_2042254547319901591[19] = 0;
out_2042254547319901591[20] = 1;
out_2042254547319901591[21] = 0;
out_2042254547319901591[22] = 0;
out_2042254547319901591[23] = 0;
out_2042254547319901591[24] = 0;
out_2042254547319901591[25] = 0;
out_2042254547319901591[26] = 0;
out_2042254547319901591[27] = 0;
out_2042254547319901591[28] = 0;
out_2042254547319901591[29] = 0;
out_2042254547319901591[30] = 1;
out_2042254547319901591[31] = 0;
out_2042254547319901591[32] = 0;
out_2042254547319901591[33] = 0;
out_2042254547319901591[34] = 0;
out_2042254547319901591[35] = 0;
out_2042254547319901591[36] = 0;
out_2042254547319901591[37] = 0;
out_2042254547319901591[38] = 0;
out_2042254547319901591[39] = 0;
out_2042254547319901591[40] = 1;
out_2042254547319901591[41] = 0;
out_2042254547319901591[42] = 0;
out_2042254547319901591[43] = 0;
out_2042254547319901591[44] = 0;
out_2042254547319901591[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_2042254547319901591[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_2042254547319901591[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_2042254547319901591[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_2042254547319901591[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_2042254547319901591[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_2042254547319901591[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_2042254547319901591[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_2042254547319901591[53] = -9.8000000000000007*dt;
out_2042254547319901591[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_2042254547319901591[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_2042254547319901591[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2042254547319901591[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2042254547319901591[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_2042254547319901591[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_2042254547319901591[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_2042254547319901591[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_2042254547319901591[62] = 0;
out_2042254547319901591[63] = 0;
out_2042254547319901591[64] = 0;
out_2042254547319901591[65] = 0;
out_2042254547319901591[66] = 0;
out_2042254547319901591[67] = 0;
out_2042254547319901591[68] = 0;
out_2042254547319901591[69] = 0;
out_2042254547319901591[70] = 1;
out_2042254547319901591[71] = 0;
out_2042254547319901591[72] = 0;
out_2042254547319901591[73] = 0;
out_2042254547319901591[74] = 0;
out_2042254547319901591[75] = 0;
out_2042254547319901591[76] = 0;
out_2042254547319901591[77] = 0;
out_2042254547319901591[78] = 0;
out_2042254547319901591[79] = 0;
out_2042254547319901591[80] = 1;
void F_fun(double *state, double dt, double *out_5596968740727191559) {
out_5596968740727191559[0] = 1;
out_5596968740727191559[1] = 0;
out_5596968740727191559[2] = 0;
out_5596968740727191559[3] = 0;
out_5596968740727191559[4] = 0;
out_5596968740727191559[5] = 0;
out_5596968740727191559[6] = 0;
out_5596968740727191559[7] = 0;
out_5596968740727191559[8] = 0;
out_5596968740727191559[9] = 0;
out_5596968740727191559[10] = 1;
out_5596968740727191559[11] = 0;
out_5596968740727191559[12] = 0;
out_5596968740727191559[13] = 0;
out_5596968740727191559[14] = 0;
out_5596968740727191559[15] = 0;
out_5596968740727191559[16] = 0;
out_5596968740727191559[17] = 0;
out_5596968740727191559[18] = 0;
out_5596968740727191559[19] = 0;
out_5596968740727191559[20] = 1;
out_5596968740727191559[21] = 0;
out_5596968740727191559[22] = 0;
out_5596968740727191559[23] = 0;
out_5596968740727191559[24] = 0;
out_5596968740727191559[25] = 0;
out_5596968740727191559[26] = 0;
out_5596968740727191559[27] = 0;
out_5596968740727191559[28] = 0;
out_5596968740727191559[29] = 0;
out_5596968740727191559[30] = 1;
out_5596968740727191559[31] = 0;
out_5596968740727191559[32] = 0;
out_5596968740727191559[33] = 0;
out_5596968740727191559[34] = 0;
out_5596968740727191559[35] = 0;
out_5596968740727191559[36] = 0;
out_5596968740727191559[37] = 0;
out_5596968740727191559[38] = 0;
out_5596968740727191559[39] = 0;
out_5596968740727191559[40] = 1;
out_5596968740727191559[41] = 0;
out_5596968740727191559[42] = 0;
out_5596968740727191559[43] = 0;
out_5596968740727191559[44] = 0;
out_5596968740727191559[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_5596968740727191559[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_5596968740727191559[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_5596968740727191559[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_5596968740727191559[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_5596968740727191559[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_5596968740727191559[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_5596968740727191559[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_5596968740727191559[53] = -9.8000000000000007*dt;
out_5596968740727191559[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_5596968740727191559[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_5596968740727191559[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5596968740727191559[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5596968740727191559[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_5596968740727191559[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_5596968740727191559[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_5596968740727191559[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_5596968740727191559[62] = 0;
out_5596968740727191559[63] = 0;
out_5596968740727191559[64] = 0;
out_5596968740727191559[65] = 0;
out_5596968740727191559[66] = 0;
out_5596968740727191559[67] = 0;
out_5596968740727191559[68] = 0;
out_5596968740727191559[69] = 0;
out_5596968740727191559[70] = 1;
out_5596968740727191559[71] = 0;
out_5596968740727191559[72] = 0;
out_5596968740727191559[73] = 0;
out_5596968740727191559[74] = 0;
out_5596968740727191559[75] = 0;
out_5596968740727191559[76] = 0;
out_5596968740727191559[77] = 0;
out_5596968740727191559[78] = 0;
out_5596968740727191559[79] = 0;
out_5596968740727191559[80] = 1;
}
void h_25(double *state, double *unused, double *out_9038137973563738704) {
out_9038137973563738704[0] = state[6];
void h_25(double *state, double *unused, double *out_8255502319032992897) {
out_8255502319032992897[0] = state[6];
}
void H_25(double *state, double *unused, double *out_6219790086191979796) {
out_6219790086191979796[0] = 0;
out_6219790086191979796[1] = 0;
out_6219790086191979796[2] = 0;
out_6219790086191979796[3] = 0;
out_6219790086191979796[4] = 0;
out_6219790086191979796[5] = 0;
out_6219790086191979796[6] = 1;
out_6219790086191979796[7] = 0;
out_6219790086191979796[8] = 0;
void H_25(double *state, double *unused, double *out_8118118653173567889) {
out_8118118653173567889[0] = 0;
out_8118118653173567889[1] = 0;
out_8118118653173567889[2] = 0;
out_8118118653173567889[3] = 0;
out_8118118653173567889[4] = 0;
out_8118118653173567889[5] = 0;
out_8118118653173567889[6] = 1;
out_8118118653173567889[7] = 0;
out_8118118653173567889[8] = 0;
}
void h_24(double *state, double *unused, double *out_8933773235415263694) {
out_8933773235415263694[0] = state[4];
out_8933773235415263694[1] = state[5];
void h_24(double *state, double *unused, double *out_5005612245441996689) {
out_5005612245441996689[0] = state[4];
out_5005612245441996689[1] = state[5];
}
void H_24(double *state, double *unused, double *out_2661864487389656660) {
out_2661864487389656660[0] = 0;
out_2661864487389656660[1] = 0;
out_2661864487389656660[2] = 0;
out_2661864487389656660[3] = 0;
out_2661864487389656660[4] = 1;
out_2661864487389656660[5] = 0;
out_2661864487389656660[6] = 0;
out_2661864487389656660[7] = 0;
out_2661864487389656660[8] = 0;
out_2661864487389656660[9] = 0;
out_2661864487389656660[10] = 0;
out_2661864487389656660[11] = 0;
out_2661864487389656660[12] = 0;
out_2661864487389656660[13] = 0;
out_2661864487389656660[14] = 1;
out_2661864487389656660[15] = 0;
out_2661864487389656660[16] = 0;
out_2661864487389656660[17] = 0;
void H_24(double *state, double *unused, double *out_5892410869194699327) {
out_5892410869194699327[0] = 0;
out_5892410869194699327[1] = 0;
out_5892410869194699327[2] = 0;
out_5892410869194699327[3] = 0;
out_5892410869194699327[4] = 1;
out_5892410869194699327[5] = 0;
out_5892410869194699327[6] = 0;
out_5892410869194699327[7] = 0;
out_5892410869194699327[8] = 0;
out_5892410869194699327[9] = 0;
out_5892410869194699327[10] = 0;
out_5892410869194699327[11] = 0;
out_5892410869194699327[12] = 0;
out_5892410869194699327[13] = 0;
out_5892410869194699327[14] = 1;
out_5892410869194699327[15] = 0;
out_5892410869194699327[16] = 0;
out_5892410869194699327[17] = 0;
}
void h_30(double *state, double *unused, double *out_1353345261140371077) {
out_1353345261140371077[0] = state[4];
void h_30(double *state, double *unused, double *out_3420406528326207783) {
out_3420406528326207783[0] = state[4];
}
void H_30(double *state, double *unused, double *out_3701457127684731169) {
out_3701457127684731169[0] = 0;
out_3701457127684731169[1] = 0;
out_3701457127684731169[2] = 0;
out_3701457127684731169[3] = 0;
out_3701457127684731169[4] = 1;
out_3701457127684731169[5] = 0;
out_3701457127684731169[6] = 0;
out_3701457127684731169[7] = 0;
out_3701457127684731169[8] = 0;
void H_30(double *state, double *unused, double *out_1201428311681951134) {
out_1201428311681951134[0] = 0;
out_1201428311681951134[1] = 0;
out_1201428311681951134[2] = 0;
out_1201428311681951134[3] = 0;
out_1201428311681951134[4] = 1;
out_1201428311681951134[5] = 0;
out_1201428311681951134[6] = 0;
out_1201428311681951134[7] = 0;
out_1201428311681951134[8] = 0;
}
void h_26(double *state, double *unused, double *out_3791544801975276460) {
out_3791544801975276460[0] = state[7];
void h_26(double *state, double *unused, double *out_1805500687509117164) {
out_1805500687509117164[0] = state[7];
}
void H_26(double *state, double *unused, double *out_8485450668643515596) {
out_8485450668643515596[0] = 0;
out_8485450668643515596[1] = 0;
out_8485450668643515596[2] = 0;
out_8485450668643515596[3] = 0;
out_8485450668643515596[4] = 0;
out_8485450668643515596[5] = 0;
out_8485450668643515596[6] = 0;
out_8485450668643515596[7] = 1;
out_8485450668643515596[8] = 0;
void H_26(double *state, double *unused, double *out_6587122101661927503) {
out_6587122101661927503[0] = 0;
out_6587122101661927503[1] = 0;
out_6587122101661927503[2] = 0;
out_6587122101661927503[3] = 0;
out_6587122101661927503[4] = 0;
out_6587122101661927503[5] = 0;
out_6587122101661927503[6] = 0;
out_6587122101661927503[7] = 1;
out_6587122101661927503[8] = 0;
}
void h_27(double *state, double *unused, double *out_4406932832484967267) {
out_4406932832484967267[0] = state[3];
void h_27(double *state, double *unused, double *out_9023497953600416982) {
out_9023497953600416982[0] = state[3];
}
void H_27(double *state, double *unused, double *out_5876220439485156080) {
out_5876220439485156080[0] = 0;
out_5876220439485156080[1] = 0;
out_5876220439485156080[2] = 0;
out_5876220439485156080[3] = 1;
out_5876220439485156080[4] = 0;
out_5876220439485156080[5] = 0;
out_5876220439485156080[6] = 0;
out_5876220439485156080[7] = 0;
out_5876220439485156080[8] = 0;
void H_27(double *state, double *unused, double *out_3376191623482376045) {
out_3376191623482376045[0] = 0;
out_3376191623482376045[1] = 0;
out_3376191623482376045[2] = 0;
out_3376191623482376045[3] = 1;
out_3376191623482376045[4] = 0;
out_3376191623482376045[5] = 0;
out_3376191623482376045[6] = 0;
out_3376191623482376045[7] = 0;
out_3376191623482376045[8] = 0;
}
void h_29(double *state, double *unused, double *out_6276545984403911908) {
out_6276545984403911908[0] = state[1];
void h_29(double *state, double *unused, double *out_9148052057824628745) {
out_9148052057824628745[0] = state[1];
}
void H_29(double *state, double *unused, double *out_3191225783370338985) {
out_3191225783370338985[0] = 0;
out_3191225783370338985[1] = 1;
out_3191225783370338985[2] = 0;
out_3191225783370338985[3] = 0;
out_3191225783370338985[4] = 0;
out_3191225783370338985[5] = 0;
out_3191225783370338985[6] = 0;
out_3191225783370338985[7] = 0;
out_3191225783370338985[8] = 0;
void H_29(double *state, double *unused, double *out_691196967367558950) {
out_691196967367558950[0] = 0;
out_691196967367558950[1] = 1;
out_691196967367558950[2] = 0;
out_691196967367558950[3] = 0;
out_691196967367558950[4] = 0;
out_691196967367558950[5] = 0;
out_691196967367558950[6] = 0;
out_691196967367558950[7] = 0;
out_691196967367558950[8] = 0;
}
void h_28(double *state, double *unused, double *out_7011338153293664918) {
out_7011338153293664918[0] = state[0];
void h_28(double *state, double *unused, double *out_402702707668658192) {
out_402702707668658192[0] = state[0];
}
void H_28(double *state, double *unused, double *out_8273624800439869559) {
out_8273624800439869559[0] = 1;
out_8273624800439869559[1] = 0;
out_8273624800439869559[2] = 0;
out_8273624800439869559[3] = 0;
out_8273624800439869559[4] = 0;
out_8273624800439869559[5] = 0;
out_8273624800439869559[6] = 0;
out_8273624800439869559[7] = 0;
out_8273624800439869559[8] = 0;
void H_28(double *state, double *unused, double *out_5773595984437089524) {
out_5773595984437089524[0] = 1;
out_5773595984437089524[1] = 0;
out_5773595984437089524[2] = 0;
out_5773595984437089524[3] = 0;
out_5773595984437089524[4] = 0;
out_5773595984437089524[5] = 0;
out_5773595984437089524[6] = 0;
out_5773595984437089524[7] = 0;
out_5773595984437089524[8] = 0;
}
void h_31(double *state, double *unused, double *out_8647698245415467134) {
out_8647698245415467134[0] = state[8];
void h_31(double *state, double *unused, double *out_6661654130949307838) {
out_6661654130949307838[0] = state[8];
}
void H_31(double *state, double *unused, double *out_7859242566410164120) {
out_7859242566410164120[0] = 0;
out_7859242566410164120[1] = 0;
out_7859242566410164120[2] = 0;
out_7859242566410164120[3] = 0;
out_7859242566410164120[4] = 0;
out_7859242566410164120[5] = 0;
out_7859242566410164120[6] = 0;
out_7859242566410164120[7] = 0;
out_7859242566410164120[8] = 1;
void H_31(double *state, double *unused, double *out_8087472691296607461) {
out_8087472691296607461[0] = 0;
out_8087472691296607461[1] = 0;
out_8087472691296607461[2] = 0;
out_8087472691296607461[3] = 0;
out_8087472691296607461[4] = 0;
out_8087472691296607461[5] = 0;
out_8087472691296607461[6] = 0;
out_8087472691296607461[7] = 0;
out_8087472691296607461[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_7743502251243671447) {
err_fun(nom_x, delta_x, out_7743502251243671447);
void car_err_fun(double *nom_x, double *delta_x, double *out_8581923876106076715) {
err_fun(nom_x, delta_x, out_8581923876106076715);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
inv_err_fun(nom_x, true_x, out_8632432640030731669);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961) {
inv_err_fun(nom_x, true_x, out_4891521151268278961);
}
void car_H_mod_fun(double *state, double *out_6955983744751080330) {
H_mod_fun(state, out_6955983744751080330);
void car_H_mod_fun(double *state, double *out_6523549794488946483) {
H_mod_fun(state, out_6523549794488946483);
}
void car_f_fun(double *state, double dt, double *out_1057256165937700131) {
f_fun(state, dt, out_1057256165937700131);
void car_f_fun(double *state, double dt, double *out_3111820312432597939) {
f_fun(state, dt, out_3111820312432597939);
}
void car_F_fun(double *state, double dt, double *out_2042254547319901591) {
F_fun(state, dt, out_2042254547319901591);
void car_F_fun(double *state, double dt, double *out_5596968740727191559) {
F_fun(state, dt, out_5596968740727191559);
}
void car_h_25(double *state, double *unused, double *out_9038137973563738704) {
h_25(state, unused, out_9038137973563738704);
void car_h_25(double *state, double *unused, double *out_8255502319032992897) {
h_25(state, unused, out_8255502319032992897);
}
void car_H_25(double *state, double *unused, double *out_6219790086191979796) {
H_25(state, unused, out_6219790086191979796);
void car_H_25(double *state, double *unused, double *out_8118118653173567889) {
H_25(state, unused, out_8118118653173567889);
}
void car_h_24(double *state, double *unused, double *out_8933773235415263694) {
h_24(state, unused, out_8933773235415263694);
void car_h_24(double *state, double *unused, double *out_5005612245441996689) {
h_24(state, unused, out_5005612245441996689);
}
void car_H_24(double *state, double *unused, double *out_2661864487389656660) {
H_24(state, unused, out_2661864487389656660);
void car_H_24(double *state, double *unused, double *out_5892410869194699327) {
H_24(state, unused, out_5892410869194699327);
}
void car_h_30(double *state, double *unused, double *out_1353345261140371077) {
h_30(state, unused, out_1353345261140371077);
void car_h_30(double *state, double *unused, double *out_3420406528326207783) {
h_30(state, unused, out_3420406528326207783);
}
void car_H_30(double *state, double *unused, double *out_3701457127684731169) {
H_30(state, unused, out_3701457127684731169);
void car_H_30(double *state, double *unused, double *out_1201428311681951134) {
H_30(state, unused, out_1201428311681951134);
}
void car_h_26(double *state, double *unused, double *out_3791544801975276460) {
h_26(state, unused, out_3791544801975276460);
void car_h_26(double *state, double *unused, double *out_1805500687509117164) {
h_26(state, unused, out_1805500687509117164);
}
void car_H_26(double *state, double *unused, double *out_8485450668643515596) {
H_26(state, unused, out_8485450668643515596);
void car_H_26(double *state, double *unused, double *out_6587122101661927503) {
H_26(state, unused, out_6587122101661927503);
}
void car_h_27(double *state, double *unused, double *out_4406932832484967267) {
h_27(state, unused, out_4406932832484967267);
void car_h_27(double *state, double *unused, double *out_9023497953600416982) {
h_27(state, unused, out_9023497953600416982);
}
void car_H_27(double *state, double *unused, double *out_5876220439485156080) {
H_27(state, unused, out_5876220439485156080);
void car_H_27(double *state, double *unused, double *out_3376191623482376045) {
H_27(state, unused, out_3376191623482376045);
}
void car_h_29(double *state, double *unused, double *out_6276545984403911908) {
h_29(state, unused, out_6276545984403911908);
void car_h_29(double *state, double *unused, double *out_9148052057824628745) {
h_29(state, unused, out_9148052057824628745);
}
void car_H_29(double *state, double *unused, double *out_3191225783370338985) {
H_29(state, unused, out_3191225783370338985);
void car_H_29(double *state, double *unused, double *out_691196967367558950) {
H_29(state, unused, out_691196967367558950);
}
void car_h_28(double *state, double *unused, double *out_7011338153293664918) {
h_28(state, unused, out_7011338153293664918);
void car_h_28(double *state, double *unused, double *out_402702707668658192) {
h_28(state, unused, out_402702707668658192);
}
void car_H_28(double *state, double *unused, double *out_8273624800439869559) {
H_28(state, unused, out_8273624800439869559);
void car_H_28(double *state, double *unused, double *out_5773595984437089524) {
H_28(state, unused, out_5773595984437089524);
}
void car_h_31(double *state, double *unused, double *out_8647698245415467134) {
h_31(state, unused, out_8647698245415467134);
void car_h_31(double *state, double *unused, double *out_6661654130949307838) {
h_31(state, unused, out_6661654130949307838);
}
void car_H_31(double *state, double *unused, double *out_7859242566410164120) {
H_31(state, unused, out_7859242566410164120);
void car_H_31(double *state, double *unused, double *out_8087472691296607461) {
H_31(state, unused, out_8087472691296607461);
}
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/car.h
+21 -21
View File
@@ -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_7743502251243671447);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669);
void car_H_mod_fun(double *state, double *out_6955983744751080330);
void car_f_fun(double *state, double dt, double *out_1057256165937700131);
void car_F_fun(double *state, double dt, double *out_2042254547319901591);
void car_h_25(double *state, double *unused, double *out_9038137973563738704);
void car_H_25(double *state, double *unused, double *out_6219790086191979796);
void car_h_24(double *state, double *unused, double *out_8933773235415263694);
void car_H_24(double *state, double *unused, double *out_2661864487389656660);
void car_h_30(double *state, double *unused, double *out_1353345261140371077);
void car_H_30(double *state, double *unused, double *out_3701457127684731169);
void car_h_26(double *state, double *unused, double *out_3791544801975276460);
void car_H_26(double *state, double *unused, double *out_8485450668643515596);
void car_h_27(double *state, double *unused, double *out_4406932832484967267);
void car_H_27(double *state, double *unused, double *out_5876220439485156080);
void car_h_29(double *state, double *unused, double *out_6276545984403911908);
void car_H_29(double *state, double *unused, double *out_3191225783370338985);
void car_h_28(double *state, double *unused, double *out_7011338153293664918);
void car_H_28(double *state, double *unused, double *out_8273624800439869559);
void car_h_31(double *state, double *unused, double *out_8647698245415467134);
void car_H_31(double *state, double *unused, double *out_7859242566410164120);
void car_err_fun(double *nom_x, double *delta_x, double *out_8581923876106076715);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4891521151268278961);
void car_H_mod_fun(double *state, double *out_6523549794488946483);
void car_f_fun(double *state, double dt, double *out_3111820312432597939);
void car_F_fun(double *state, double dt, double *out_5596968740727191559);
void car_h_25(double *state, double *unused, double *out_8255502319032992897);
void car_H_25(double *state, double *unused, double *out_8118118653173567889);
void car_h_24(double *state, double *unused, double *out_5005612245441996689);
void car_H_24(double *state, double *unused, double *out_5892410869194699327);
void car_h_30(double *state, double *unused, double *out_3420406528326207783);
void car_H_30(double *state, double *unused, double *out_1201428311681951134);
void car_h_26(double *state, double *unused, double *out_1805500687509117164);
void car_H_26(double *state, double *unused, double *out_6587122101661927503);
void car_h_27(double *state, double *unused, double *out_9023497953600416982);
void car_H_27(double *state, double *unused, double *out_3376191623482376045);
void car_h_29(double *state, double *unused, double *out_9148052057824628745);
void car_H_29(double *state, double *unused, double *out_691196967367558950);
void car_h_28(double *state, double *unused, double *out_402702707668658192);
void car_H_28(double *state, double *unused, double *out_5773595984437089524);
void car_h_31(double *state, double *unused, double *out_6661654130949307838);
void car_H_31(double *state, double *unused, double *out_8087472691296607461);
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);
selfdrive/locationd/models/generated/gnss.cpp
+362 -362
View File
@@ -17,354 +17,354 @@ const static double MAHA_THRESH_21 = 3.8414588206941227;
* *
* This file is part of 'ekf' *
******************************************************************************/
void err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
out_1734633203049768488[0] = delta_x[0] + nom_x[0];
out_1734633203049768488[1] = delta_x[1] + nom_x[1];
out_1734633203049768488[2] = delta_x[2] + nom_x[2];
out_1734633203049768488[3] = delta_x[3] + nom_x[3];
out_1734633203049768488[4] = delta_x[4] + nom_x[4];
out_1734633203049768488[5] = delta_x[5] + nom_x[5];
out_1734633203049768488[6] = delta_x[6] + nom_x[6];
out_1734633203049768488[7] = delta_x[7] + nom_x[7];
out_1734633203049768488[8] = delta_x[8] + nom_x[8];
out_1734633203049768488[9] = delta_x[9] + nom_x[9];
out_1734633203049768488[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_6740791193231733360) {
out_6740791193231733360[0] = delta_x[0] + nom_x[0];
out_6740791193231733360[1] = delta_x[1] + nom_x[1];
out_6740791193231733360[2] = delta_x[2] + nom_x[2];
out_6740791193231733360[3] = delta_x[3] + nom_x[3];
out_6740791193231733360[4] = delta_x[4] + nom_x[4];
out_6740791193231733360[5] = delta_x[5] + nom_x[5];
out_6740791193231733360[6] = delta_x[6] + nom_x[6];
out_6740791193231733360[7] = delta_x[7] + nom_x[7];
out_6740791193231733360[8] = delta_x[8] + nom_x[8];
out_6740791193231733360[9] = delta_x[9] + nom_x[9];
out_6740791193231733360[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
out_7226800202829737504[0] = -nom_x[0] + true_x[0];
out_7226800202829737504[1] = -nom_x[1] + true_x[1];
out_7226800202829737504[2] = -nom_x[2] + true_x[2];
out_7226800202829737504[3] = -nom_x[3] + true_x[3];
out_7226800202829737504[4] = -nom_x[4] + true_x[4];
out_7226800202829737504[5] = -nom_x[5] + true_x[5];
out_7226800202829737504[6] = -nom_x[6] + true_x[6];
out_7226800202829737504[7] = -nom_x[7] + true_x[7];
out_7226800202829737504[8] = -nom_x[8] + true_x[8];
out_7226800202829737504[9] = -nom_x[9] + true_x[9];
out_7226800202829737504[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419) {
out_5435998030533772419[0] = -nom_x[0] + true_x[0];
out_5435998030533772419[1] = -nom_x[1] + true_x[1];
out_5435998030533772419[2] = -nom_x[2] + true_x[2];
out_5435998030533772419[3] = -nom_x[3] + true_x[3];
out_5435998030533772419[4] = -nom_x[4] + true_x[4];
out_5435998030533772419[5] = -nom_x[5] + true_x[5];
out_5435998030533772419[6] = -nom_x[6] + true_x[6];
out_5435998030533772419[7] = -nom_x[7] + true_x[7];
out_5435998030533772419[8] = -nom_x[8] + true_x[8];
out_5435998030533772419[9] = -nom_x[9] + true_x[9];
out_5435998030533772419[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_7212715412967761026) {
out_7212715412967761026[0] = 1.0;
out_7212715412967761026[1] = 0;
out_7212715412967761026[2] = 0;
out_7212715412967761026[3] = 0;
out_7212715412967761026[4] = 0;
out_7212715412967761026[5] = 0;
out_7212715412967761026[6] = 0;
out_7212715412967761026[7] = 0;
out_7212715412967761026[8] = 0;
out_7212715412967761026[9] = 0;
out_7212715412967761026[10] = 0;
out_7212715412967761026[11] = 0;
out_7212715412967761026[12] = 1.0;
out_7212715412967761026[13] = 0;
out_7212715412967761026[14] = 0;
out_7212715412967761026[15] = 0;
out_7212715412967761026[16] = 0;
out_7212715412967761026[17] = 0;
out_7212715412967761026[18] = 0;
out_7212715412967761026[19] = 0;
out_7212715412967761026[20] = 0;
out_7212715412967761026[21] = 0;
out_7212715412967761026[22] = 0;
out_7212715412967761026[23] = 0;
out_7212715412967761026[24] = 1.0;
out_7212715412967761026[25] = 0;
out_7212715412967761026[26] = 0;
out_7212715412967761026[27] = 0;
out_7212715412967761026[28] = 0;
out_7212715412967761026[29] = 0;
out_7212715412967761026[30] = 0;
out_7212715412967761026[31] = 0;
out_7212715412967761026[32] = 0;
out_7212715412967761026[33] = 0;
out_7212715412967761026[34] = 0;
out_7212715412967761026[35] = 0;
out_7212715412967761026[36] = 1.0;
out_7212715412967761026[37] = 0;
out_7212715412967761026[38] = 0;
out_7212715412967761026[39] = 0;
out_7212715412967761026[40] = 0;
out_7212715412967761026[41] = 0;
out_7212715412967761026[42] = 0;
out_7212715412967761026[43] = 0;
out_7212715412967761026[44] = 0;
out_7212715412967761026[45] = 0;
out_7212715412967761026[46] = 0;
out_7212715412967761026[47] = 0;
out_7212715412967761026[48] = 1.0;
out_7212715412967761026[49] = 0;
out_7212715412967761026[50] = 0;
out_7212715412967761026[51] = 0;
out_7212715412967761026[52] = 0;
out_7212715412967761026[53] = 0;
out_7212715412967761026[54] = 0;
out_7212715412967761026[55] = 0;
out_7212715412967761026[56] = 0;
out_7212715412967761026[57] = 0;
out_7212715412967761026[58] = 0;
out_7212715412967761026[59] = 0;
out_7212715412967761026[60] = 1.0;
out_7212715412967761026[61] = 0;
out_7212715412967761026[62] = 0;
out_7212715412967761026[63] = 0;
out_7212715412967761026[64] = 0;
out_7212715412967761026[65] = 0;
out_7212715412967761026[66] = 0;
out_7212715412967761026[67] = 0;
out_7212715412967761026[68] = 0;
out_7212715412967761026[69] = 0;
out_7212715412967761026[70] = 0;
out_7212715412967761026[71] = 0;
out_7212715412967761026[72] = 1.0;
out_7212715412967761026[73] = 0;
out_7212715412967761026[74] = 0;
out_7212715412967761026[75] = 0;
out_7212715412967761026[76] = 0;
out_7212715412967761026[77] = 0;
out_7212715412967761026[78] = 0;
out_7212715412967761026[79] = 0;
out_7212715412967761026[80] = 0;
out_7212715412967761026[81] = 0;
out_7212715412967761026[82] = 0;
out_7212715412967761026[83] = 0;
out_7212715412967761026[84] = 1.0;
out_7212715412967761026[85] = 0;
out_7212715412967761026[86] = 0;
out_7212715412967761026[87] = 0;
out_7212715412967761026[88] = 0;
out_7212715412967761026[89] = 0;
out_7212715412967761026[90] = 0;
out_7212715412967761026[91] = 0;
out_7212715412967761026[92] = 0;
out_7212715412967761026[93] = 0;
out_7212715412967761026[94] = 0;
out_7212715412967761026[95] = 0;
out_7212715412967761026[96] = 1.0;
out_7212715412967761026[97] = 0;
out_7212715412967761026[98] = 0;
out_7212715412967761026[99] = 0;
out_7212715412967761026[100] = 0;
out_7212715412967761026[101] = 0;
out_7212715412967761026[102] = 0;
out_7212715412967761026[103] = 0;
out_7212715412967761026[104] = 0;
out_7212715412967761026[105] = 0;
out_7212715412967761026[106] = 0;
out_7212715412967761026[107] = 0;
out_7212715412967761026[108] = 1.0;
out_7212715412967761026[109] = 0;
out_7212715412967761026[110] = 0;
out_7212715412967761026[111] = 0;
out_7212715412967761026[112] = 0;
out_7212715412967761026[113] = 0;
out_7212715412967761026[114] = 0;
out_7212715412967761026[115] = 0;
out_7212715412967761026[116] = 0;
out_7212715412967761026[117] = 0;
out_7212715412967761026[118] = 0;
out_7212715412967761026[119] = 0;
out_7212715412967761026[120] = 1.0;
void H_mod_fun(double *state, double *out_8750904480143678209) {
out_8750904480143678209[0] = 1.0;
out_8750904480143678209[1] = 0;
out_8750904480143678209[2] = 0;
out_8750904480143678209[3] = 0;
out_8750904480143678209[4] = 0;
out_8750904480143678209[5] = 0;
out_8750904480143678209[6] = 0;
out_8750904480143678209[7] = 0;
out_8750904480143678209[8] = 0;
out_8750904480143678209[9] = 0;
out_8750904480143678209[10] = 0;
out_8750904480143678209[11] = 0;
out_8750904480143678209[12] = 1.0;
out_8750904480143678209[13] = 0;
out_8750904480143678209[14] = 0;
out_8750904480143678209[15] = 0;
out_8750904480143678209[16] = 0;
out_8750904480143678209[17] = 0;
out_8750904480143678209[18] = 0;
out_8750904480143678209[19] = 0;
out_8750904480143678209[20] = 0;
out_8750904480143678209[21] = 0;
out_8750904480143678209[22] = 0;
out_8750904480143678209[23] = 0;
out_8750904480143678209[24] = 1.0;
out_8750904480143678209[25] = 0;
out_8750904480143678209[26] = 0;
out_8750904480143678209[27] = 0;
out_8750904480143678209[28] = 0;
out_8750904480143678209[29] = 0;
out_8750904480143678209[30] = 0;
out_8750904480143678209[31] = 0;
out_8750904480143678209[32] = 0;
out_8750904480143678209[33] = 0;
out_8750904480143678209[34] = 0;
out_8750904480143678209[35] = 0;
out_8750904480143678209[36] = 1.0;
out_8750904480143678209[37] = 0;
out_8750904480143678209[38] = 0;
out_8750904480143678209[39] = 0;
out_8750904480143678209[40] = 0;
out_8750904480143678209[41] = 0;
out_8750904480143678209[42] = 0;
out_8750904480143678209[43] = 0;
out_8750904480143678209[44] = 0;
out_8750904480143678209[45] = 0;
out_8750904480143678209[46] = 0;
out_8750904480143678209[47] = 0;
out_8750904480143678209[48] = 1.0;
out_8750904480143678209[49] = 0;
out_8750904480143678209[50] = 0;
out_8750904480143678209[51] = 0;
out_8750904480143678209[52] = 0;
out_8750904480143678209[53] = 0;
out_8750904480143678209[54] = 0;
out_8750904480143678209[55] = 0;
out_8750904480143678209[56] = 0;
out_8750904480143678209[57] = 0;
out_8750904480143678209[58] = 0;
out_8750904480143678209[59] = 0;
out_8750904480143678209[60] = 1.0;
out_8750904480143678209[61] = 0;
out_8750904480143678209[62] = 0;
out_8750904480143678209[63] = 0;
out_8750904480143678209[64] = 0;
out_8750904480143678209[65] = 0;
out_8750904480143678209[66] = 0;
out_8750904480143678209[67] = 0;
out_8750904480143678209[68] = 0;
out_8750904480143678209[69] = 0;
out_8750904480143678209[70] = 0;
out_8750904480143678209[71] = 0;
out_8750904480143678209[72] = 1.0;
out_8750904480143678209[73] = 0;
out_8750904480143678209[74] = 0;
out_8750904480143678209[75] = 0;
out_8750904480143678209[76] = 0;
out_8750904480143678209[77] = 0;
out_8750904480143678209[78] = 0;
out_8750904480143678209[79] = 0;
out_8750904480143678209[80] = 0;
out_8750904480143678209[81] = 0;
out_8750904480143678209[82] = 0;
out_8750904480143678209[83] = 0;
out_8750904480143678209[84] = 1.0;
out_8750904480143678209[85] = 0;
out_8750904480143678209[86] = 0;
out_8750904480143678209[87] = 0;
out_8750904480143678209[88] = 0;
out_8750904480143678209[89] = 0;
out_8750904480143678209[90] = 0;
out_8750904480143678209[91] = 0;
out_8750904480143678209[92] = 0;
out_8750904480143678209[93] = 0;
out_8750904480143678209[94] = 0;
out_8750904480143678209[95] = 0;
out_8750904480143678209[96] = 1.0;
out_8750904480143678209[97] = 0;
out_8750904480143678209[98] = 0;
out_8750904480143678209[99] = 0;
out_8750904480143678209[100] = 0;
out_8750904480143678209[101] = 0;
out_8750904480143678209[102] = 0;
out_8750904480143678209[103] = 0;
out_8750904480143678209[104] = 0;
out_8750904480143678209[105] = 0;
out_8750904480143678209[106] = 0;
out_8750904480143678209[107] = 0;
out_8750904480143678209[108] = 1.0;
out_8750904480143678209[109] = 0;
out_8750904480143678209[110] = 0;
out_8750904480143678209[111] = 0;
out_8750904480143678209[112] = 0;
out_8750904480143678209[113] = 0;
out_8750904480143678209[114] = 0;
out_8750904480143678209[115] = 0;
out_8750904480143678209[116] = 0;
out_8750904480143678209[117] = 0;
out_8750904480143678209[118] = 0;
out_8750904480143678209[119] = 0;
out_8750904480143678209[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_4622219999608348212) {
out_4622219999608348212[0] = dt*state[3] + state[0];
out_4622219999608348212[1] = dt*state[4] + state[1];
out_4622219999608348212[2] = dt*state[5] + state[2];
out_4622219999608348212[3] = state[3];
out_4622219999608348212[4] = state[4];
out_4622219999608348212[5] = state[5];
out_4622219999608348212[6] = dt*state[7] + state[6];
out_4622219999608348212[7] = dt*state[8] + state[7];
out_4622219999608348212[8] = state[8];
out_4622219999608348212[9] = state[9];
out_4622219999608348212[10] = state[10];
void f_fun(double *state, double dt, double *out_571033032076185663) {
out_571033032076185663[0] = dt*state[3] + state[0];
out_571033032076185663[1] = dt*state[4] + state[1];
out_571033032076185663[2] = dt*state[5] + state[2];
out_571033032076185663[3] = state[3];
out_571033032076185663[4] = state[4];
out_571033032076185663[5] = state[5];
out_571033032076185663[6] = dt*state[7] + state[6];
out_571033032076185663[7] = dt*state[8] + state[7];
out_571033032076185663[8] = state[8];
out_571033032076185663[9] = state[9];
out_571033032076185663[10] = state[10];
}
void F_fun(double *state, double dt, double *out_1489675256084431222) {
out_1489675256084431222[0] = 1;
out_1489675256084431222[1] = 0;
out_1489675256084431222[2] = 0;
out_1489675256084431222[3] = dt;
out_1489675256084431222[4] = 0;
out_1489675256084431222[5] = 0;
out_1489675256084431222[6] = 0;
out_1489675256084431222[7] = 0;
out_1489675256084431222[8] = 0;
out_1489675256084431222[9] = 0;
out_1489675256084431222[10] = 0;
out_1489675256084431222[11] = 0;
out_1489675256084431222[12] = 1;
out_1489675256084431222[13] = 0;
out_1489675256084431222[14] = 0;
out_1489675256084431222[15] = dt;
out_1489675256084431222[16] = 0;
out_1489675256084431222[17] = 0;
out_1489675256084431222[18] = 0;
out_1489675256084431222[19] = 0;
out_1489675256084431222[20] = 0;
out_1489675256084431222[21] = 0;
out_1489675256084431222[22] = 0;
out_1489675256084431222[23] = 0;
out_1489675256084431222[24] = 1;
out_1489675256084431222[25] = 0;
out_1489675256084431222[26] = 0;
out_1489675256084431222[27] = dt;
out_1489675256084431222[28] = 0;
out_1489675256084431222[29] = 0;
out_1489675256084431222[30] = 0;
out_1489675256084431222[31] = 0;
out_1489675256084431222[32] = 0;
out_1489675256084431222[33] = 0;
out_1489675256084431222[34] = 0;
out_1489675256084431222[35] = 0;
out_1489675256084431222[36] = 1;
out_1489675256084431222[37] = 0;
out_1489675256084431222[38] = 0;
out_1489675256084431222[39] = 0;
out_1489675256084431222[40] = 0;
out_1489675256084431222[41] = 0;
out_1489675256084431222[42] = 0;
out_1489675256084431222[43] = 0;
out_1489675256084431222[44] = 0;
out_1489675256084431222[45] = 0;
out_1489675256084431222[46] = 0;
out_1489675256084431222[47] = 0;
out_1489675256084431222[48] = 1;
out_1489675256084431222[49] = 0;
out_1489675256084431222[50] = 0;
out_1489675256084431222[51] = 0;
out_1489675256084431222[52] = 0;
out_1489675256084431222[53] = 0;
out_1489675256084431222[54] = 0;
out_1489675256084431222[55] = 0;
out_1489675256084431222[56] = 0;
out_1489675256084431222[57] = 0;
out_1489675256084431222[58] = 0;
out_1489675256084431222[59] = 0;
out_1489675256084431222[60] = 1;
out_1489675256084431222[61] = 0;
out_1489675256084431222[62] = 0;
out_1489675256084431222[63] = 0;
out_1489675256084431222[64] = 0;
out_1489675256084431222[65] = 0;
out_1489675256084431222[66] = 0;
out_1489675256084431222[67] = 0;
out_1489675256084431222[68] = 0;
out_1489675256084431222[69] = 0;
out_1489675256084431222[70] = 0;
out_1489675256084431222[71] = 0;
out_1489675256084431222[72] = 1;
out_1489675256084431222[73] = dt;
out_1489675256084431222[74] = 0;
out_1489675256084431222[75] = 0;
out_1489675256084431222[76] = 0;
out_1489675256084431222[77] = 0;
out_1489675256084431222[78] = 0;
out_1489675256084431222[79] = 0;
out_1489675256084431222[80] = 0;
out_1489675256084431222[81] = 0;
out_1489675256084431222[82] = 0;
out_1489675256084431222[83] = 0;
out_1489675256084431222[84] = 1;
out_1489675256084431222[85] = dt;
out_1489675256084431222[86] = 0;
out_1489675256084431222[87] = 0;
out_1489675256084431222[88] = 0;
out_1489675256084431222[89] = 0;
out_1489675256084431222[90] = 0;
out_1489675256084431222[91] = 0;
out_1489675256084431222[92] = 0;
out_1489675256084431222[93] = 0;
out_1489675256084431222[94] = 0;
out_1489675256084431222[95] = 0;
out_1489675256084431222[96] = 1;
out_1489675256084431222[97] = 0;
out_1489675256084431222[98] = 0;
out_1489675256084431222[99] = 0;
out_1489675256084431222[100] = 0;
out_1489675256084431222[101] = 0;
out_1489675256084431222[102] = 0;
out_1489675256084431222[103] = 0;
out_1489675256084431222[104] = 0;
out_1489675256084431222[105] = 0;
out_1489675256084431222[106] = 0;
out_1489675256084431222[107] = 0;
out_1489675256084431222[108] = 1;
out_1489675256084431222[109] = 0;
out_1489675256084431222[110] = 0;
out_1489675256084431222[111] = 0;
out_1489675256084431222[112] = 0;
out_1489675256084431222[113] = 0;
out_1489675256084431222[114] = 0;
out_1489675256084431222[115] = 0;
out_1489675256084431222[116] = 0;
out_1489675256084431222[117] = 0;
out_1489675256084431222[118] = 0;
out_1489675256084431222[119] = 0;
out_1489675256084431222[120] = 1;
void F_fun(double *state, double dt, double *out_8071961880976414619) {
out_8071961880976414619[0] = 1;
out_8071961880976414619[1] = 0;
out_8071961880976414619[2] = 0;
out_8071961880976414619[3] = dt;
out_8071961880976414619[4] = 0;
out_8071961880976414619[5] = 0;
out_8071961880976414619[6] = 0;
out_8071961880976414619[7] = 0;
out_8071961880976414619[8] = 0;
out_8071961880976414619[9] = 0;
out_8071961880976414619[10] = 0;
out_8071961880976414619[11] = 0;
out_8071961880976414619[12] = 1;
out_8071961880976414619[13] = 0;
out_8071961880976414619[14] = 0;
out_8071961880976414619[15] = dt;
out_8071961880976414619[16] = 0;
out_8071961880976414619[17] = 0;
out_8071961880976414619[18] = 0;
out_8071961880976414619[19] = 0;
out_8071961880976414619[20] = 0;
out_8071961880976414619[21] = 0;
out_8071961880976414619[22] = 0;
out_8071961880976414619[23] = 0;
out_8071961880976414619[24] = 1;
out_8071961880976414619[25] = 0;
out_8071961880976414619[26] = 0;
out_8071961880976414619[27] = dt;
out_8071961880976414619[28] = 0;
out_8071961880976414619[29] = 0;
out_8071961880976414619[30] = 0;
out_8071961880976414619[31] = 0;
out_8071961880976414619[32] = 0;
out_8071961880976414619[33] = 0;
out_8071961880976414619[34] = 0;
out_8071961880976414619[35] = 0;
out_8071961880976414619[36] = 1;
out_8071961880976414619[37] = 0;
out_8071961880976414619[38] = 0;
out_8071961880976414619[39] = 0;
out_8071961880976414619[40] = 0;
out_8071961880976414619[41] = 0;
out_8071961880976414619[42] = 0;
out_8071961880976414619[43] = 0;
out_8071961880976414619[44] = 0;
out_8071961880976414619[45] = 0;
out_8071961880976414619[46] = 0;
out_8071961880976414619[47] = 0;
out_8071961880976414619[48] = 1;
out_8071961880976414619[49] = 0;
out_8071961880976414619[50] = 0;
out_8071961880976414619[51] = 0;
out_8071961880976414619[52] = 0;
out_8071961880976414619[53] = 0;
out_8071961880976414619[54] = 0;
out_8071961880976414619[55] = 0;
out_8071961880976414619[56] = 0;
out_8071961880976414619[57] = 0;
out_8071961880976414619[58] = 0;
out_8071961880976414619[59] = 0;
out_8071961880976414619[60] = 1;
out_8071961880976414619[61] = 0;
out_8071961880976414619[62] = 0;
out_8071961880976414619[63] = 0;
out_8071961880976414619[64] = 0;
out_8071961880976414619[65] = 0;
out_8071961880976414619[66] = 0;
out_8071961880976414619[67] = 0;
out_8071961880976414619[68] = 0;
out_8071961880976414619[69] = 0;
out_8071961880976414619[70] = 0;
out_8071961880976414619[71] = 0;
out_8071961880976414619[72] = 1;
out_8071961880976414619[73] = dt;
out_8071961880976414619[74] = 0;
out_8071961880976414619[75] = 0;
out_8071961880976414619[76] = 0;
out_8071961880976414619[77] = 0;
out_8071961880976414619[78] = 0;
out_8071961880976414619[79] = 0;
out_8071961880976414619[80] = 0;
out_8071961880976414619[81] = 0;
out_8071961880976414619[82] = 0;
out_8071961880976414619[83] = 0;
out_8071961880976414619[84] = 1;
out_8071961880976414619[85] = dt;
out_8071961880976414619[86] = 0;
out_8071961880976414619[87] = 0;
out_8071961880976414619[88] = 0;
out_8071961880976414619[89] = 0;
out_8071961880976414619[90] = 0;
out_8071961880976414619[91] = 0;
out_8071961880976414619[92] = 0;
out_8071961880976414619[93] = 0;
out_8071961880976414619[94] = 0;
out_8071961880976414619[95] = 0;
out_8071961880976414619[96] = 1;
out_8071961880976414619[97] = 0;
out_8071961880976414619[98] = 0;
out_8071961880976414619[99] = 0;
out_8071961880976414619[100] = 0;
out_8071961880976414619[101] = 0;
out_8071961880976414619[102] = 0;
out_8071961880976414619[103] = 0;
out_8071961880976414619[104] = 0;
out_8071961880976414619[105] = 0;
out_8071961880976414619[106] = 0;
out_8071961880976414619[107] = 0;
out_8071961880976414619[108] = 1;
out_8071961880976414619[109] = 0;
out_8071961880976414619[110] = 0;
out_8071961880976414619[111] = 0;
out_8071961880976414619[112] = 0;
out_8071961880976414619[113] = 0;
out_8071961880976414619[114] = 0;
out_8071961880976414619[115] = 0;
out_8071961880976414619[116] = 0;
out_8071961880976414619[117] = 0;
out_8071961880976414619[118] = 0;
out_8071961880976414619[119] = 0;
out_8071961880976414619[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_7567908033892933921) {
out_7567908033892933921[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
void h_6(double *state, double *sat_pos, double *out_3924727623087114070) {
out_3924727623087114070[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
}
void H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
out_7455993273502557413[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7455993273502557413[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7455993273502557413[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7455993273502557413[3] = 0;
out_7455993273502557413[4] = 0;
out_7455993273502557413[5] = 0;
out_7455993273502557413[6] = 1;
out_7455993273502557413[7] = 0;
out_7455993273502557413[8] = 0;
out_7455993273502557413[9] = 0;
out_7455993273502557413[10] = 0;
void H_6(double *state, double *sat_pos, double *out_3168437365824362595) {
out_3168437365824362595[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3168437365824362595[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3168437365824362595[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_3168437365824362595[3] = 0;
out_3168437365824362595[4] = 0;
out_3168437365824362595[5] = 0;
out_3168437365824362595[6] = 1;
out_3168437365824362595[7] = 0;
out_3168437365824362595[8] = 0;
out_3168437365824362595[9] = 0;
out_3168437365824362595[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_3841468768416351937) {
out_3841468768416351937[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
void h_20(double *state, double *sat_pos, double *out_7434817830173438872) {
out_7434817830173438872[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
}
void H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
out_5623941665124898301[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5623941665124898301[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5623941665124898301[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_5623941665124898301[3] = 0;
out_5623941665124898301[4] = 0;
out_5623941665124898301[5] = 0;
out_5623941665124898301[6] = 1;
out_5623941665124898301[7] = 0;
out_5623941665124898301[8] = 0;
out_5623941665124898301[9] = 1;
out_5623941665124898301[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_7560749815445785198) {
out_7560749815445785198[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7560749815445785198[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7560749815445785198[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
out_7560749815445785198[3] = 0;
out_7560749815445785198[4] = 0;
out_7560749815445785198[5] = 0;
out_7560749815445785198[6] = 1;
out_7560749815445785198[7] = 0;
out_7560749815445785198[8] = 0;
out_7560749815445785198[9] = 1;
out_7560749815445785198[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_7(double *state, double *sat_pos_vel, double *out_7603906676528442007) {
out_7603906676528442007[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[6] = 0;
out_2159917500592958723[7] = 1;
out_2159917500592958723[8] = 0;
out_2159917500592958723[9] = 0;
out_2159917500592958723[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_8038884862828261433) {
out_8038884862828261433[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[6] = 0;
out_8038884862828261433[7] = 1;
out_8038884862828261433[8] = 0;
out_8038884862828261433[9] = 0;
out_8038884862828261433[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
void h_21(double *state, double *sat_pos_vel, double *out_7603906676528442007) {
out_7603906676528442007[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
}
void H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_2159917500592958723[6] = 0;
out_2159917500592958723[7] = 1;
out_2159917500592958723[8] = 0;
out_2159917500592958723[9] = 0;
out_2159917500592958723[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_8038884862828261433) {
out_8038884862828261433[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
out_8038884862828261433[6] = 0;
out_8038884862828261433[7] = 1;
out_8038884862828261433[8] = 0;
out_8038884862828261433[9] = 0;
out_8038884862828261433[10] = 0;
}
#include <eigen3/Eigen/Dense>
#include <iostream>
@@ -506,44 +506,44 @@ void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
update<1, 3, 0>(in_x, in_P, h_21, H_21, NULL, in_z, in_R, in_ea, MAHA_THRESH_21);
}
void gnss_err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
err_fun(nom_x, delta_x, out_1734633203049768488);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_6740791193231733360) {
err_fun(nom_x, delta_x, out_6740791193231733360);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
inv_err_fun(nom_x, true_x, out_7226800202829737504);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419) {
inv_err_fun(nom_x, true_x, out_5435998030533772419);
}
void gnss_H_mod_fun(double *state, double *out_7212715412967761026) {
H_mod_fun(state, out_7212715412967761026);
void gnss_H_mod_fun(double *state, double *out_8750904480143678209) {
H_mod_fun(state, out_8750904480143678209);
}
void gnss_f_fun(double *state, double dt, double *out_4622219999608348212) {
f_fun(state, dt, out_4622219999608348212);
void gnss_f_fun(double *state, double dt, double *out_571033032076185663) {
f_fun(state, dt, out_571033032076185663);
}
void gnss_F_fun(double *state, double dt, double *out_1489675256084431222) {
F_fun(state, dt, out_1489675256084431222);
void gnss_F_fun(double *state, double dt, double *out_8071961880976414619) {
F_fun(state, dt, out_8071961880976414619);
}
void gnss_h_6(double *state, double *sat_pos, double *out_7567908033892933921) {
h_6(state, sat_pos, out_7567908033892933921);
void gnss_h_6(double *state, double *sat_pos, double *out_3924727623087114070) {
h_6(state, sat_pos, out_3924727623087114070);
}
void gnss_H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
H_6(state, sat_pos, out_7455993273502557413);
void gnss_H_6(double *state, double *sat_pos, double *out_3168437365824362595) {
H_6(state, sat_pos, out_3168437365824362595);
}
void gnss_h_20(double *state, double *sat_pos, double *out_3841468768416351937) {
h_20(state, sat_pos, out_3841468768416351937);
void gnss_h_20(double *state, double *sat_pos, double *out_7434817830173438872) {
h_20(state, sat_pos, out_7434817830173438872);
}
void gnss_H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
H_20(state, sat_pos, out_5623941665124898301);
void gnss_H_20(double *state, double *sat_pos, double *out_7560749815445785198) {
H_20(state, sat_pos, out_7560749815445785198);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
h_7(state, sat_pos_vel, out_1211039377275791210);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7603906676528442007) {
h_7(state, sat_pos_vel, out_7603906676528442007);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
H_7(state, sat_pos_vel, out_2159917500592958723);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_8038884862828261433) {
H_7(state, sat_pos_vel, out_8038884862828261433);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
h_21(state, sat_pos_vel, out_1211039377275791210);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7603906676528442007) {
h_21(state, sat_pos_vel, out_7603906676528442007);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
H_21(state, sat_pos_vel, out_2159917500592958723);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_8038884862828261433) {
H_21(state, sat_pos_vel, out_8038884862828261433);
}
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
predict(in_x, in_P, in_Q, dt);
selfdrive/locationd/models/generated/gnss.h
+13 -13
View File
@@ -5,18 +5,18 @@ void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, doubl
void gnss_update_20(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_7(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_update_21(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504);
void gnss_H_mod_fun(double *state, double *out_7212715412967761026);
void gnss_f_fun(double *state, double dt, double *out_4622219999608348212);
void gnss_F_fun(double *state, double dt, double *out_1489675256084431222);
void gnss_h_6(double *state, double *sat_pos, double *out_7567908033892933921);
void gnss_H_6(double *state, double *sat_pos, double *out_7455993273502557413);
void gnss_h_20(double *state, double *sat_pos, double *out_3841468768416351937);
void gnss_H_20(double *state, double *sat_pos, double *out_5623941665124898301);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_6740791193231733360);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_5435998030533772419);
void gnss_H_mod_fun(double *state, double *out_8750904480143678209);
void gnss_f_fun(double *state, double dt, double *out_571033032076185663);
void gnss_F_fun(double *state, double dt, double *out_8071961880976414619);
void gnss_h_6(double *state, double *sat_pos, double *out_3924727623087114070);
void gnss_H_6(double *state, double *sat_pos, double *out_3168437365824362595);
void gnss_h_20(double *state, double *sat_pos, double *out_7434817830173438872);
void gnss_H_20(double *state, double *sat_pos, double *out_7560749815445785198);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7603906676528442007);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_8038884862828261433);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7603906676528442007);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_8038884862828261433);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.cpp
+1702 -1702
View File
File diff suppressed because it is too large Load Diff
selfdrive/locationd/models/generated/live.h
+24 -24
View File
@@ -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_4500389823279432985);
void live_err_fun(double *nom_x, double *delta_x, double *out_1913535744177293566);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_2851117358798162907);
void live_H_mod_fun(double *state, double *out_2697157995522224710);
void live_f_fun(double *state, double dt, double *out_258969813900144001);
void live_F_fun(double *state, double dt, double *out_3982048512447115253);
void live_h_4(double *state, double *unused, double *out_3062002944620705961);
void live_H_4(double *state, double *unused, double *out_3011098296458653357);
void live_h_9(double *state, double *unused, double *out_5633073678966500073);
void live_H_9(double *state, double *unused, double *out_3252287943088244002);
void live_h_10(double *state, double *unused, double *out_4956664653644560279);
void live_H_10(double *state, double *unused, double *out_4513091182693755627);
void live_h_12(double *state, double *unused, double *out_5780595779529914179);
void live_H_12(double *state, double *unused, double *out_3632197321506247024);
void live_h_35(double *state, double *unused, double *out_190113666816480701);
void live_H_35(double *state, double *unused, double *out_6377760353831260733);
void live_h_32(double *state, double *unused, double *out_1962437249848893520);
void live_H_32(double *state, double *unused, double *out_6267679347670678325);
void live_h_13(double *state, double *unused, double *out_1422098576590259012);
void live_H_13(double *state, double *unused, double *out_979728642438066826);
void live_h_14(double *state, double *unused, double *out_5633073678966500073);
void live_H_14(double *state, double *unused, double *out_3252287943088244002);
void live_h_33(double *state, double *unused, double *out_4297372334290366217);
void live_H_33(double *state, double *unused, double *out_8918426715239433279);
void live_H(double *in_vec, double *out_3693999405139314794);
void live_err_fun(double *nom_x, double *delta_x, double *out_3414598820419176679);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_5364111469669926070);
void live_H_mod_fun(double *state, double *out_8664355600254952032);
void live_f_fun(double *state, double dt, double *out_5944879704050977948);
void live_F_fun(double *state, double dt, double *out_1511312995606333672);
void live_h_4(double *state, double *unused, double *out_6748587584352144008);
void live_H_4(double *state, double *unused, double *out_6619224273712010162);
void live_h_9(double *state, double *unused, double *out_8367559596262948470);
void live_H_9(double *state, double *unused, double *out_6860413920341600807);
void live_h_10(double *state, double *unused, double *out_9168195956800795275);
void live_H_10(double *state, double *unused, double *out_1822418700523668742);
void live_h_12(double *state, double *unused, double *out_2333080179053030376);
void live_H_12(double *state, double *unused, double *out_6808063391965579659);
void live_h_35(double *state, double *unused, double *out_4784374292918501119);
void live_H_35(double *state, double *unused, double *out_8460857742624934078);
void live_h_32(double *state, double *unused, double *out_2752828006802561656);
void live_H_32(double *state, double *unused, double *out_8329692957684193609);
void live_h_13(double *state, double *unused, double *out_392591640858112584);
void live_H_13(double *state, double *unused, double *out_4626093262240260019);
void live_h_14(double *state, double *unused, double *out_8367559596262948470);
void live_H_14(double *state, double *unused, double *out_6860413920341600807);
void live_h_33(double *state, double *unused, double *out_9004392775721978689);
void live_H_33(double *state, double *unused, double *out_5310300737986076474);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/ubloxd
BIN
View File
Binary file not shown.
selfdrive/modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/sensord/_sensord
BIN
View File
Binary file not shown.
selfdrive/ui/_ui
BIN
View File
Binary file not shown.
selfdrive/ui/_ui_nonav
BIN
View File
Binary file not shown.
selfdrive/ui/qt/spinner
BIN
View File
Binary file not shown.
selfdrive/ui/qt/text
BIN
View File
Binary file not shown.
selfdrive/ui/soundd/_soundd
BIN
View File
Binary file not shown.
system/camerad/camerad
BIN
View File
Binary file not shown.
system/clocksd/clocksd
BIN
View File
Binary file not shown.