mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-30 03:13:41 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot beta3

Browse Source 
date: 2023-08-14T16:27:10
commit: 90b4f6596bf852f4add5a8faa7581aabb57e4ba4
This commit is contained in:
dragonpilot
2023-08-14 16:24:20 +08:00
parent 53534a739b
commit 58934b517d
82 changed files with 2994 additions and 3024 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOGS.md
+1 -1
View File
@@ -1,4 +1,4 @@
dragonpilot beta3 2023.08.12
dragonpilot beta3 2023.08.14
=======================
* openpilot master branch on 2023.07.21.
body/board/obj/body.bin
BIN
View File
Binary file not shown.
body/board/obj/body.bin.signed
BIN
View File
Binary file not shown.
body/board/obj/body.elf
BIN
View File
Binary file not shown.
body/board/obj/bootstub.body.bin
BIN
View File
Binary file not shown.
body/board/obj/bootstub.body.elf
BIN
View File
Binary file not shown.
body/board/obj/gitversion.h
+1 -1
View File
@@ -1 +1 @@
const uint8_t gitversion[8] = "3a753738";
const uint8_t gitversion[8] = "56e43cbd";
cereal/messaging/__init__.py
+2 -3
View File
@@ -1,7 +1,6 @@
# must be built with scons
from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
from .messaging_pyx import MultiplePublishersError, MessagingError
from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event # pylint: disable=no-name-in-module, import-error
from .messaging_pyx import MultiplePublishersError, MessagingError # pylint: disable=no-name-in-module, import-error
import os
import capnp
cereal/messaging/messaging.h
-12
View File
@@ -116,18 +116,6 @@ public:
return heapArray_.asBytes();
}
size_t getSerializedSize() {
return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word);
}
int serializeToBuffer(unsigned char *buffer, size_t buffer_size) {
size_t serialized_size = getSerializedSize();
if (serialized_size > buffer_size) { return -1; }
kj::ArrayOutputStream out(kj::ArrayPtr<capnp::byte>(buffer, buffer_size));
capnp::writeMessage(out, *this);
return serialized_size;
}
private:
kj::Array<capnp::word> heapArray_;
};
cereal/visionipc/__init__.py
+1 -1
View File
@@ -1,4 +1,4 @@
from cereal.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name
from cereal.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name # pylint: disable=no-name-in-module, import-error
assert VisionBuf
assert VisionIpcClient
assert VisionIpcServer
cereal/visionipc/visionipc_pyx.so
BIN
View File
Binary file not shown.
common/params_pyx.so
BIN
View File
Binary file not shown.
common/version.h
+1 -1
View File
@@ -1 +1 @@
#define COMMA_VERSION "2023.08.12"
#define COMMA_VERSION "2023.08.14"
laika/astro_dog.py
+1 -1
View File
@@ -40,7 +40,7 @@ class AstroDog:
for const in valid_const:
if not isinstance(const, ConstellationId):
raise TypeError(f"valid_const must be a list of ConstellationId, got {const}")
self.auto_update = auto_update
self.cache_dir = cache_dir
self.clear_old_ephemeris = clear_old_ephemeris
laika/downloader.py
+1 -1
View File
@@ -185,7 +185,7 @@ def https_download_file(url):
crl.setopt(crl.SSL_CIPHER_LIST, 'DEFAULT@SECLEVEL=1')
crl.setopt(crl.COOKIEJAR, '/tmp/cddis_cookies')
crl.setopt(pycurl.CONNECTTIMEOUT, 10)
buf = BytesIO()
crl.setopt(crl.WRITEDATA, buf)
crl.perform()
laika/ephemeris.py
+3 -3
View File
@@ -455,7 +455,7 @@ def parse_rinex_nav_msg_glonass(file_name):
e['n4'], e['nt'], toe_seconds = epoch.as_glonass()
tb = toe_seconds / (15 * SECS_IN_MIN)
e['tb'] = tb
e['tauN'] = -float(line[23:42])
@@ -465,12 +465,12 @@ def parse_rinex_nav_msg_glonass(file_name):
e['x'], e['xVel'], e['xAccel'], e['svHealth'] = read4(f, rinex_ver)
e['y'], e['yVel'], e['yAccel'], e['freqNum'] = read4(f, rinex_ver)
e['z'], e['zVel'], e['zAccel'], e['age'] = read4(f, rinex_ver)
# TODO unclear why glonass sometimes has nav messages 3s after correct one
if abs(tb - int(tb)) > 1e-3:
continue
data_struct = ephemeris_structs.GlonassEphemeris.new_message(**e)
ephem = GLONASSEphemeris(data_struct, file_name=file_name)
laika/raw_gnss.py
+3 -4
View File
@@ -53,8 +53,8 @@ class GNSSMeasurement:
SAT_POS = slice(8, 11)
SAT_VEL = slice(11, 14)
def __init__(self, constellation_id: ConstellationId, sv_id: int, recv_time_week: int, recv_time_sec: float, observables: Dict[str, float],
observables_std: Dict[str, float], glonass_freq: Union[int, float, None] = None):
def __init__(self, constellation_id: ConstellationId, sv_id: int, recv_time_week: int, recv_time_sec: float, observables: Dict[str, float], observables_std: Dict[str, float],
glonass_freq: Union[int, float, None] = None):
# Metadata
# prn: unique satellite id
self.prn = "%s%02d" % (constellation_id.to_rinex_char(), sv_id) # satellite ID in rinex convention
@@ -201,8 +201,7 @@ def read_raw_qcom(report):
# logging.debug(recv_time, report.source, time_bias_ms, dr)
measurements = []
for i in report.sv:
# todo change svId to nmea_id in cereal message. Or better: change the publisher to publish correct svId's, since constellation id is also given
nmea_id = i.svId
nmea_id = i.svId # todo change svId to nmea_id in cereal message. Or better: change the publisher to publish correct svId's, since constellation id is also given
if nmea_id == 255:
# TODO nmea_id is not valid. Fix publisher
continue
opendbc/can/libdbc.so
BIN
View File
Binary file not shown.
opendbc/can/parser_pyx.cpp
+436 -460
View File
File diff suppressed because it is too large Load Diff
opendbc/can/parser_pyx.so
BIN
View File
Binary file not shown.
opendbc/toyota_new_mc_pt_generated.dbc
+3 -5
View File
@@ -253,7 +253,6 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -261,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
BO_ 1041 PCS_HUD: 8 DSU
BO_ 1041 ACC_HUD: 8 DSU
SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -433,7 +432,7 @@ BO_ 1880 DEBUG: 8 XXX
SG_ BLINDSPOTD2 : 55|8@0+ (1,0) [0|255] "" XXX
CM_ SG_ 36 YAW_RATE "verify";
CM_ SG_ 36 ACCEL_X "x-axis accel";
CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -448,7 +447,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -536,7 +535,6 @@ VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
opendbc/toyota_nodsu_pt_generated.dbc
+3 -5
View File
@@ -253,7 +253,6 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -261,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
BO_ 1041 PCS_HUD: 8 DSU
BO_ 1041 ACC_HUD: 8 DSU
SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -433,7 +432,7 @@ BO_ 1880 DEBUG: 8 XXX
SG_ BLINDSPOTD2 : 55|8@0+ (1,0) [0|255] "" XXX
CM_ SG_ 36 YAW_RATE "verify";
CM_ SG_ 36 ACCEL_X "x-axis accel";
CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -448,7 +447,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -536,7 +535,6 @@ VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
opendbc/toyota_tnga_k_pt_generated.dbc
+3 -5
View File
@@ -253,7 +253,6 @@ BO_ 1005 REVERSE_CAMERA_STATE: 2 BGM
SG_ REVERSE_CAMERA_GUIDELINES : 9|2@0+ (1,0) [1|3] "" XXX
BO_ 1009 PCM_CRUISE_ALT: 8 XXX
SG_ PCM_FOLLOW_DISTANCE : 4|2@1+ (1,0) [0|3] "" XXX
SG_ MAIN_ON : 13|1@0+ (1,0) [0|3] "" XXX
SG_ CRUISE_STATE : 10|1@0+ (1,0) [0|1] "" XXX
SG_ UI_SET_SPEED : 23|8@0+ (1,0) [0|255] "mph" XXX
@@ -261,7 +260,7 @@ BO_ 1009 PCM_CRUISE_ALT: 8 XXX
BO_ 1020 SOLAR_SENSOR: 8 XXX
SG_ LUX_SENSOR : 55|13@0+ (1,0) [0|0] "" XXX
BO_ 1041 PCS_HUD: 8 DSU
BO_ 1041 ACC_HUD: 8 DSU
SG_ PCS_INDICATOR : 7|2@0+ (1,0) [0|3] "" XXX
SG_ FCW : 4|1@0+ (1,0) [0|1] "" XXX
SG_ SET_ME_X20 : 15|8@0+ (1,0) [0|1] "" XXX
@@ -433,7 +432,7 @@ BO_ 1880 DEBUG: 8 XXX
SG_ BLINDSPOTD2 : 55|8@0+ (1,0) [0|255] "" XXX
CM_ SG_ 36 YAW_RATE "verify";
CM_ SG_ 36 ACCEL_X "x-axis accel";
CM_ SG_ 36 STEERING_TORQUE "does not seem the steer torque, tbd";
CM_ SG_ 37 STEER_FRACTION "1/15th of the signal STEER_ANGLE, which is 1.5 deg; note that 0x8 is never set";
CM_ SG_ 37 STEER_RATE "factor is tbd";
CM_ SG_ 466 NEUTRAL_FORCE "force in newtons the engine/electric motors are applying without any acceleration commands or user input";
@@ -448,7 +447,7 @@ CM_ SG_ 581 GAS_PEDAL "it seems slightly filtered";
CM_ SG_ 608 STEER_TORQUE_DRIVER "driver torque";
CM_ SG_ 608 STEER_OVERRIDE "set when driver torque exceeds a certain value";
CM_ SG_ 614 ANGLE "set to measured angle when ipas control isn't active";
CM_ SG_ 643 _COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 COUNTER "only used on cars that use this msg for cruise control";
CM_ SG_ 643 BRAKE_STATUS "only used on cars that use this msg for cruise control";
CM_ SG_ 643 PRECOLLISION_ACTIVE "set 0.5s before any braking";
CM_ SG_ 835 ACC_TYPE "if 2, car is likely to have a permanent low speed lockout. 1 is ok";
@@ -536,7 +535,6 @@ VAL_ 956 SPORT_ON_2 0 "off" 1 "on";
VAL_ 956 B_GEAR_ENGAGED 0 "off" 1 "on";
VAL_ 956 DRIVE_ENGAGED 0 "off" 1 "on";
VAL_ 1005 REVERSE_CAMERA_GUIDELINES 3 "No guidelines" 2 "Static guidelines" 1 "Active guidelines";
VAL_ 1009 PCM_FOLLOW_DISTANCE 1 "far" 2 "medium" 3 "close";
VAL_ 1041 PCS_INDICATOR 2 "PCS Faulted" 1 "PCS Turned Off By User" 0 "PCS Enabled";
VAL_ 1041 PCS_SENSITIVITY 64 "high sensitivity" 128 "mid sensitivity" 192 "low sensitivity" 0 "off";
VAL_ 1042 LDA_ALERT 3 "hold with continuous beep" 2 "LDA unavailable" 1 "hold" 0 "none";
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_h7.bin.signed
BIN
View File
Binary file not shown.
panda/python/__init__.py
+15 -3
View File
@@ -7,6 +7,7 @@ import struct
import hashlib
import binascii
import datetime
import warnings
import logging
from functools import wraps
from typing import Optional
@@ -424,10 +425,10 @@ class Panda:
if device.getVendorID() == 0xbbaa and device.getProductID() in cls.USB_PIDS:
try:
serial = device.getSerialNumber()
if len(serial) == 24 or serial == "pedal":
if len(serial) == 24:
ret.append(serial)
else:
logging.warning(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
warnings.warn(f"found device with panda descriptors but invalid serial: {serial}", RuntimeWarning)
except Exception:
continue
except Exception:
@@ -473,7 +474,11 @@ class Panda:
success = True
break
except Exception:
pass
try:
dfu = PandaDFU(self.get_dfu_serial())
dfu.recover()
except Exception:
pass
time.sleep(0.1)
if not success:
raise Exception("reconnect failed")
@@ -767,6 +772,13 @@ class Panda:
def enable_deepsleep(self):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xfb, 0, 0, b'')
def set_esp_power(self, on):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xd9, int(on), 0, b'')
def esp_reset(self, bootmode=0):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xda, int(bootmode), 0, b'')
time.sleep(0.2)
def set_safety_mode(self, mode=SAFETY_SILENT, param=0):
self._handle.controlWrite(Panda.REQUEST_OUT, 0xdc, mode, param, b'')
panda/python/isotp.py
+6 -4
View File
@@ -6,8 +6,10 @@ DEBUG = False
def msg(x):
if DEBUG:
print("S:", binascii.hexlify(x))
assert len(x) <= 7
ret = bytes([len(x)]) + x
if len(x) <= 7:
ret = bytes([len(x)]) + x
else:
assert False
return ret.ljust(8, b"\x00")
kmsgs = []
@@ -54,7 +56,7 @@ def isotp_recv_subaddr(panda, addr, bus, sendaddr, subaddr):
dat = msg[2:]
else:
print(binascii.hexlify(msg))
raise AssertionError
assert False
return dat[0:tlen]
@@ -131,7 +133,7 @@ def isotp_recv(panda, addr, bus=0, sendaddr=None, subaddr=None):
tlen = msg[0] & 0xf
dat = msg[1:]
else:
raise AssertionError
assert False
dat = dat[0:tlen]
if DEBUG:
panda/python/spi.py
+1 -1
View File
@@ -314,7 +314,7 @@ class STBootloaderSPIHandle(BaseSTBootloaderHandle):
self._mcu_type = MCU_TYPE_BY_IDCODE[self.get_chip_id()]
except PandaSpiException:
raise PandaSpiException("failed to connect to panda") from None
raise PandaSpiException("failed to connect to panda") # pylint: disable=W0707
def _get_ack(self, spi, timeout=1.0):
data = 0x00
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
+1 -1
View File
@@ -646,7 +646,7 @@ class Controls:
(not standstill or self.joystick_mode)
CC.longActive = self.enabled and not self.events.any(ET.OVERRIDE_LONGITUDINAL) and self.CP.openpilotLongitudinalControl
if self._dp_alka_active and not standstill and CS.cruiseState.available:
if not self.read_only and self.initialized and self._dp_alka_active and not standstill and CS.cruiseState.available:
if self.sm['liveCalibration'].calStatus != log.LiveCalibrationData.Status.calibrated:
pass
elif CS.steerFaultTemporary or CS.steerFaultPermanent:
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_8412408275907562510) {
out_8412408275907562510[0] = delta_x[0] + nom_x[0];
out_8412408275907562510[1] = delta_x[1] + nom_x[1];
out_8412408275907562510[2] = delta_x[2] + nom_x[2];
out_8412408275907562510[3] = delta_x[3] + nom_x[3];
out_8412408275907562510[4] = delta_x[4] + nom_x[4];
out_8412408275907562510[5] = delta_x[5] + nom_x[5];
out_8412408275907562510[6] = delta_x[6] + nom_x[6];
out_8412408275907562510[7] = delta_x[7] + nom_x[7];
out_8412408275907562510[8] = delta_x[8] + nom_x[8];
void err_fun(double *nom_x, double *delta_x, double *out_1282327502940326821) {
out_1282327502940326821[0] = delta_x[0] + nom_x[0];
out_1282327502940326821[1] = delta_x[1] + nom_x[1];
out_1282327502940326821[2] = delta_x[2] + nom_x[2];
out_1282327502940326821[3] = delta_x[3] + nom_x[3];
out_1282327502940326821[4] = delta_x[4] + nom_x[4];
out_1282327502940326821[5] = delta_x[5] + nom_x[5];
out_1282327502940326821[6] = delta_x[6] + nom_x[6];
out_1282327502940326821[7] = delta_x[7] + nom_x[7];
out_1282327502940326821[8] = delta_x[8] + nom_x[8];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_5009429012443487067) {
out_5009429012443487067[0] = -nom_x[0] + true_x[0];
out_5009429012443487067[1] = -nom_x[1] + true_x[1];
out_5009429012443487067[2] = -nom_x[2] + true_x[2];
out_5009429012443487067[3] = -nom_x[3] + true_x[3];
out_5009429012443487067[4] = -nom_x[4] + true_x[4];
out_5009429012443487067[5] = -nom_x[5] + true_x[5];
out_5009429012443487067[6] = -nom_x[6] + true_x[6];
out_5009429012443487067[7] = -nom_x[7] + true_x[7];
out_5009429012443487067[8] = -nom_x[8] + true_x[8];
void inv_err_fun(double *nom_x, double *true_x, double *out_4590721196226970281) {
out_4590721196226970281[0] = -nom_x[0] + true_x[0];
out_4590721196226970281[1] = -nom_x[1] + true_x[1];
out_4590721196226970281[2] = -nom_x[2] + true_x[2];
out_4590721196226970281[3] = -nom_x[3] + true_x[3];
out_4590721196226970281[4] = -nom_x[4] + true_x[4];
out_4590721196226970281[5] = -nom_x[5] + true_x[5];
out_4590721196226970281[6] = -nom_x[6] + true_x[6];
out_4590721196226970281[7] = -nom_x[7] + true_x[7];
out_4590721196226970281[8] = -nom_x[8] + true_x[8];
}
void H_mod_fun(double *state, double *out_7201068384612818103) {
out_7201068384612818103[0] = 1.0;
out_7201068384612818103[1] = 0;
out_7201068384612818103[2] = 0;
out_7201068384612818103[3] = 0;
out_7201068384612818103[4] = 0;
out_7201068384612818103[5] = 0;
out_7201068384612818103[6] = 0;
out_7201068384612818103[7] = 0;
out_7201068384612818103[8] = 0;
out_7201068384612818103[9] = 0;
out_7201068384612818103[10] = 1.0;
out_7201068384612818103[11] = 0;
out_7201068384612818103[12] = 0;
out_7201068384612818103[13] = 0;
out_7201068384612818103[14] = 0;
out_7201068384612818103[15] = 0;
out_7201068384612818103[16] = 0;
out_7201068384612818103[17] = 0;
out_7201068384612818103[18] = 0;
out_7201068384612818103[19] = 0;
out_7201068384612818103[20] = 1.0;
out_7201068384612818103[21] = 0;
out_7201068384612818103[22] = 0;
out_7201068384612818103[23] = 0;
out_7201068384612818103[24] = 0;
out_7201068384612818103[25] = 0;
out_7201068384612818103[26] = 0;
out_7201068384612818103[27] = 0;
out_7201068384612818103[28] = 0;
out_7201068384612818103[29] = 0;
out_7201068384612818103[30] = 1.0;
out_7201068384612818103[31] = 0;
out_7201068384612818103[32] = 0;
out_7201068384612818103[33] = 0;
out_7201068384612818103[34] = 0;
out_7201068384612818103[35] = 0;
out_7201068384612818103[36] = 0;
out_7201068384612818103[37] = 0;
out_7201068384612818103[38] = 0;
out_7201068384612818103[39] = 0;
out_7201068384612818103[40] = 1.0;
out_7201068384612818103[41] = 0;
out_7201068384612818103[42] = 0;
out_7201068384612818103[43] = 0;
out_7201068384612818103[44] = 0;
out_7201068384612818103[45] = 0;
out_7201068384612818103[46] = 0;
out_7201068384612818103[47] = 0;
out_7201068384612818103[48] = 0;
out_7201068384612818103[49] = 0;
out_7201068384612818103[50] = 1.0;
out_7201068384612818103[51] = 0;
out_7201068384612818103[52] = 0;
out_7201068384612818103[53] = 0;
out_7201068384612818103[54] = 0;
out_7201068384612818103[55] = 0;
out_7201068384612818103[56] = 0;
out_7201068384612818103[57] = 0;
out_7201068384612818103[58] = 0;
out_7201068384612818103[59] = 0;
out_7201068384612818103[60] = 1.0;
out_7201068384612818103[61] = 0;
out_7201068384612818103[62] = 0;
out_7201068384612818103[63] = 0;
out_7201068384612818103[64] = 0;
out_7201068384612818103[65] = 0;
out_7201068384612818103[66] = 0;
out_7201068384612818103[67] = 0;
out_7201068384612818103[68] = 0;
out_7201068384612818103[69] = 0;
out_7201068384612818103[70] = 1.0;
out_7201068384612818103[71] = 0;
out_7201068384612818103[72] = 0;
out_7201068384612818103[73] = 0;
out_7201068384612818103[74] = 0;
out_7201068384612818103[75] = 0;
out_7201068384612818103[76] = 0;
out_7201068384612818103[77] = 0;
out_7201068384612818103[78] = 0;
out_7201068384612818103[79] = 0;
out_7201068384612818103[80] = 1.0;
void H_mod_fun(double *state, double *out_2163953626510216661) {
out_2163953626510216661[0] = 1.0;
out_2163953626510216661[1] = 0;
out_2163953626510216661[2] = 0;
out_2163953626510216661[3] = 0;
out_2163953626510216661[4] = 0;
out_2163953626510216661[5] = 0;
out_2163953626510216661[6] = 0;
out_2163953626510216661[7] = 0;
out_2163953626510216661[8] = 0;
out_2163953626510216661[9] = 0;
out_2163953626510216661[10] = 1.0;
out_2163953626510216661[11] = 0;
out_2163953626510216661[12] = 0;
out_2163953626510216661[13] = 0;
out_2163953626510216661[14] = 0;
out_2163953626510216661[15] = 0;
out_2163953626510216661[16] = 0;
out_2163953626510216661[17] = 0;
out_2163953626510216661[18] = 0;
out_2163953626510216661[19] = 0;
out_2163953626510216661[20] = 1.0;
out_2163953626510216661[21] = 0;
out_2163953626510216661[22] = 0;
out_2163953626510216661[23] = 0;
out_2163953626510216661[24] = 0;
out_2163953626510216661[25] = 0;
out_2163953626510216661[26] = 0;
out_2163953626510216661[27] = 0;
out_2163953626510216661[28] = 0;
out_2163953626510216661[29] = 0;
out_2163953626510216661[30] = 1.0;
out_2163953626510216661[31] = 0;
out_2163953626510216661[32] = 0;
out_2163953626510216661[33] = 0;
out_2163953626510216661[34] = 0;
out_2163953626510216661[35] = 0;
out_2163953626510216661[36] = 0;
out_2163953626510216661[37] = 0;
out_2163953626510216661[38] = 0;
out_2163953626510216661[39] = 0;
out_2163953626510216661[40] = 1.0;
out_2163953626510216661[41] = 0;
out_2163953626510216661[42] = 0;
out_2163953626510216661[43] = 0;
out_2163953626510216661[44] = 0;
out_2163953626510216661[45] = 0;
out_2163953626510216661[46] = 0;
out_2163953626510216661[47] = 0;
out_2163953626510216661[48] = 0;
out_2163953626510216661[49] = 0;
out_2163953626510216661[50] = 1.0;
out_2163953626510216661[51] = 0;
out_2163953626510216661[52] = 0;
out_2163953626510216661[53] = 0;
out_2163953626510216661[54] = 0;
out_2163953626510216661[55] = 0;
out_2163953626510216661[56] = 0;
out_2163953626510216661[57] = 0;
out_2163953626510216661[58] = 0;
out_2163953626510216661[59] = 0;
out_2163953626510216661[60] = 1.0;
out_2163953626510216661[61] = 0;
out_2163953626510216661[62] = 0;
out_2163953626510216661[63] = 0;
out_2163953626510216661[64] = 0;
out_2163953626510216661[65] = 0;
out_2163953626510216661[66] = 0;
out_2163953626510216661[67] = 0;
out_2163953626510216661[68] = 0;
out_2163953626510216661[69] = 0;
out_2163953626510216661[70] = 1.0;
out_2163953626510216661[71] = 0;
out_2163953626510216661[72] = 0;
out_2163953626510216661[73] = 0;
out_2163953626510216661[74] = 0;
out_2163953626510216661[75] = 0;
out_2163953626510216661[76] = 0;
out_2163953626510216661[77] = 0;
out_2163953626510216661[78] = 0;
out_2163953626510216661[79] = 0;
out_2163953626510216661[80] = 1.0;
}
void f_fun(double *state, double dt, double *out_2117395969407442559) {
out_2117395969407442559[0] = state[0];
out_2117395969407442559[1] = state[1];
out_2117395969407442559[2] = state[2];
out_2117395969407442559[3] = state[3];
out_2117395969407442559[4] = state[4];
out_2117395969407442559[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_2117395969407442559[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_2117395969407442559[7] = state[7];
out_2117395969407442559[8] = state[8];
void f_fun(double *state, double dt, double *out_405606070335708994) {
out_405606070335708994[0] = state[0];
out_405606070335708994[1] = state[1];
out_405606070335708994[2] = state[2];
out_405606070335708994[3] = state[3];
out_405606070335708994[4] = state[4];
out_405606070335708994[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_405606070335708994[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_405606070335708994[7] = state[7];
out_405606070335708994[8] = state[8];
}
void F_fun(double *state, double dt, double *out_1930106235119052766) {
out_1930106235119052766[0] = 1;
out_1930106235119052766[1] = 0;
out_1930106235119052766[2] = 0;
out_1930106235119052766[3] = 0;
out_1930106235119052766[4] = 0;
out_1930106235119052766[5] = 0;
out_1930106235119052766[6] = 0;
out_1930106235119052766[7] = 0;
out_1930106235119052766[8] = 0;
out_1930106235119052766[9] = 0;
out_1930106235119052766[10] = 1;
out_1930106235119052766[11] = 0;
out_1930106235119052766[12] = 0;
out_1930106235119052766[13] = 0;
out_1930106235119052766[14] = 0;
out_1930106235119052766[15] = 0;
out_1930106235119052766[16] = 0;
out_1930106235119052766[17] = 0;
out_1930106235119052766[18] = 0;
out_1930106235119052766[19] = 0;
out_1930106235119052766[20] = 1;
out_1930106235119052766[21] = 0;
out_1930106235119052766[22] = 0;
out_1930106235119052766[23] = 0;
out_1930106235119052766[24] = 0;
out_1930106235119052766[25] = 0;
out_1930106235119052766[26] = 0;
out_1930106235119052766[27] = 0;
out_1930106235119052766[28] = 0;
out_1930106235119052766[29] = 0;
out_1930106235119052766[30] = 1;
out_1930106235119052766[31] = 0;
out_1930106235119052766[32] = 0;
out_1930106235119052766[33] = 0;
out_1930106235119052766[34] = 0;
out_1930106235119052766[35] = 0;
out_1930106235119052766[36] = 0;
out_1930106235119052766[37] = 0;
out_1930106235119052766[38] = 0;
out_1930106235119052766[39] = 0;
out_1930106235119052766[40] = 1;
out_1930106235119052766[41] = 0;
out_1930106235119052766[42] = 0;
out_1930106235119052766[43] = 0;
out_1930106235119052766[44] = 0;
out_1930106235119052766[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_1930106235119052766[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_1930106235119052766[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_1930106235119052766[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_1930106235119052766[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_1930106235119052766[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_1930106235119052766[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_1930106235119052766[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_1930106235119052766[53] = -9.8000000000000007*dt;
out_1930106235119052766[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_1930106235119052766[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_1930106235119052766[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1930106235119052766[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1930106235119052766[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_1930106235119052766[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_1930106235119052766[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_1930106235119052766[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_1930106235119052766[62] = 0;
out_1930106235119052766[63] = 0;
out_1930106235119052766[64] = 0;
out_1930106235119052766[65] = 0;
out_1930106235119052766[66] = 0;
out_1930106235119052766[67] = 0;
out_1930106235119052766[68] = 0;
out_1930106235119052766[69] = 0;
out_1930106235119052766[70] = 1;
out_1930106235119052766[71] = 0;
out_1930106235119052766[72] = 0;
out_1930106235119052766[73] = 0;
out_1930106235119052766[74] = 0;
out_1930106235119052766[75] = 0;
out_1930106235119052766[76] = 0;
out_1930106235119052766[77] = 0;
out_1930106235119052766[78] = 0;
out_1930106235119052766[79] = 0;
out_1930106235119052766[80] = 1;
void F_fun(double *state, double dt, double *out_7303340041697938188) {
out_7303340041697938188[0] = 1;
out_7303340041697938188[1] = 0;
out_7303340041697938188[2] = 0;
out_7303340041697938188[3] = 0;
out_7303340041697938188[4] = 0;
out_7303340041697938188[5] = 0;
out_7303340041697938188[6] = 0;
out_7303340041697938188[7] = 0;
out_7303340041697938188[8] = 0;
out_7303340041697938188[9] = 0;
out_7303340041697938188[10] = 1;
out_7303340041697938188[11] = 0;
out_7303340041697938188[12] = 0;
out_7303340041697938188[13] = 0;
out_7303340041697938188[14] = 0;
out_7303340041697938188[15] = 0;
out_7303340041697938188[16] = 0;
out_7303340041697938188[17] = 0;
out_7303340041697938188[18] = 0;
out_7303340041697938188[19] = 0;
out_7303340041697938188[20] = 1;
out_7303340041697938188[21] = 0;
out_7303340041697938188[22] = 0;
out_7303340041697938188[23] = 0;
out_7303340041697938188[24] = 0;
out_7303340041697938188[25] = 0;
out_7303340041697938188[26] = 0;
out_7303340041697938188[27] = 0;
out_7303340041697938188[28] = 0;
out_7303340041697938188[29] = 0;
out_7303340041697938188[30] = 1;
out_7303340041697938188[31] = 0;
out_7303340041697938188[32] = 0;
out_7303340041697938188[33] = 0;
out_7303340041697938188[34] = 0;
out_7303340041697938188[35] = 0;
out_7303340041697938188[36] = 0;
out_7303340041697938188[37] = 0;
out_7303340041697938188[38] = 0;
out_7303340041697938188[39] = 0;
out_7303340041697938188[40] = 1;
out_7303340041697938188[41] = 0;
out_7303340041697938188[42] = 0;
out_7303340041697938188[43] = 0;
out_7303340041697938188[44] = 0;
out_7303340041697938188[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_7303340041697938188[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
out_7303340041697938188[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7303340041697938188[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
out_7303340041697938188[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_7303340041697938188[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
out_7303340041697938188[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
out_7303340041697938188[52] = dt*stiffness_front*state[0]/(mass*state[1]);
out_7303340041697938188[53] = -9.8000000000000007*dt;
out_7303340041697938188[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_7303340041697938188[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
out_7303340041697938188[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7303340041697938188[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7303340041697938188[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_7303340041697938188[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
out_7303340041697938188[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_7303340041697938188[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
out_7303340041697938188[62] = 0;
out_7303340041697938188[63] = 0;
out_7303340041697938188[64] = 0;
out_7303340041697938188[65] = 0;
out_7303340041697938188[66] = 0;
out_7303340041697938188[67] = 0;
out_7303340041697938188[68] = 0;
out_7303340041697938188[69] = 0;
out_7303340041697938188[70] = 1;
out_7303340041697938188[71] = 0;
out_7303340041697938188[72] = 0;
out_7303340041697938188[73] = 0;
out_7303340041697938188[74] = 0;
out_7303340041697938188[75] = 0;
out_7303340041697938188[76] = 0;
out_7303340041697938188[77] = 0;
out_7303340041697938188[78] = 0;
out_7303340041697938188[79] = 0;
out_7303340041697938188[80] = 1;
}
void h_25(double *state, double *unused, double *out_4962192466318968957) {
out_4962192466318968957[0] = state[6];
void h_25(double *state, double *unused, double *out_5588110016210508170) {
out_5588110016210508170[0] = state[6];
}
void H_25(double *state, double *unused, double *out_279667768067758393) {
out_279667768067758393[0] = 0;
out_279667768067758393[1] = 0;
out_279667768067758393[2] = 0;
out_279667768067758393[3] = 0;
out_279667768067758393[4] = 0;
out_279667768067758393[5] = 0;
out_279667768067758393[6] = 1;
out_279667768067758393[7] = 0;
out_279667768067758393[8] = 0;
void H_25(double *state, double *unused, double *out_6037113083971364302) {
out_6037113083971364302[0] = 0;
out_6037113083971364302[1] = 0;
out_6037113083971364302[2] = 0;
out_6037113083971364302[3] = 0;
out_6037113083971364302[4] = 0;
out_6037113083971364302[5] = 0;
out_6037113083971364302[6] = 1;
out_6037113083971364302[7] = 0;
out_6037113083971364302[8] = 0;
}
void h_24(double *state, double *unused, double *out_8672504443832107776) {
out_8672504443832107776[0] = state[4];
out_8672504443832107776[1] = state[5];
void h_24(double *state, double *unused, double *out_4380624216713203670) {
out_4380624216713203670[0] = state[4];
out_4380624216713203670[1] = state[5];
}
void H_24(double *state, double *unused, double *out_1897546655539391580) {
out_1897546655539391580[0] = 0;
out_1897546655539391580[1] = 0;
out_1897546655539391580[2] = 0;
out_1897546655539391580[3] = 0;
out_1897546655539391580[4] = 1;
out_1897546655539391580[5] = 0;
out_1897546655539391580[6] = 0;
out_1897546655539391580[7] = 0;
out_1897546655539391580[8] = 0;
out_1897546655539391580[9] = 0;
out_1897546655539391580[10] = 0;
out_1897546655539391580[11] = 0;
out_1897546655539391580[12] = 0;
out_1897546655539391580[13] = 0;
out_1897546655539391580[14] = 1;
out_1897546655539391580[15] = 0;
out_1897546655539391580[16] = 0;
out_1897546655539391580[17] = 0;
void H_24(double *state, double *unused, double *out_8209762682976863868) {
out_8209762682976863868[0] = 0;
out_8209762682976863868[1] = 0;
out_8209762682976863868[2] = 0;
out_8209762682976863868[3] = 0;
out_8209762682976863868[4] = 1;
out_8209762682976863868[5] = 0;
out_8209762682976863868[6] = 0;
out_8209762682976863868[7] = 0;
out_8209762682976863868[8] = 0;
out_8209762682976863868[9] = 0;
out_8209762682976863868[10] = 0;
out_8209762682976863868[11] = 0;
out_8209762682976863868[12] = 0;
out_8209762682976863868[13] = 0;
out_8209762682976863868[14] = 1;
out_8209762682976863868[15] = 0;
out_8209762682976863868[16] = 0;
out_8209762682976863868[17] = 0;
}
void h_30(double *state, double *unused, double *out_4360774308705605403) {
out_4360774308705605403[0] = state[4];
void h_30(double *state, double *unused, double *out_4965694562614520087) {
out_4965694562614520087[0] = state[4];
}
void H_30(double *state, double *unused, double *out_2238665190439490234) {
out_2238665190439490234[0] = 0;
out_2238665190439490234[1] = 0;
out_2238665190439490234[2] = 0;
out_2238665190439490234[3] = 0;
out_2238665190439490234[4] = 1;
out_2238665190439490234[5] = 0;
out_2238665190439490234[6] = 0;
out_2238665190439490234[7] = 0;
out_2238665190439490234[8] = 0;
void H_30(double *state, double *unused, double *out_879577257520252453) {
out_879577257520252453[0] = 0;
out_879577257520252453[1] = 0;
out_879577257520252453[2] = 0;
out_879577257520252453[3] = 0;
out_879577257520252453[4] = 1;
out_879577257520252453[5] = 0;
out_879577257520252453[6] = 0;
out_879577257520252453[7] = 0;
out_879577257520252453[8] = 0;
}
void h_26(double *state, double *unused, double *out_640898958955240242) {
out_640898958955240242[0] = state[7];
void h_26(double *state, double *unused, double *out_4499080339088190764) {
out_4499080339088190764[0] = state[7];
}
void H_26(double *state, double *unused, double *out_4021171086941814617) {
out_4021171086941814617[0] = 0;
out_4021171086941814617[1] = 0;
out_4021171086941814617[2] = 0;
out_4021171086941814617[3] = 0;
out_4021171086941814617[4] = 0;
out_4021171086941814617[5] = 0;
out_4021171086941814617[6] = 0;
out_4021171086941814617[7] = 1;
out_4021171086941814617[8] = 0;
void H_26(double *state, double *unused, double *out_8668127670864131090) {
out_8668127670864131090[0] = 0;
out_8668127670864131090[1] = 0;
out_8668127670864131090[2] = 0;
out_8668127670864131090[3] = 0;
out_8668127670864131090[4] = 0;
out_8668127670864131090[5] = 0;
out_8668127670864131090[6] = 0;
out_8668127670864131090[7] = 1;
out_8668127670864131090[8] = 0;
}
void h_27(double *state, double *unused, double *out_3142070393519437079) {
out_3142070393519437079[0] = state[3];
void h_27(double *state, double *unused, double *out_586436748549662525) {
out_586436748549662525[0] = state[3];
}
void H_27(double *state, double *unused, double *out_4462259261623433451) {
out_4462259261623433451[0] = 0;
out_4462259261623433451[1] = 0;
out_4462259261623433451[2] = 0;
out_4462259261623433451[3] = 1;
out_4462259261623433451[4] = 0;
out_4462259261623433451[5] = 0;
out_4462259261623433451[6] = 0;
out_4462259261623433451[7] = 0;
out_4462259261623433451[8] = 0;
void H_27(double *state, double *unused, double *out_1295186054280172458) {
out_1295186054280172458[0] = 0;
out_1295186054280172458[1] = 0;
out_1295186054280172458[2] = 0;
out_1295186054280172458[3] = 1;
out_1295186054280172458[4] = 0;
out_1295186054280172458[5] = 0;
out_1295186054280172458[6] = 0;
out_1295186054280172458[7] = 0;
out_1295186054280172458[8] = 0;
}
void h_29(double *state, double *unused, double *out_4621194863301235362) {
out_4621194863301235362[0] = state[1];
void h_29(double *state, double *unused, double *out_5569010447290997604) {
out_5569010447290997604[0] = state[1];
}
void H_29(double *state, double *unused, double *out_2748896534753882418) {
out_2748896534753882418[0] = 0;
out_2748896534753882418[1] = 1;
out_2748896534753882418[2] = 0;
out_2748896534753882418[3] = 0;
out_2748896534753882418[4] = 0;
out_2748896534753882418[5] = 0;
out_2748896534753882418[6] = 0;
out_2748896534753882418[7] = 0;
out_2748896534753882418[8] = 0;
void H_29(double *state, double *unused, double *out_3008548781149723491) {
out_3008548781149723491[0] = 0;
out_3008548781149723491[1] = 1;
out_3008548781149723491[2] = 0;
out_3008548781149723491[3] = 0;
out_3008548781149723491[4] = 0;
out_3008548781149723491[5] = 0;
out_3008548781149723491[6] = 0;
out_3008548781149723491[7] = 0;
out_3008548781149723491[8] = 0;
}
void h_28(double *state, double *unused, double *out_7655328506878780195) {
out_7655328506878780195[0] = state[0];
void h_28(double *state, double *unused, double *out_2689411064197794064) {
out_2689411064197794064[0] = state[0];
}
void H_28(double *state, double *unused, double *out_2333502482315648156) {
out_2333502482315648156[0] = 1;
out_2333502482315648156[1] = 0;
out_2333502482315648156[2] = 0;
out_2333502482315648156[3] = 0;
out_2333502482315648156[4] = 0;
out_2333502482315648156[5] = 0;
out_2333502482315648156[6] = 0;
out_2333502482315648156[7] = 0;
out_2333502482315648156[8] = 0;
void H_28(double *state, double *unused, double *out_8090947798219254065) {
out_8090947798219254065[0] = 1;
out_8090947798219254065[1] = 0;
out_8090947798219254065[2] = 0;
out_8090947798219254065[3] = 0;
out_8090947798219254065[4] = 0;
out_8090947798219254065[5] = 0;
out_8090947798219254065[6] = 0;
out_8090947798219254065[7] = 0;
out_8090947798219254065[8] = 0;
}
void h_31(double *state, double *unused, double *out_5721286350569910218) {
out_5721286350569910218[0] = state[8];
void h_31(double *state, double *unused, double *out_5863304078495014059) {
out_5863304078495014059[0] = state[8];
}
void H_31(double *state, double *unused, double *out_249021806190797965) {
out_249021806190797965[0] = 0;
out_249021806190797965[1] = 0;
out_249021806190797965[2] = 0;
out_249021806190797965[3] = 0;
out_249021806190797965[4] = 0;
out_249021806190797965[5] = 0;
out_249021806190797965[6] = 0;
out_249021806190797965[7] = 0;
out_249021806190797965[8] = 1;
void H_31(double *state, double *unused, double *out_6006467122094403874) {
out_6006467122094403874[0] = 0;
out_6006467122094403874[1] = 0;
out_6006467122094403874[2] = 0;
out_6006467122094403874[3] = 0;
out_6006467122094403874[4] = 0;
out_6006467122094403874[5] = 0;
out_6006467122094403874[6] = 0;
out_6006467122094403874[7] = 0;
out_6006467122094403874[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_8412408275907562510) {
err_fun(nom_x, delta_x, out_8412408275907562510);
void car_err_fun(double *nom_x, double *delta_x, double *out_1282327502940326821) {
err_fun(nom_x, delta_x, out_1282327502940326821);
}
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5009429012443487067) {
inv_err_fun(nom_x, true_x, out_5009429012443487067);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4590721196226970281) {
inv_err_fun(nom_x, true_x, out_4590721196226970281);
}
void car_H_mod_fun(double *state, double *out_7201068384612818103) {
H_mod_fun(state, out_7201068384612818103);
void car_H_mod_fun(double *state, double *out_2163953626510216661) {
H_mod_fun(state, out_2163953626510216661);
}
void car_f_fun(double *state, double dt, double *out_2117395969407442559) {
f_fun(state, dt, out_2117395969407442559);
void car_f_fun(double *state, double dt, double *out_405606070335708994) {
f_fun(state, dt, out_405606070335708994);
}
void car_F_fun(double *state, double dt, double *out_1930106235119052766) {
F_fun(state, dt, out_1930106235119052766);
void car_F_fun(double *state, double dt, double *out_7303340041697938188) {
F_fun(state, dt, out_7303340041697938188);
}
void car_h_25(double *state, double *unused, double *out_4962192466318968957) {
h_25(state, unused, out_4962192466318968957);
void car_h_25(double *state, double *unused, double *out_5588110016210508170) {
h_25(state, unused, out_5588110016210508170);
}
void car_H_25(double *state, double *unused, double *out_279667768067758393) {
H_25(state, unused, out_279667768067758393);
void car_H_25(double *state, double *unused, double *out_6037113083971364302) {
H_25(state, unused, out_6037113083971364302);
}
void car_h_24(double *state, double *unused, double *out_8672504443832107776) {
h_24(state, unused, out_8672504443832107776);
void car_h_24(double *state, double *unused, double *out_4380624216713203670) {
h_24(state, unused, out_4380624216713203670);
}
void car_H_24(double *state, double *unused, double *out_1897546655539391580) {
H_24(state, unused, out_1897546655539391580);
void car_H_24(double *state, double *unused, double *out_8209762682976863868) {
H_24(state, unused, out_8209762682976863868);
}
void car_h_30(double *state, double *unused, double *out_4360774308705605403) {
h_30(state, unused, out_4360774308705605403);
void car_h_30(double *state, double *unused, double *out_4965694562614520087) {
h_30(state, unused, out_4965694562614520087);
}
void car_H_30(double *state, double *unused, double *out_2238665190439490234) {
H_30(state, unused, out_2238665190439490234);
void car_H_30(double *state, double *unused, double *out_879577257520252453) {
H_30(state, unused, out_879577257520252453);
}
void car_h_26(double *state, double *unused, double *out_640898958955240242) {
h_26(state, unused, out_640898958955240242);
void car_h_26(double *state, double *unused, double *out_4499080339088190764) {
h_26(state, unused, out_4499080339088190764);
}
void car_H_26(double *state, double *unused, double *out_4021171086941814617) {
H_26(state, unused, out_4021171086941814617);
void car_H_26(double *state, double *unused, double *out_8668127670864131090) {
H_26(state, unused, out_8668127670864131090);
}
void car_h_27(double *state, double *unused, double *out_3142070393519437079) {
h_27(state, unused, out_3142070393519437079);
void car_h_27(double *state, double *unused, double *out_586436748549662525) {
h_27(state, unused, out_586436748549662525);
}
void car_H_27(double *state, double *unused, double *out_4462259261623433451) {
H_27(state, unused, out_4462259261623433451);
void car_H_27(double *state, double *unused, double *out_1295186054280172458) {
H_27(state, unused, out_1295186054280172458);
}
void car_h_29(double *state, double *unused, double *out_4621194863301235362) {
h_29(state, unused, out_4621194863301235362);
void car_h_29(double *state, double *unused, double *out_5569010447290997604) {
h_29(state, unused, out_5569010447290997604);
}
void car_H_29(double *state, double *unused, double *out_2748896534753882418) {
H_29(state, unused, out_2748896534753882418);
void car_H_29(double *state, double *unused, double *out_3008548781149723491) {
H_29(state, unused, out_3008548781149723491);
}
void car_h_28(double *state, double *unused, double *out_7655328506878780195) {
h_28(state, unused, out_7655328506878780195);
void car_h_28(double *state, double *unused, double *out_2689411064197794064) {
h_28(state, unused, out_2689411064197794064);
}
void car_H_28(double *state, double *unused, double *out_2333502482315648156) {
H_28(state, unused, out_2333502482315648156);
void car_H_28(double *state, double *unused, double *out_8090947798219254065) {
H_28(state, unused, out_8090947798219254065);
}
void car_h_31(double *state, double *unused, double *out_5721286350569910218) {
h_31(state, unused, out_5721286350569910218);
void car_h_31(double *state, double *unused, double *out_5863304078495014059) {
h_31(state, unused, out_5863304078495014059);
}
void car_H_31(double *state, double *unused, double *out_249021806190797965) {
H_31(state, unused, out_249021806190797965);
void car_H_31(double *state, double *unused, double *out_6006467122094403874) {
H_31(state, unused, out_6006467122094403874);
}
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_8412408275907562510);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5009429012443487067);
void car_H_mod_fun(double *state, double *out_7201068384612818103);
void car_f_fun(double *state, double dt, double *out_2117395969407442559);
void car_F_fun(double *state, double dt, double *out_1930106235119052766);
void car_h_25(double *state, double *unused, double *out_4962192466318968957);
void car_H_25(double *state, double *unused, double *out_279667768067758393);
void car_h_24(double *state, double *unused, double *out_8672504443832107776);
void car_H_24(double *state, double *unused, double *out_1897546655539391580);
void car_h_30(double *state, double *unused, double *out_4360774308705605403);
void car_H_30(double *state, double *unused, double *out_2238665190439490234);
void car_h_26(double *state, double *unused, double *out_640898958955240242);
void car_H_26(double *state, double *unused, double *out_4021171086941814617);
void car_h_27(double *state, double *unused, double *out_3142070393519437079);
void car_H_27(double *state, double *unused, double *out_4462259261623433451);
void car_h_29(double *state, double *unused, double *out_4621194863301235362);
void car_H_29(double *state, double *unused, double *out_2748896534753882418);
void car_h_28(double *state, double *unused, double *out_7655328506878780195);
void car_H_28(double *state, double *unused, double *out_2333502482315648156);
void car_h_31(double *state, double *unused, double *out_5721286350569910218);
void car_H_31(double *state, double *unused, double *out_249021806190797965);
void car_err_fun(double *nom_x, double *delta_x, double *out_1282327502940326821);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_4590721196226970281);
void car_H_mod_fun(double *state, double *out_2163953626510216661);
void car_f_fun(double *state, double dt, double *out_405606070335708994);
void car_F_fun(double *state, double dt, double *out_7303340041697938188);
void car_h_25(double *state, double *unused, double *out_5588110016210508170);
void car_H_25(double *state, double *unused, double *out_6037113083971364302);
void car_h_24(double *state, double *unused, double *out_4380624216713203670);
void car_H_24(double *state, double *unused, double *out_8209762682976863868);
void car_h_30(double *state, double *unused, double *out_4965694562614520087);
void car_H_30(double *state, double *unused, double *out_879577257520252453);
void car_h_26(double *state, double *unused, double *out_4499080339088190764);
void car_H_26(double *state, double *unused, double *out_8668127670864131090);
void car_h_27(double *state, double *unused, double *out_586436748549662525);
void car_H_27(double *state, double *unused, double *out_1295186054280172458);
void car_h_29(double *state, double *unused, double *out_5569010447290997604);
void car_H_29(double *state, double *unused, double *out_3008548781149723491);
void car_h_28(double *state, double *unused, double *out_2689411064197794064);
void car_H_28(double *state, double *unused, double *out_8090947798219254065);
void car_h_31(double *state, double *unused, double *out_5863304078495014059);
void car_H_31(double *state, double *unused, double *out_6006467122094403874);
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_2067482950567323166) {
out_2067482950567323166[0] = delta_x[0] + nom_x[0];
out_2067482950567323166[1] = delta_x[1] + nom_x[1];
out_2067482950567323166[2] = delta_x[2] + nom_x[2];
out_2067482950567323166[3] = delta_x[3] + nom_x[3];
out_2067482950567323166[4] = delta_x[4] + nom_x[4];
out_2067482950567323166[5] = delta_x[5] + nom_x[5];
out_2067482950567323166[6] = delta_x[6] + nom_x[6];
out_2067482950567323166[7] = delta_x[7] + nom_x[7];
out_2067482950567323166[8] = delta_x[8] + nom_x[8];
out_2067482950567323166[9] = delta_x[9] + nom_x[9];
out_2067482950567323166[10] = delta_x[10] + nom_x[10];
void err_fun(double *nom_x, double *delta_x, double *out_5270397344340919402) {
out_5270397344340919402[0] = delta_x[0] + nom_x[0];
out_5270397344340919402[1] = delta_x[1] + nom_x[1];
out_5270397344340919402[2] = delta_x[2] + nom_x[2];
out_5270397344340919402[3] = delta_x[3] + nom_x[3];
out_5270397344340919402[4] = delta_x[4] + nom_x[4];
out_5270397344340919402[5] = delta_x[5] + nom_x[5];
out_5270397344340919402[6] = delta_x[6] + nom_x[6];
out_5270397344340919402[7] = delta_x[7] + nom_x[7];
out_5270397344340919402[8] = delta_x[8] + nom_x[8];
out_5270397344340919402[9] = delta_x[9] + nom_x[9];
out_5270397344340919402[10] = delta_x[10] + nom_x[10];
}
void inv_err_fun(double *nom_x, double *true_x, double *out_2603530649313476471) {
out_2603530649313476471[0] = -nom_x[0] + true_x[0];
out_2603530649313476471[1] = -nom_x[1] + true_x[1];
out_2603530649313476471[2] = -nom_x[2] + true_x[2];
out_2603530649313476471[3] = -nom_x[3] + true_x[3];
out_2603530649313476471[4] = -nom_x[4] + true_x[4];
out_2603530649313476471[5] = -nom_x[5] + true_x[5];
out_2603530649313476471[6] = -nom_x[6] + true_x[6];
out_2603530649313476471[7] = -nom_x[7] + true_x[7];
out_2603530649313476471[8] = -nom_x[8] + true_x[8];
out_2603530649313476471[9] = -nom_x[9] + true_x[9];
out_2603530649313476471[10] = -nom_x[10] + true_x[10];
void inv_err_fun(double *nom_x, double *true_x, double *out_1512259161409062554) {
out_1512259161409062554[0] = -nom_x[0] + true_x[0];
out_1512259161409062554[1] = -nom_x[1] + true_x[1];
out_1512259161409062554[2] = -nom_x[2] + true_x[2];
out_1512259161409062554[3] = -nom_x[3] + true_x[3];
out_1512259161409062554[4] = -nom_x[4] + true_x[4];
out_1512259161409062554[5] = -nom_x[5] + true_x[5];
out_1512259161409062554[6] = -nom_x[6] + true_x[6];
out_1512259161409062554[7] = -nom_x[7] + true_x[7];
out_1512259161409062554[8] = -nom_x[8] + true_x[8];
out_1512259161409062554[9] = -nom_x[9] + true_x[9];
out_1512259161409062554[10] = -nom_x[10] + true_x[10];
}
void H_mod_fun(double *state, double *out_5820927568950649229) {
out_5820927568950649229[0] = 1.0;
out_5820927568950649229[1] = 0;
out_5820927568950649229[2] = 0;
out_5820927568950649229[3] = 0;
out_5820927568950649229[4] = 0;
out_5820927568950649229[5] = 0;
out_5820927568950649229[6] = 0;
out_5820927568950649229[7] = 0;
out_5820927568950649229[8] = 0;
out_5820927568950649229[9] = 0;
out_5820927568950649229[10] = 0;
out_5820927568950649229[11] = 0;
out_5820927568950649229[12] = 1.0;
out_5820927568950649229[13] = 0;
out_5820927568950649229[14] = 0;
out_5820927568950649229[15] = 0;
out_5820927568950649229[16] = 0;
out_5820927568950649229[17] = 0;
out_5820927568950649229[18] = 0;
out_5820927568950649229[19] = 0;
out_5820927568950649229[20] = 0;
out_5820927568950649229[21] = 0;
out_5820927568950649229[22] = 0;
out_5820927568950649229[23] = 0;
out_5820927568950649229[24] = 1.0;
out_5820927568950649229[25] = 0;
out_5820927568950649229[26] = 0;
out_5820927568950649229[27] = 0;
out_5820927568950649229[28] = 0;
out_5820927568950649229[29] = 0;
out_5820927568950649229[30] = 0;
out_5820927568950649229[31] = 0;
out_5820927568950649229[32] = 0;
out_5820927568950649229[33] = 0;
out_5820927568950649229[34] = 0;
out_5820927568950649229[35] = 0;
out_5820927568950649229[36] = 1.0;
out_5820927568950649229[37] = 0;
out_5820927568950649229[38] = 0;
out_5820927568950649229[39] = 0;
out_5820927568950649229[40] = 0;
out_5820927568950649229[41] = 0;
out_5820927568950649229[42] = 0;
out_5820927568950649229[43] = 0;
out_5820927568950649229[44] = 0;
out_5820927568950649229[45] = 0;
out_5820927568950649229[46] = 0;
out_5820927568950649229[47] = 0;
out_5820927568950649229[48] = 1.0;
out_5820927568950649229[49] = 0;
out_5820927568950649229[50] = 0;
out_5820927568950649229[51] = 0;
out_5820927568950649229[52] = 0;
out_5820927568950649229[53] = 0;
out_5820927568950649229[54] = 0;
out_5820927568950649229[55] = 0;
out_5820927568950649229[56] = 0;
out_5820927568950649229[57] = 0;
out_5820927568950649229[58] = 0;
out_5820927568950649229[59] = 0;
out_5820927568950649229[60] = 1.0;
out_5820927568950649229[61] = 0;
out_5820927568950649229[62] = 0;
out_5820927568950649229[63] = 0;
out_5820927568950649229[64] = 0;
out_5820927568950649229[65] = 0;
out_5820927568950649229[66] = 0;
out_5820927568950649229[67] = 0;
out_5820927568950649229[68] = 0;
out_5820927568950649229[69] = 0;
out_5820927568950649229[70] = 0;
out_5820927568950649229[71] = 0;
out_5820927568950649229[72] = 1.0;
out_5820927568950649229[73] = 0;
out_5820927568950649229[74] = 0;
out_5820927568950649229[75] = 0;
out_5820927568950649229[76] = 0;
out_5820927568950649229[77] = 0;
out_5820927568950649229[78] = 0;
out_5820927568950649229[79] = 0;
out_5820927568950649229[80] = 0;
out_5820927568950649229[81] = 0;
out_5820927568950649229[82] = 0;
out_5820927568950649229[83] = 0;
out_5820927568950649229[84] = 1.0;
out_5820927568950649229[85] = 0;
out_5820927568950649229[86] = 0;
out_5820927568950649229[87] = 0;
out_5820927568950649229[88] = 0;
out_5820927568950649229[89] = 0;
out_5820927568950649229[90] = 0;
out_5820927568950649229[91] = 0;
out_5820927568950649229[92] = 0;
out_5820927568950649229[93] = 0;
out_5820927568950649229[94] = 0;
out_5820927568950649229[95] = 0;
out_5820927568950649229[96] = 1.0;
out_5820927568950649229[97] = 0;
out_5820927568950649229[98] = 0;
out_5820927568950649229[99] = 0;
out_5820927568950649229[100] = 0;
out_5820927568950649229[101] = 0;
out_5820927568950649229[102] = 0;
out_5820927568950649229[103] = 0;
out_5820927568950649229[104] = 0;
out_5820927568950649229[105] = 0;
out_5820927568950649229[106] = 0;
out_5820927568950649229[107] = 0;
out_5820927568950649229[108] = 1.0;
out_5820927568950649229[109] = 0;
out_5820927568950649229[110] = 0;
out_5820927568950649229[111] = 0;
out_5820927568950649229[112] = 0;
out_5820927568950649229[113] = 0;
out_5820927568950649229[114] = 0;
out_5820927568950649229[115] = 0;
out_5820927568950649229[116] = 0;
out_5820927568950649229[117] = 0;
out_5820927568950649229[118] = 0;
out_5820927568950649229[119] = 0;
out_5820927568950649229[120] = 1.0;
void H_mod_fun(double *state, double *out_8413937430488034064) {
out_8413937430488034064[0] = 1.0;
out_8413937430488034064[1] = 0;
out_8413937430488034064[2] = 0;
out_8413937430488034064[3] = 0;
out_8413937430488034064[4] = 0;
out_8413937430488034064[5] = 0;
out_8413937430488034064[6] = 0;
out_8413937430488034064[7] = 0;
out_8413937430488034064[8] = 0;
out_8413937430488034064[9] = 0;
out_8413937430488034064[10] = 0;
out_8413937430488034064[11] = 0;
out_8413937430488034064[12] = 1.0;
out_8413937430488034064[13] = 0;
out_8413937430488034064[14] = 0;
out_8413937430488034064[15] = 0;
out_8413937430488034064[16] = 0;
out_8413937430488034064[17] = 0;
out_8413937430488034064[18] = 0;
out_8413937430488034064[19] = 0;
out_8413937430488034064[20] = 0;
out_8413937430488034064[21] = 0;
out_8413937430488034064[22] = 0;
out_8413937430488034064[23] = 0;
out_8413937430488034064[24] = 1.0;
out_8413937430488034064[25] = 0;
out_8413937430488034064[26] = 0;
out_8413937430488034064[27] = 0;
out_8413937430488034064[28] = 0;
out_8413937430488034064[29] = 0;
out_8413937430488034064[30] = 0;
out_8413937430488034064[31] = 0;
out_8413937430488034064[32] = 0;
out_8413937430488034064[33] = 0;
out_8413937430488034064[34] = 0;
out_8413937430488034064[35] = 0;
out_8413937430488034064[36] = 1.0;
out_8413937430488034064[37] = 0;
out_8413937430488034064[38] = 0;
out_8413937430488034064[39] = 0;
out_8413937430488034064[40] = 0;
out_8413937430488034064[41] = 0;
out_8413937430488034064[42] = 0;
out_8413937430488034064[43] = 0;
out_8413937430488034064[44] = 0;
out_8413937430488034064[45] = 0;
out_8413937430488034064[46] = 0;
out_8413937430488034064[47] = 0;
out_8413937430488034064[48] = 1.0;
out_8413937430488034064[49] = 0;
out_8413937430488034064[50] = 0;
out_8413937430488034064[51] = 0;
out_8413937430488034064[52] = 0;
out_8413937430488034064[53] = 0;
out_8413937430488034064[54] = 0;
out_8413937430488034064[55] = 0;
out_8413937430488034064[56] = 0;
out_8413937430488034064[57] = 0;
out_8413937430488034064[58] = 0;
out_8413937430488034064[59] = 0;
out_8413937430488034064[60] = 1.0;
out_8413937430488034064[61] = 0;
out_8413937430488034064[62] = 0;
out_8413937430488034064[63] = 0;
out_8413937430488034064[64] = 0;
out_8413937430488034064[65] = 0;
out_8413937430488034064[66] = 0;
out_8413937430488034064[67] = 0;
out_8413937430488034064[68] = 0;
out_8413937430488034064[69] = 0;
out_8413937430488034064[70] = 0;
out_8413937430488034064[71] = 0;
out_8413937430488034064[72] = 1.0;
out_8413937430488034064[73] = 0;
out_8413937430488034064[74] = 0;
out_8413937430488034064[75] = 0;
out_8413937430488034064[76] = 0;
out_8413937430488034064[77] = 0;
out_8413937430488034064[78] = 0;
out_8413937430488034064[79] = 0;
out_8413937430488034064[80] = 0;
out_8413937430488034064[81] = 0;
out_8413937430488034064[82] = 0;
out_8413937430488034064[83] = 0;
out_8413937430488034064[84] = 1.0;
out_8413937430488034064[85] = 0;
out_8413937430488034064[86] = 0;
out_8413937430488034064[87] = 0;
out_8413937430488034064[88] = 0;
out_8413937430488034064[89] = 0;
out_8413937430488034064[90] = 0;
out_8413937430488034064[91] = 0;
out_8413937430488034064[92] = 0;
out_8413937430488034064[93] = 0;
out_8413937430488034064[94] = 0;
out_8413937430488034064[95] = 0;
out_8413937430488034064[96] = 1.0;
out_8413937430488034064[97] = 0;
out_8413937430488034064[98] = 0;
out_8413937430488034064[99] = 0;
out_8413937430488034064[100] = 0;
out_8413937430488034064[101] = 0;
out_8413937430488034064[102] = 0;
out_8413937430488034064[103] = 0;
out_8413937430488034064[104] = 0;
out_8413937430488034064[105] = 0;
out_8413937430488034064[106] = 0;
out_8413937430488034064[107] = 0;
out_8413937430488034064[108] = 1.0;
out_8413937430488034064[109] = 0;
out_8413937430488034064[110] = 0;
out_8413937430488034064[111] = 0;
out_8413937430488034064[112] = 0;
out_8413937430488034064[113] = 0;
out_8413937430488034064[114] = 0;
out_8413937430488034064[115] = 0;
out_8413937430488034064[116] = 0;
out_8413937430488034064[117] = 0;
out_8413937430488034064[118] = 0;
out_8413937430488034064[119] = 0;
out_8413937430488034064[120] = 1.0;
}
void f_fun(double *state, double dt, double *out_17551380326686977) {
out_17551380326686977[0] = dt*state[3] + state[0];
out_17551380326686977[1] = dt*state[4] + state[1];
out_17551380326686977[2] = dt*state[5] + state[2];
out_17551380326686977[3] = state[3];
out_17551380326686977[4] = state[4];
out_17551380326686977[5] = state[5];
out_17551380326686977[6] = dt*state[7] + state[6];
out_17551380326686977[7] = dt*state[8] + state[7];
out_17551380326686977[8] = state[8];
out_17551380326686977[9] = state[9];
out_17551380326686977[10] = state[10];
void f_fun(double *state, double dt, double *out_8044790491496694058) {
out_8044790491496694058[0] = dt*state[3] + state[0];
out_8044790491496694058[1] = dt*state[4] + state[1];
out_8044790491496694058[2] = dt*state[5] + state[2];
out_8044790491496694058[3] = state[3];
out_8044790491496694058[4] = state[4];
out_8044790491496694058[5] = state[5];
out_8044790491496694058[6] = dt*state[7] + state[6];
out_8044790491496694058[7] = dt*state[8] + state[7];
out_8044790491496694058[8] = state[8];
out_8044790491496694058[9] = state[9];
out_8044790491496694058[10] = state[10];
}
void F_fun(double *state, double dt, double *out_414838326010165834) {
out_414838326010165834[0] = 1;
out_414838326010165834[1] = 0;
out_414838326010165834[2] = 0;
out_414838326010165834[3] = dt;
out_414838326010165834[4] = 0;
out_414838326010165834[5] = 0;
out_414838326010165834[6] = 0;
out_414838326010165834[7] = 0;
out_414838326010165834[8] = 0;
out_414838326010165834[9] = 0;
out_414838326010165834[10] = 0;
out_414838326010165834[11] = 0;
out_414838326010165834[12] = 1;
out_414838326010165834[13] = 0;
out_414838326010165834[14] = 0;
out_414838326010165834[15] = dt;
out_414838326010165834[16] = 0;
out_414838326010165834[17] = 0;
out_414838326010165834[18] = 0;
out_414838326010165834[19] = 0;
out_414838326010165834[20] = 0;
out_414838326010165834[21] = 0;
out_414838326010165834[22] = 0;
out_414838326010165834[23] = 0;
out_414838326010165834[24] = 1;
out_414838326010165834[25] = 0;
out_414838326010165834[26] = 0;
out_414838326010165834[27] = dt;
out_414838326010165834[28] = 0;
out_414838326010165834[29] = 0;
out_414838326010165834[30] = 0;
out_414838326010165834[31] = 0;
out_414838326010165834[32] = 0;
out_414838326010165834[33] = 0;
out_414838326010165834[34] = 0;
out_414838326010165834[35] = 0;
out_414838326010165834[36] = 1;
out_414838326010165834[37] = 0;
out_414838326010165834[38] = 0;
out_414838326010165834[39] = 0;
out_414838326010165834[40] = 0;
out_414838326010165834[41] = 0;
out_414838326010165834[42] = 0;
out_414838326010165834[43] = 0;
out_414838326010165834[44] = 0;
out_414838326010165834[45] = 0;
out_414838326010165834[46] = 0;
out_414838326010165834[47] = 0;
out_414838326010165834[48] = 1;
out_414838326010165834[49] = 0;
out_414838326010165834[50] = 0;
out_414838326010165834[51] = 0;
out_414838326010165834[52] = 0;
out_414838326010165834[53] = 0;
out_414838326010165834[54] = 0;
out_414838326010165834[55] = 0;
out_414838326010165834[56] = 0;
out_414838326010165834[57] = 0;
out_414838326010165834[58] = 0;
out_414838326010165834[59] = 0;
out_414838326010165834[60] = 1;
out_414838326010165834[61] = 0;
out_414838326010165834[62] = 0;
out_414838326010165834[63] = 0;
out_414838326010165834[64] = 0;
out_414838326010165834[65] = 0;
out_414838326010165834[66] = 0;
out_414838326010165834[67] = 0;
out_414838326010165834[68] = 0;
out_414838326010165834[69] = 0;
out_414838326010165834[70] = 0;
out_414838326010165834[71] = 0;
out_414838326010165834[72] = 1;
out_414838326010165834[73] = dt;
out_414838326010165834[74] = 0;
out_414838326010165834[75] = 0;
out_414838326010165834[76] = 0;
out_414838326010165834[77] = 0;
out_414838326010165834[78] = 0;
out_414838326010165834[79] = 0;
out_414838326010165834[80] = 0;
out_414838326010165834[81] = 0;
out_414838326010165834[82] = 0;
out_414838326010165834[83] = 0;
out_414838326010165834[84] = 1;
out_414838326010165834[85] = dt;
out_414838326010165834[86] = 0;
out_414838326010165834[87] = 0;
out_414838326010165834[88] = 0;
out_414838326010165834[89] = 0;
out_414838326010165834[90] = 0;
out_414838326010165834[91] = 0;
out_414838326010165834[92] = 0;
out_414838326010165834[93] = 0;
out_414838326010165834[94] = 0;
out_414838326010165834[95] = 0;
out_414838326010165834[96] = 1;
out_414838326010165834[97] = 0;
out_414838326010165834[98] = 0;
out_414838326010165834[99] = 0;
out_414838326010165834[100] = 0;
out_414838326010165834[101] = 0;
out_414838326010165834[102] = 0;
out_414838326010165834[103] = 0;
out_414838326010165834[104] = 0;
out_414838326010165834[105] = 0;
out_414838326010165834[106] = 0;
out_414838326010165834[107] = 0;
out_414838326010165834[108] = 1;
out_414838326010165834[109] = 0;
out_414838326010165834[110] = 0;
out_414838326010165834[111] = 0;
out_414838326010165834[112] = 0;
out_414838326010165834[113] = 0;
out_414838326010165834[114] = 0;
out_414838326010165834[115] = 0;
out_414838326010165834[116] = 0;
out_414838326010165834[117] = 0;
out_414838326010165834[118] = 0;
out_414838326010165834[119] = 0;
out_414838326010165834[120] = 1;
void F_fun(double *state, double dt, double *out_7107643139068643538) {
out_7107643139068643538[0] = 1;
out_7107643139068643538[1] = 0;
out_7107643139068643538[2] = 0;
out_7107643139068643538[3] = dt;
out_7107643139068643538[4] = 0;
out_7107643139068643538[5] = 0;
out_7107643139068643538[6] = 0;
out_7107643139068643538[7] = 0;
out_7107643139068643538[8] = 0;
out_7107643139068643538[9] = 0;
out_7107643139068643538[10] = 0;
out_7107643139068643538[11] = 0;
out_7107643139068643538[12] = 1;
out_7107643139068643538[13] = 0;
out_7107643139068643538[14] = 0;
out_7107643139068643538[15] = dt;
out_7107643139068643538[16] = 0;
out_7107643139068643538[17] = 0;
out_7107643139068643538[18] = 0;
out_7107643139068643538[19] = 0;
out_7107643139068643538[20] = 0;
out_7107643139068643538[21] = 0;
out_7107643139068643538[22] = 0;
out_7107643139068643538[23] = 0;
out_7107643139068643538[24] = 1;
out_7107643139068643538[25] = 0;
out_7107643139068643538[26] = 0;
out_7107643139068643538[27] = dt;
out_7107643139068643538[28] = 0;
out_7107643139068643538[29] = 0;
out_7107643139068643538[30] = 0;
out_7107643139068643538[31] = 0;
out_7107643139068643538[32] = 0;
out_7107643139068643538[33] = 0;
out_7107643139068643538[34] = 0;
out_7107643139068643538[35] = 0;
out_7107643139068643538[36] = 1;
out_7107643139068643538[37] = 0;
out_7107643139068643538[38] = 0;
out_7107643139068643538[39] = 0;
out_7107643139068643538[40] = 0;
out_7107643139068643538[41] = 0;
out_7107643139068643538[42] = 0;
out_7107643139068643538[43] = 0;
out_7107643139068643538[44] = 0;
out_7107643139068643538[45] = 0;
out_7107643139068643538[46] = 0;
out_7107643139068643538[47] = 0;
out_7107643139068643538[48] = 1;
out_7107643139068643538[49] = 0;
out_7107643139068643538[50] = 0;
out_7107643139068643538[51] = 0;
out_7107643139068643538[52] = 0;
out_7107643139068643538[53] = 0;
out_7107643139068643538[54] = 0;
out_7107643139068643538[55] = 0;
out_7107643139068643538[56] = 0;
out_7107643139068643538[57] = 0;
out_7107643139068643538[58] = 0;
out_7107643139068643538[59] = 0;
out_7107643139068643538[60] = 1;
out_7107643139068643538[61] = 0;
out_7107643139068643538[62] = 0;
out_7107643139068643538[63] = 0;
out_7107643139068643538[64] = 0;
out_7107643139068643538[65] = 0;
out_7107643139068643538[66] = 0;
out_7107643139068643538[67] = 0;
out_7107643139068643538[68] = 0;
out_7107643139068643538[69] = 0;
out_7107643139068643538[70] = 0;
out_7107643139068643538[71] = 0;
out_7107643139068643538[72] = 1;
out_7107643139068643538[73] = dt;
out_7107643139068643538[74] = 0;
out_7107643139068643538[75] = 0;
out_7107643139068643538[76] = 0;
out_7107643139068643538[77] = 0;
out_7107643139068643538[78] = 0;
out_7107643139068643538[79] = 0;
out_7107643139068643538[80] = 0;
out_7107643139068643538[81] = 0;
out_7107643139068643538[82] = 0;
out_7107643139068643538[83] = 0;
out_7107643139068643538[84] = 1;
out_7107643139068643538[85] = dt;
out_7107643139068643538[86] = 0;
out_7107643139068643538[87] = 0;
out_7107643139068643538[88] = 0;
out_7107643139068643538[89] = 0;
out_7107643139068643538[90] = 0;
out_7107643139068643538[91] = 0;
out_7107643139068643538[92] = 0;
out_7107643139068643538[93] = 0;
out_7107643139068643538[94] = 0;
out_7107643139068643538[95] = 0;
out_7107643139068643538[96] = 1;
out_7107643139068643538[97] = 0;
out_7107643139068643538[98] = 0;
out_7107643139068643538[99] = 0;
out_7107643139068643538[100] = 0;
out_7107643139068643538[101] = 0;
out_7107643139068643538[102] = 0;
out_7107643139068643538[103] = 0;
out_7107643139068643538[104] = 0;
out_7107643139068643538[105] = 0;
out_7107643139068643538[106] = 0;
out_7107643139068643538[107] = 0;
out_7107643139068643538[108] = 1;
out_7107643139068643538[109] = 0;
out_7107643139068643538[110] = 0;
out_7107643139068643538[111] = 0;
out_7107643139068643538[112] = 0;
out_7107643139068643538[113] = 0;
out_7107643139068643538[114] = 0;
out_7107643139068643538[115] = 0;
out_7107643139068643538[116] = 0;
out_7107643139068643538[117] = 0;
out_7107643139068643538[118] = 0;
out_7107643139068643538[119] = 0;
out_7107643139068643538[120] = 1;
}
void h_6(double *state, double *sat_pos, double *out_9074704291738973277) {
out_9074704291738973277[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_1014783744438669605) {
out_1014783744438669605[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_8056400913830408537) {
out_8056400913830408537[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_8056400913830408537[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_8056400913830408537[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_8056400913830408537[3] = 0;
out_8056400913830408537[4] = 0;
out_8056400913830408537[5] = 0;
out_8056400913830408537[6] = 1;
out_8056400913830408537[7] = 0;
out_8056400913830408537[8] = 0;
out_8056400913830408537[9] = 0;
out_8056400913830408537[10] = 0;
void H_6(double *state, double *sat_pos, double *out_2647892629803094557) {
out_2647892629803094557[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_2647892629803094557[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_2647892629803094557[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_2647892629803094557[3] = 0;
out_2647892629803094557[4] = 0;
out_2647892629803094557[5] = 0;
out_2647892629803094557[6] = 1;
out_2647892629803094557[7] = 0;
out_2647892629803094557[8] = 0;
out_2647892629803094557[9] = 0;
out_2647892629803094557[10] = 0;
}
void h_20(double *state, double *sat_pos, double *out_5771271505288635021) {
out_5771271505288635021[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_4446463785726682298) {
out_4446463785726682298[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_5454875328892457956) {
out_5454875328892457956[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_5454875328892457956[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_5454875328892457956[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_5454875328892457956[3] = 0;
out_5454875328892457956[4] = 0;
out_5454875328892457956[5] = 0;
out_5454875328892457956[6] = 1;
out_5454875328892457956[7] = 0;
out_5454875328892457956[8] = 0;
out_5454875328892457956[9] = 1;
out_5454875328892457956[10] = sat_pos[3];
void H_20(double *state, double *sat_pos, double *out_2058835805956563056) {
out_2058835805956563056[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_2058835805956563056[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_2058835805956563056[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_2058835805956563056[3] = 0;
out_2058835805956563056[4] = 0;
out_2058835805956563056[5] = 0;
out_2058835805956563056[6] = 1;
out_2058835805956563056[7] = 0;
out_2058835805956563056[8] = 0;
out_2058835805956563056[9] = 1;
out_2058835805956563056[10] = sat_pos[3];
}
void h_7(double *state, double *sat_pos_vel, double *out_5412429359018771727) {
out_5412429359018771727[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_6977013047509885177) {
out_6977013047509885177[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_1987528380465191465) {
out_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[6] = 0;
out_1987528380465191465[7] = 1;
out_1987528380465191465[8] = 0;
out_1987528380465191465[9] = 0;
out_1987528380465191465[10] = 0;
void H_7(double *state, double *sat_pos_vel, double *out_3396370189700754510) {
out_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[6] = 0;
out_3396370189700754510[7] = 1;
out_3396370189700754510[8] = 0;
out_3396370189700754510[9] = 0;
out_3396370189700754510[10] = 0;
}
void h_21(double *state, double *sat_pos_vel, double *out_5412429359018771727) {
out_5412429359018771727[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_6977013047509885177) {
out_6977013047509885177[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_1987528380465191465) {
out_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[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_1987528380465191465[6] = 0;
out_1987528380465191465[7] = 1;
out_1987528380465191465[8] = 0;
out_1987528380465191465[9] = 0;
out_1987528380465191465[10] = 0;
void H_21(double *state, double *sat_pos_vel, double *out_3396370189700754510) {
out_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[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_3396370189700754510[6] = 0;
out_3396370189700754510[7] = 1;
out_3396370189700754510[8] = 0;
out_3396370189700754510[9] = 0;
out_3396370189700754510[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_2067482950567323166) {
err_fun(nom_x, delta_x, out_2067482950567323166);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_5270397344340919402) {
err_fun(nom_x, delta_x, out_5270397344340919402);
}
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_2603530649313476471) {
inv_err_fun(nom_x, true_x, out_2603530649313476471);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_1512259161409062554) {
inv_err_fun(nom_x, true_x, out_1512259161409062554);
}
void gnss_H_mod_fun(double *state, double *out_5820927568950649229) {
H_mod_fun(state, out_5820927568950649229);
void gnss_H_mod_fun(double *state, double *out_8413937430488034064) {
H_mod_fun(state, out_8413937430488034064);
}
void gnss_f_fun(double *state, double dt, double *out_17551380326686977) {
f_fun(state, dt, out_17551380326686977);
void gnss_f_fun(double *state, double dt, double *out_8044790491496694058) {
f_fun(state, dt, out_8044790491496694058);
}
void gnss_F_fun(double *state, double dt, double *out_414838326010165834) {
F_fun(state, dt, out_414838326010165834);
void gnss_F_fun(double *state, double dt, double *out_7107643139068643538) {
F_fun(state, dt, out_7107643139068643538);
}
void gnss_h_6(double *state, double *sat_pos, double *out_9074704291738973277) {
h_6(state, sat_pos, out_9074704291738973277);
void gnss_h_6(double *state, double *sat_pos, double *out_1014783744438669605) {
h_6(state, sat_pos, out_1014783744438669605);
}
void gnss_H_6(double *state, double *sat_pos, double *out_8056400913830408537) {
H_6(state, sat_pos, out_8056400913830408537);
void gnss_H_6(double *state, double *sat_pos, double *out_2647892629803094557) {
H_6(state, sat_pos, out_2647892629803094557);
}
void gnss_h_20(double *state, double *sat_pos, double *out_5771271505288635021) {
h_20(state, sat_pos, out_5771271505288635021);
void gnss_h_20(double *state, double *sat_pos, double *out_4446463785726682298) {
h_20(state, sat_pos, out_4446463785726682298);
}
void gnss_H_20(double *state, double *sat_pos, double *out_5454875328892457956) {
H_20(state, sat_pos, out_5454875328892457956);
void gnss_H_20(double *state, double *sat_pos, double *out_2058835805956563056) {
H_20(state, sat_pos, out_2058835805956563056);
}
void gnss_h_7(double *state, double *sat_pos_vel, double *out_5412429359018771727) {
h_7(state, sat_pos_vel, out_5412429359018771727);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_6977013047509885177) {
h_7(state, sat_pos_vel, out_6977013047509885177);
}
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1987528380465191465) {
H_7(state, sat_pos_vel, out_1987528380465191465);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_3396370189700754510) {
H_7(state, sat_pos_vel, out_3396370189700754510);
}
void gnss_h_21(double *state, double *sat_pos_vel, double *out_5412429359018771727) {
h_21(state, sat_pos_vel, out_5412429359018771727);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_6977013047509885177) {
h_21(state, sat_pos_vel, out_6977013047509885177);
}
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1987528380465191465) {
H_21(state, sat_pos_vel, out_1987528380465191465);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_3396370189700754510) {
H_21(state, sat_pos_vel, out_3396370189700754510);
}
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_2067482950567323166);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_2603530649313476471);
void gnss_H_mod_fun(double *state, double *out_5820927568950649229);
void gnss_f_fun(double *state, double dt, double *out_17551380326686977);
void gnss_F_fun(double *state, double dt, double *out_414838326010165834);
void gnss_h_6(double *state, double *sat_pos, double *out_9074704291738973277);
void gnss_H_6(double *state, double *sat_pos, double *out_8056400913830408537);
void gnss_h_20(double *state, double *sat_pos, double *out_5771271505288635021);
void gnss_H_20(double *state, double *sat_pos, double *out_5454875328892457956);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_5412429359018771727);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1987528380465191465);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_5412429359018771727);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1987528380465191465);
void gnss_err_fun(double *nom_x, double *delta_x, double *out_5270397344340919402);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_1512259161409062554);
void gnss_H_mod_fun(double *state, double *out_8413937430488034064);
void gnss_f_fun(double *state, double dt, double *out_8044790491496694058);
void gnss_F_fun(double *state, double dt, double *out_7107643139068643538);
void gnss_h_6(double *state, double *sat_pos, double *out_1014783744438669605);
void gnss_H_6(double *state, double *sat_pos, double *out_2647892629803094557);
void gnss_h_20(double *state, double *sat_pos, double *out_4446463785726682298);
void gnss_H_20(double *state, double *sat_pos, double *out_2058835805956563056);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_6977013047509885177);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_3396370189700754510);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_6977013047509885177);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_3396370189700754510);
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_3981994659674141472);
void live_err_fun(double *nom_x, double *delta_x, double *out_3356596097038928074);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_4391465575185495497);
void live_H_mod_fun(double *state, double *out_4990766548554672415);
void live_f_fun(double *state, double dt, double *out_3056772812413117361);
void live_F_fun(double *state, double dt, double *out_3001170024940786206);
void live_h_4(double *state, double *unused, double *out_4623444193655221191);
void live_H_4(double *state, double *unused, double *out_2603623762087390332);
void live_h_9(double *state, double *unused, double *out_4168973980757166041);
void live_H_9(double *state, double *unused, double *out_2362434115457799687);
void live_h_10(double *state, double *unused, double *out_3520412221597307166);
void live_H_10(double *state, double *unused, double *out_7396015770213526348);
void live_h_12(double *state, double *unused, double *out_7977792700499542139);
void live_H_12(double *state, double *unused, double *out_2415832645944571463);
void live_h_35(double *state, double *unused, double *out_3752355986187417342);
void live_H_35(double *state, double *unused, double *out_763038295285217044);
void live_h_32(double *state, double *unused, double *out_2897039262415537399);
void live_H_32(double *state, double *unused, double *out_6564342667492829602);
void live_h_13(double *state, double *unused, double *out_7535737061045930452);
void live_H_13(double *state, double *unused, double *out_3042894167287587122);
void live_h_14(double *state, double *unused, double *out_4168973980757166041);
void live_H_14(double *state, double *unused, double *out_2362434115457799687);
void live_h_33(double *state, double *unused, double *out_3687821174760154582);
void live_H_33(double *state, double *unused, double *out_3913595299924074648);
void live_H(double *in_vec, double *out_2107061543090158966);
void live_err_fun(double *nom_x, double *delta_x, double *out_1090842864546838841);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_1751534880763809428);
void live_H_mod_fun(double *state, double *out_2270168032272230264);
void live_f_fun(double *state, double dt, double *out_9047640957774126431);
void live_F_fun(double *state, double dt, double *out_4752414459068816126);
void live_h_4(double *state, double *unused, double *out_1938047718740512528);
void live_H_4(double *state, double *unused, double *out_8437989887064667391);
void live_h_9(double *state, double *unused, double *out_2016376091952434025);
void live_H_9(double *state, double *unused, double *out_2721535251380436755);
void live_h_10(double *state, double *unused, double *out_8252027734007822314);
void live_H_10(double *state, double *unused, double *out_1022365631619946756);
void live_h_12(double *state, double *unused, double *out_490832941153373832);
void live_H_12(double *state, double *unused, double *out_4989297778612922430);
void live_h_35(double *state, double *unused, double *out_851812883202032952);
void live_H_35(double *state, double *unused, double *out_6642092129272276849);
void live_h_32(double *state, double *unused, double *out_7198045605532929596);
void live_H_32(double *state, double *unused, double *out_6510927344331536380);
void live_h_13(double *state, double *unused, double *out_6813957582124674447);
void live_H_13(double *state, double *unused, double *out_7851984426394690165);
void live_h_14(double *state, double *unused, double *out_2016376091952434025);
void live_H_14(double *state, double *unused, double *out_2721535251380436755);
void live_h_33(double *state, double *unused, double *out_1521077794722315231);
void live_H_33(double *state, double *unused, double *out_3491535124633419245);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/modeld/_dmonitoringmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_modeld
BIN
View File
Binary file not shown.
selfdrive/modeld/_navmodeld
BIN
View File
Binary file not shown.
selfdrive/modeld/models/supercombo.thneed
BIN
View File
Binary file not shown.
selfdrive/navd/libmaprender.so
BIN
View File
Binary file not shown.
selfdrive/ui/_ui
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.
selfdrive/ui/translations/main_de.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
selfdrive/ui/translations/main_ja.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
selfdrive/ui/translations/main_ko.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
selfdrive/ui/translations/main_pt-BR.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
selfdrive/ui/translations/main_zh-CHS.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
selfdrive/ui/translations/main_zh-CHT.ts
+8 -8
View File
@@ -362,6 +362,14 @@ NOR - Normal tune.
SPT - Sport tune.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>Enable Stock Longitudinal Control</source>
<translation type="unfinished"></translation>
@@ -371,14 +379,6 @@ SPT - Sport tune.</source>
Reboot Required.</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>*β* Enable Otisserv</source>
<translation type="unfinished"></translation>
</message>
<message>
<source>When enabled, you will be able to access some features remotely through dragonpilot.org.</source>
<translation type="unfinished"></translation>
</message>
</context>
<context>
<name>DeclinePage</name>
system/camerad/camerad
BIN
View File
Binary file not shown.
system/clocksd/clocksd
BIN
View File
Binary file not shown.
system/logcatd/logcatd
BIN
View File
Binary file not shown.
system/loggerd/bootlog
BIN
View File
Binary file not shown.
system/loggerd/encoderd
BIN
View File
Binary file not shown.
system/loggerd/loggerd
BIN
View File
Binary file not shown.
system/proclogd/proclogd
BIN
View File
Binary file not shown.
system/sensord/_sensord
BIN
View File
Binary file not shown.
system/ubloxd/ubloxd
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/__init__.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_model.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_ocp.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_ocp_solver.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_sim.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/acados_sim_solver.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/builders.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_constraint.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_discrete_dynamics.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_explicit_ode.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_external_cost.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_gnsf.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_implicit_ode.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/generate_c_code_nls_cost.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
third_party/acados/acados_template/__pycache__/utils.cpython-311.pyc
Vendored
BIN
View File
Binary file not shown.
tools/bodyteleop/__pycache__/bodyav.cpython-311.pyc
BIN
View File
Binary file not shown.
tools/bodyteleop/__pycache__/web.cpython-311.pyc
BIN
View File
Binary file not shown.