mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-10-01 03:43:42 +08:00
dragonpilot 2022-11-30T09:37:39 for EON/C2
version: dragonpilot v0.9.1 beta for EON/C2 date: 2022-11-30T09:37:39 dp-dev(priv2) master commit: e2204c868277f20a263c2f93e88c42999d0b0c15
This commit is contained in:
Dragonpilot Team
committed by
Comma Device
Comma Device
parent
1fdc1202d1
commit
de34fa87ee
@@ -166,10 +166,7 @@ class Calibrator:
|
||||
self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES)
|
||||
|
||||
straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER))
|
||||
if self.wide_camera:
|
||||
angle_std_threshold = 4*MAX_VEL_ANGLE_STD
|
||||
else:
|
||||
angle_std_threshold = MAX_VEL_ANGLE_STD
|
||||
angle_std_threshold = MAX_VEL_ANGLE_STD
|
||||
certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or
|
||||
(self.valid_blocks < INPUTS_NEEDED))
|
||||
if not (straight_and_fast and certain_if_calib):
|
||||
@@ -185,7 +182,6 @@ class Calibrator:
|
||||
new_wide_from_device_euler = np.array(wide_from_device_euler)
|
||||
else:
|
||||
new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT
|
||||
|
||||
self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] +
|
||||
(BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE)
|
||||
self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] +
|
||||
|
||||
@@ -187,6 +187,7 @@ class Laikad:
|
||||
"gpsTimeOfWeek": tow,
|
||||
"positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid),
|
||||
"velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid),
|
||||
# TODO std is incorrectly the dimension of the measurements and not 3D
|
||||
"positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t),
|
||||
"ubloxMonoTime": gnss_mono_time,
|
||||
"correctedMeasurements": meas_msgs
|
||||
|
||||
Binary file not shown.
@@ -32,7 +32,7 @@ public:
|
||||
void finite_check(double current_time = NAN);
|
||||
void time_check(double current_time = NAN);
|
||||
void update_reset_tracker();
|
||||
bool isGpsOK();
|
||||
bool is_gps_ok();
|
||||
void determine_gps_mode(double current_time);
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
|
||||
@@ -69,8 +69,9 @@ private:
|
||||
std::unique_ptr<LocalCoord> converter;
|
||||
|
||||
int64_t unix_timestamp_millis = 0;
|
||||
double last_gps_fix = 0;
|
||||
double reset_tracker = 0.0;
|
||||
bool device_fell = false;
|
||||
bool gps_mode = false;
|
||||
bool gps_valid = false;
|
||||
// bool ublox_available = true;
|
||||
};
|
||||
|
||||
@@ -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_1840749225442711100) {
|
||||
out_1840749225442711100[0] = delta_x[0] + nom_x[0];
|
||||
out_1840749225442711100[1] = delta_x[1] + nom_x[1];
|
||||
out_1840749225442711100[2] = delta_x[2] + nom_x[2];
|
||||
out_1840749225442711100[3] = delta_x[3] + nom_x[3];
|
||||
out_1840749225442711100[4] = delta_x[4] + nom_x[4];
|
||||
out_1840749225442711100[5] = delta_x[5] + nom_x[5];
|
||||
out_1840749225442711100[6] = delta_x[6] + nom_x[6];
|
||||
out_1840749225442711100[7] = delta_x[7] + nom_x[7];
|
||||
out_1840749225442711100[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447) {
|
||||
out_7743502251243671447[0] = delta_x[0] + nom_x[0];
|
||||
out_7743502251243671447[1] = delta_x[1] + nom_x[1];
|
||||
out_7743502251243671447[2] = delta_x[2] + nom_x[2];
|
||||
out_7743502251243671447[3] = delta_x[3] + nom_x[3];
|
||||
out_7743502251243671447[4] = delta_x[4] + nom_x[4];
|
||||
out_7743502251243671447[5] = delta_x[5] + nom_x[5];
|
||||
out_7743502251243671447[6] = delta_x[6] + nom_x[6];
|
||||
out_7743502251243671447[7] = delta_x[7] + nom_x[7];
|
||||
out_7743502251243671447[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
|
||||
out_7887258947850824197[0] = -nom_x[0] + true_x[0];
|
||||
out_7887258947850824197[1] = -nom_x[1] + true_x[1];
|
||||
out_7887258947850824197[2] = -nom_x[2] + true_x[2];
|
||||
out_7887258947850824197[3] = -nom_x[3] + true_x[3];
|
||||
out_7887258947850824197[4] = -nom_x[4] + true_x[4];
|
||||
out_7887258947850824197[5] = -nom_x[5] + true_x[5];
|
||||
out_7887258947850824197[6] = -nom_x[6] + true_x[6];
|
||||
out_7887258947850824197[7] = -nom_x[7] + true_x[7];
|
||||
out_7887258947850824197[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
|
||||
out_8632432640030731669[0] = -nom_x[0] + true_x[0];
|
||||
out_8632432640030731669[1] = -nom_x[1] + true_x[1];
|
||||
out_8632432640030731669[2] = -nom_x[2] + true_x[2];
|
||||
out_8632432640030731669[3] = -nom_x[3] + true_x[3];
|
||||
out_8632432640030731669[4] = -nom_x[4] + true_x[4];
|
||||
out_8632432640030731669[5] = -nom_x[5] + true_x[5];
|
||||
out_8632432640030731669[6] = -nom_x[6] + true_x[6];
|
||||
out_8632432640030731669[7] = -nom_x[7] + true_x[7];
|
||||
out_8632432640030731669[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_5849575757077882696) {
|
||||
out_5849575757077882696[0] = 1.0;
|
||||
out_5849575757077882696[1] = 0;
|
||||
out_5849575757077882696[2] = 0;
|
||||
out_5849575757077882696[3] = 0;
|
||||
out_5849575757077882696[4] = 0;
|
||||
out_5849575757077882696[5] = 0;
|
||||
out_5849575757077882696[6] = 0;
|
||||
out_5849575757077882696[7] = 0;
|
||||
out_5849575757077882696[8] = 0;
|
||||
out_5849575757077882696[9] = 0;
|
||||
out_5849575757077882696[10] = 1.0;
|
||||
out_5849575757077882696[11] = 0;
|
||||
out_5849575757077882696[12] = 0;
|
||||
out_5849575757077882696[13] = 0;
|
||||
out_5849575757077882696[14] = 0;
|
||||
out_5849575757077882696[15] = 0;
|
||||
out_5849575757077882696[16] = 0;
|
||||
out_5849575757077882696[17] = 0;
|
||||
out_5849575757077882696[18] = 0;
|
||||
out_5849575757077882696[19] = 0;
|
||||
out_5849575757077882696[20] = 1.0;
|
||||
out_5849575757077882696[21] = 0;
|
||||
out_5849575757077882696[22] = 0;
|
||||
out_5849575757077882696[23] = 0;
|
||||
out_5849575757077882696[24] = 0;
|
||||
out_5849575757077882696[25] = 0;
|
||||
out_5849575757077882696[26] = 0;
|
||||
out_5849575757077882696[27] = 0;
|
||||
out_5849575757077882696[28] = 0;
|
||||
out_5849575757077882696[29] = 0;
|
||||
out_5849575757077882696[30] = 1.0;
|
||||
out_5849575757077882696[31] = 0;
|
||||
out_5849575757077882696[32] = 0;
|
||||
out_5849575757077882696[33] = 0;
|
||||
out_5849575757077882696[34] = 0;
|
||||
out_5849575757077882696[35] = 0;
|
||||
out_5849575757077882696[36] = 0;
|
||||
out_5849575757077882696[37] = 0;
|
||||
out_5849575757077882696[38] = 0;
|
||||
out_5849575757077882696[39] = 0;
|
||||
out_5849575757077882696[40] = 1.0;
|
||||
out_5849575757077882696[41] = 0;
|
||||
out_5849575757077882696[42] = 0;
|
||||
out_5849575757077882696[43] = 0;
|
||||
out_5849575757077882696[44] = 0;
|
||||
out_5849575757077882696[45] = 0;
|
||||
out_5849575757077882696[46] = 0;
|
||||
out_5849575757077882696[47] = 0;
|
||||
out_5849575757077882696[48] = 0;
|
||||
out_5849575757077882696[49] = 0;
|
||||
out_5849575757077882696[50] = 1.0;
|
||||
out_5849575757077882696[51] = 0;
|
||||
out_5849575757077882696[52] = 0;
|
||||
out_5849575757077882696[53] = 0;
|
||||
out_5849575757077882696[54] = 0;
|
||||
out_5849575757077882696[55] = 0;
|
||||
out_5849575757077882696[56] = 0;
|
||||
out_5849575757077882696[57] = 0;
|
||||
out_5849575757077882696[58] = 0;
|
||||
out_5849575757077882696[59] = 0;
|
||||
out_5849575757077882696[60] = 1.0;
|
||||
out_5849575757077882696[61] = 0;
|
||||
out_5849575757077882696[62] = 0;
|
||||
out_5849575757077882696[63] = 0;
|
||||
out_5849575757077882696[64] = 0;
|
||||
out_5849575757077882696[65] = 0;
|
||||
out_5849575757077882696[66] = 0;
|
||||
out_5849575757077882696[67] = 0;
|
||||
out_5849575757077882696[68] = 0;
|
||||
out_5849575757077882696[69] = 0;
|
||||
out_5849575757077882696[70] = 1.0;
|
||||
out_5849575757077882696[71] = 0;
|
||||
out_5849575757077882696[72] = 0;
|
||||
out_5849575757077882696[73] = 0;
|
||||
out_5849575757077882696[74] = 0;
|
||||
out_5849575757077882696[75] = 0;
|
||||
out_5849575757077882696[76] = 0;
|
||||
out_5849575757077882696[77] = 0;
|
||||
out_5849575757077882696[78] = 0;
|
||||
out_5849575757077882696[79] = 0;
|
||||
out_5849575757077882696[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_6955983744751080330) {
|
||||
out_6955983744751080330[0] = 1.0;
|
||||
out_6955983744751080330[1] = 0;
|
||||
out_6955983744751080330[2] = 0;
|
||||
out_6955983744751080330[3] = 0;
|
||||
out_6955983744751080330[4] = 0;
|
||||
out_6955983744751080330[5] = 0;
|
||||
out_6955983744751080330[6] = 0;
|
||||
out_6955983744751080330[7] = 0;
|
||||
out_6955983744751080330[8] = 0;
|
||||
out_6955983744751080330[9] = 0;
|
||||
out_6955983744751080330[10] = 1.0;
|
||||
out_6955983744751080330[11] = 0;
|
||||
out_6955983744751080330[12] = 0;
|
||||
out_6955983744751080330[13] = 0;
|
||||
out_6955983744751080330[14] = 0;
|
||||
out_6955983744751080330[15] = 0;
|
||||
out_6955983744751080330[16] = 0;
|
||||
out_6955983744751080330[17] = 0;
|
||||
out_6955983744751080330[18] = 0;
|
||||
out_6955983744751080330[19] = 0;
|
||||
out_6955983744751080330[20] = 1.0;
|
||||
out_6955983744751080330[21] = 0;
|
||||
out_6955983744751080330[22] = 0;
|
||||
out_6955983744751080330[23] = 0;
|
||||
out_6955983744751080330[24] = 0;
|
||||
out_6955983744751080330[25] = 0;
|
||||
out_6955983744751080330[26] = 0;
|
||||
out_6955983744751080330[27] = 0;
|
||||
out_6955983744751080330[28] = 0;
|
||||
out_6955983744751080330[29] = 0;
|
||||
out_6955983744751080330[30] = 1.0;
|
||||
out_6955983744751080330[31] = 0;
|
||||
out_6955983744751080330[32] = 0;
|
||||
out_6955983744751080330[33] = 0;
|
||||
out_6955983744751080330[34] = 0;
|
||||
out_6955983744751080330[35] = 0;
|
||||
out_6955983744751080330[36] = 0;
|
||||
out_6955983744751080330[37] = 0;
|
||||
out_6955983744751080330[38] = 0;
|
||||
out_6955983744751080330[39] = 0;
|
||||
out_6955983744751080330[40] = 1.0;
|
||||
out_6955983744751080330[41] = 0;
|
||||
out_6955983744751080330[42] = 0;
|
||||
out_6955983744751080330[43] = 0;
|
||||
out_6955983744751080330[44] = 0;
|
||||
out_6955983744751080330[45] = 0;
|
||||
out_6955983744751080330[46] = 0;
|
||||
out_6955983744751080330[47] = 0;
|
||||
out_6955983744751080330[48] = 0;
|
||||
out_6955983744751080330[49] = 0;
|
||||
out_6955983744751080330[50] = 1.0;
|
||||
out_6955983744751080330[51] = 0;
|
||||
out_6955983744751080330[52] = 0;
|
||||
out_6955983744751080330[53] = 0;
|
||||
out_6955983744751080330[54] = 0;
|
||||
out_6955983744751080330[55] = 0;
|
||||
out_6955983744751080330[56] = 0;
|
||||
out_6955983744751080330[57] = 0;
|
||||
out_6955983744751080330[58] = 0;
|
||||
out_6955983744751080330[59] = 0;
|
||||
out_6955983744751080330[60] = 1.0;
|
||||
out_6955983744751080330[61] = 0;
|
||||
out_6955983744751080330[62] = 0;
|
||||
out_6955983744751080330[63] = 0;
|
||||
out_6955983744751080330[64] = 0;
|
||||
out_6955983744751080330[65] = 0;
|
||||
out_6955983744751080330[66] = 0;
|
||||
out_6955983744751080330[67] = 0;
|
||||
out_6955983744751080330[68] = 0;
|
||||
out_6955983744751080330[69] = 0;
|
||||
out_6955983744751080330[70] = 1.0;
|
||||
out_6955983744751080330[71] = 0;
|
||||
out_6955983744751080330[72] = 0;
|
||||
out_6955983744751080330[73] = 0;
|
||||
out_6955983744751080330[74] = 0;
|
||||
out_6955983744751080330[75] = 0;
|
||||
out_6955983744751080330[76] = 0;
|
||||
out_6955983744751080330[77] = 0;
|
||||
out_6955983744751080330[78] = 0;
|
||||
out_6955983744751080330[79] = 0;
|
||||
out_6955983744751080330[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_1306455665935687231) {
|
||||
out_1306455665935687231[0] = state[0];
|
||||
out_1306455665935687231[1] = state[1];
|
||||
out_1306455665935687231[2] = state[2];
|
||||
out_1306455665935687231[3] = state[3];
|
||||
out_1306455665935687231[4] = state[4];
|
||||
out_1306455665935687231[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_1306455665935687231[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_1306455665935687231[7] = state[7];
|
||||
out_1306455665935687231[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_1057256165937700131) {
|
||||
out_1057256165937700131[0] = state[0];
|
||||
out_1057256165937700131[1] = state[1];
|
||||
out_1057256165937700131[2] = state[2];
|
||||
out_1057256165937700131[3] = state[3];
|
||||
out_1057256165937700131[4] = state[4];
|
||||
out_1057256165937700131[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8000000000000007*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_1057256165937700131[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_1057256165937700131[7] = state[7];
|
||||
out_1057256165937700131[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_1015750461961158607) {
|
||||
out_1015750461961158607[0] = 1;
|
||||
out_1015750461961158607[1] = 0;
|
||||
out_1015750461961158607[2] = 0;
|
||||
out_1015750461961158607[3] = 0;
|
||||
out_1015750461961158607[4] = 0;
|
||||
out_1015750461961158607[5] = 0;
|
||||
out_1015750461961158607[6] = 0;
|
||||
out_1015750461961158607[7] = 0;
|
||||
out_1015750461961158607[8] = 0;
|
||||
out_1015750461961158607[9] = 0;
|
||||
out_1015750461961158607[10] = 1;
|
||||
out_1015750461961158607[11] = 0;
|
||||
out_1015750461961158607[12] = 0;
|
||||
out_1015750461961158607[13] = 0;
|
||||
out_1015750461961158607[14] = 0;
|
||||
out_1015750461961158607[15] = 0;
|
||||
out_1015750461961158607[16] = 0;
|
||||
out_1015750461961158607[17] = 0;
|
||||
out_1015750461961158607[18] = 0;
|
||||
out_1015750461961158607[19] = 0;
|
||||
out_1015750461961158607[20] = 1;
|
||||
out_1015750461961158607[21] = 0;
|
||||
out_1015750461961158607[22] = 0;
|
||||
out_1015750461961158607[23] = 0;
|
||||
out_1015750461961158607[24] = 0;
|
||||
out_1015750461961158607[25] = 0;
|
||||
out_1015750461961158607[26] = 0;
|
||||
out_1015750461961158607[27] = 0;
|
||||
out_1015750461961158607[28] = 0;
|
||||
out_1015750461961158607[29] = 0;
|
||||
out_1015750461961158607[30] = 1;
|
||||
out_1015750461961158607[31] = 0;
|
||||
out_1015750461961158607[32] = 0;
|
||||
out_1015750461961158607[33] = 0;
|
||||
out_1015750461961158607[34] = 0;
|
||||
out_1015750461961158607[35] = 0;
|
||||
out_1015750461961158607[36] = 0;
|
||||
out_1015750461961158607[37] = 0;
|
||||
out_1015750461961158607[38] = 0;
|
||||
out_1015750461961158607[39] = 0;
|
||||
out_1015750461961158607[40] = 1;
|
||||
out_1015750461961158607[41] = 0;
|
||||
out_1015750461961158607[42] = 0;
|
||||
out_1015750461961158607[43] = 0;
|
||||
out_1015750461961158607[44] = 0;
|
||||
out_1015750461961158607[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_1015750461961158607[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_1015750461961158607[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[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_1015750461961158607[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_1015750461961158607[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_1015750461961158607[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_1015750461961158607[53] = -9.8000000000000007*dt;
|
||||
out_1015750461961158607[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_1015750461961158607[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_1015750461961158607[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[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_1015750461961158607[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_1015750461961158607[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_1015750461961158607[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_1015750461961158607[62] = 0;
|
||||
out_1015750461961158607[63] = 0;
|
||||
out_1015750461961158607[64] = 0;
|
||||
out_1015750461961158607[65] = 0;
|
||||
out_1015750461961158607[66] = 0;
|
||||
out_1015750461961158607[67] = 0;
|
||||
out_1015750461961158607[68] = 0;
|
||||
out_1015750461961158607[69] = 0;
|
||||
out_1015750461961158607[70] = 1;
|
||||
out_1015750461961158607[71] = 0;
|
||||
out_1015750461961158607[72] = 0;
|
||||
out_1015750461961158607[73] = 0;
|
||||
out_1015750461961158607[74] = 0;
|
||||
out_1015750461961158607[75] = 0;
|
||||
out_1015750461961158607[76] = 0;
|
||||
out_1015750461961158607[77] = 0;
|
||||
out_1015750461961158607[78] = 0;
|
||||
out_1015750461961158607[79] = 0;
|
||||
out_1015750461961158607[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_2042254547319901591) {
|
||||
out_2042254547319901591[0] = 1;
|
||||
out_2042254547319901591[1] = 0;
|
||||
out_2042254547319901591[2] = 0;
|
||||
out_2042254547319901591[3] = 0;
|
||||
out_2042254547319901591[4] = 0;
|
||||
out_2042254547319901591[5] = 0;
|
||||
out_2042254547319901591[6] = 0;
|
||||
out_2042254547319901591[7] = 0;
|
||||
out_2042254547319901591[8] = 0;
|
||||
out_2042254547319901591[9] = 0;
|
||||
out_2042254547319901591[10] = 1;
|
||||
out_2042254547319901591[11] = 0;
|
||||
out_2042254547319901591[12] = 0;
|
||||
out_2042254547319901591[13] = 0;
|
||||
out_2042254547319901591[14] = 0;
|
||||
out_2042254547319901591[15] = 0;
|
||||
out_2042254547319901591[16] = 0;
|
||||
out_2042254547319901591[17] = 0;
|
||||
out_2042254547319901591[18] = 0;
|
||||
out_2042254547319901591[19] = 0;
|
||||
out_2042254547319901591[20] = 1;
|
||||
out_2042254547319901591[21] = 0;
|
||||
out_2042254547319901591[22] = 0;
|
||||
out_2042254547319901591[23] = 0;
|
||||
out_2042254547319901591[24] = 0;
|
||||
out_2042254547319901591[25] = 0;
|
||||
out_2042254547319901591[26] = 0;
|
||||
out_2042254547319901591[27] = 0;
|
||||
out_2042254547319901591[28] = 0;
|
||||
out_2042254547319901591[29] = 0;
|
||||
out_2042254547319901591[30] = 1;
|
||||
out_2042254547319901591[31] = 0;
|
||||
out_2042254547319901591[32] = 0;
|
||||
out_2042254547319901591[33] = 0;
|
||||
out_2042254547319901591[34] = 0;
|
||||
out_2042254547319901591[35] = 0;
|
||||
out_2042254547319901591[36] = 0;
|
||||
out_2042254547319901591[37] = 0;
|
||||
out_2042254547319901591[38] = 0;
|
||||
out_2042254547319901591[39] = 0;
|
||||
out_2042254547319901591[40] = 1;
|
||||
out_2042254547319901591[41] = 0;
|
||||
out_2042254547319901591[42] = 0;
|
||||
out_2042254547319901591[43] = 0;
|
||||
out_2042254547319901591[44] = 0;
|
||||
out_2042254547319901591[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_2042254547319901591[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_2042254547319901591[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_2042254547319901591[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_2042254547319901591[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_2042254547319901591[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_2042254547319901591[53] = -9.8000000000000007*dt;
|
||||
out_2042254547319901591[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_2042254547319901591[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_2042254547319901591[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_2042254547319901591[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_2042254547319901591[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_2042254547319901591[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_2042254547319901591[62] = 0;
|
||||
out_2042254547319901591[63] = 0;
|
||||
out_2042254547319901591[64] = 0;
|
||||
out_2042254547319901591[65] = 0;
|
||||
out_2042254547319901591[66] = 0;
|
||||
out_2042254547319901591[67] = 0;
|
||||
out_2042254547319901591[68] = 0;
|
||||
out_2042254547319901591[69] = 0;
|
||||
out_2042254547319901591[70] = 1;
|
||||
out_2042254547319901591[71] = 0;
|
||||
out_2042254547319901591[72] = 0;
|
||||
out_2042254547319901591[73] = 0;
|
||||
out_2042254547319901591[74] = 0;
|
||||
out_2042254547319901591[75] = 0;
|
||||
out_2042254547319901591[76] = 0;
|
||||
out_2042254547319901591[77] = 0;
|
||||
out_2042254547319901591[78] = 0;
|
||||
out_2042254547319901591[79] = 0;
|
||||
out_2042254547319901591[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_579481060641732431) {
|
||||
out_579481060641732431[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_9038137973563738704) {
|
||||
out_9038137973563738704[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_8838814511200069080) {
|
||||
out_8838814511200069080[0] = 0;
|
||||
out_8838814511200069080[1] = 0;
|
||||
out_8838814511200069080[2] = 0;
|
||||
out_8838814511200069080[3] = 0;
|
||||
out_8838814511200069080[4] = 0;
|
||||
out_8838814511200069080[5] = 0;
|
||||
out_8838814511200069080[6] = 1;
|
||||
out_8838814511200069080[7] = 0;
|
||||
out_8838814511200069080[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_6219790086191979796) {
|
||||
out_6219790086191979796[0] = 0;
|
||||
out_6219790086191979796[1] = 0;
|
||||
out_6219790086191979796[2] = 0;
|
||||
out_6219790086191979796[3] = 0;
|
||||
out_6219790086191979796[4] = 0;
|
||||
out_6219790086191979796[5] = 0;
|
||||
out_6219790086191979796[6] = 1;
|
||||
out_6219790086191979796[7] = 0;
|
||||
out_6219790086191979796[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_8628826922044058076) {
|
||||
out_8628826922044058076[0] = state[4];
|
||||
out_8628826922044058076[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_8933773235415263694) {
|
||||
out_8933773235415263694[0] = state[4];
|
||||
out_8933773235415263694[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7435279963503982970) {
|
||||
out_7435279963503982970[0] = 0;
|
||||
out_7435279963503982970[1] = 0;
|
||||
out_7435279963503982970[2] = 0;
|
||||
out_7435279963503982970[3] = 0;
|
||||
out_7435279963503982970[4] = 1;
|
||||
out_7435279963503982970[5] = 0;
|
||||
out_7435279963503982970[6] = 0;
|
||||
out_7435279963503982970[7] = 0;
|
||||
out_7435279963503982970[8] = 0;
|
||||
out_7435279963503982970[9] = 0;
|
||||
out_7435279963503982970[10] = 0;
|
||||
out_7435279963503982970[11] = 0;
|
||||
out_7435279963503982970[12] = 0;
|
||||
out_7435279963503982970[13] = 0;
|
||||
out_7435279963503982970[14] = 1;
|
||||
out_7435279963503982970[15] = 0;
|
||||
out_7435279963503982970[16] = 0;
|
||||
out_7435279963503982970[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_2661864487389656660) {
|
||||
out_2661864487389656660[0] = 0;
|
||||
out_2661864487389656660[1] = 0;
|
||||
out_2661864487389656660[2] = 0;
|
||||
out_2661864487389656660[3] = 0;
|
||||
out_2661864487389656660[4] = 1;
|
||||
out_2661864487389656660[5] = 0;
|
||||
out_2661864487389656660[6] = 0;
|
||||
out_2661864487389656660[7] = 0;
|
||||
out_2661864487389656660[8] = 0;
|
||||
out_2661864487389656660[9] = 0;
|
||||
out_2661864487389656660[10] = 0;
|
||||
out_2661864487389656660[11] = 0;
|
||||
out_2661864487389656660[12] = 0;
|
||||
out_2661864487389656660[13] = 0;
|
||||
out_2661864487389656660[14] = 1;
|
||||
out_2661864487389656660[15] = 0;
|
||||
out_2661864487389656660[16] = 0;
|
||||
out_2661864487389656660[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_8060396416076809453) {
|
||||
out_8060396416076809453[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_1353345261140371077) {
|
||||
out_1353345261140371077[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6320481552692820453) {
|
||||
out_6320481552692820453[0] = 0;
|
||||
out_6320481552692820453[1] = 0;
|
||||
out_6320481552692820453[2] = 0;
|
||||
out_6320481552692820453[3] = 0;
|
||||
out_6320481552692820453[4] = 1;
|
||||
out_6320481552692820453[5] = 0;
|
||||
out_6320481552692820453[6] = 0;
|
||||
out_6320481552692820453[7] = 0;
|
||||
out_6320481552692820453[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_3701457127684731169) {
|
||||
out_3701457127684731169[0] = 0;
|
||||
out_3701457127684731169[1] = 0;
|
||||
out_3701457127684731169[2] = 0;
|
||||
out_3701457127684731169[3] = 0;
|
||||
out_3701457127684731169[4] = 1;
|
||||
out_3701457127684731169[5] = 0;
|
||||
out_3701457127684731169[6] = 0;
|
||||
out_3701457127684731169[7] = 0;
|
||||
out_3701457127684731169[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_8663712300753286970) {
|
||||
out_8663712300753286970[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_3791544801975276460) {
|
||||
out_3791544801975276460[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_5866426243635426312) {
|
||||
out_5866426243635426312[0] = 0;
|
||||
out_5866426243635426312[1] = 0;
|
||||
out_5866426243635426312[2] = 0;
|
||||
out_5866426243635426312[3] = 0;
|
||||
out_5866426243635426312[4] = 0;
|
||||
out_5866426243635426312[5] = 0;
|
||||
out_5866426243635426312[6] = 0;
|
||||
out_5866426243635426312[7] = 1;
|
||||
out_5866426243635426312[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_8485450668643515596) {
|
||||
out_8485450668643515596[0] = 0;
|
||||
out_8485450668643515596[1] = 0;
|
||||
out_8485450668643515596[2] = 0;
|
||||
out_8485450668643515596[3] = 0;
|
||||
out_8485450668643515596[4] = 0;
|
||||
out_8485450668643515596[5] = 0;
|
||||
out_8485450668643515596[6] = 0;
|
||||
out_8485450668643515596[7] = 1;
|
||||
out_8485450668643515596[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_2233071042628120952) {
|
||||
out_2233071042628120952[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_4406932832484967267) {
|
||||
out_4406932832484967267[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_4096887481508877236) {
|
||||
out_4096887481508877236[0] = 0;
|
||||
out_4096887481508877236[1] = 0;
|
||||
out_4096887481508877236[2] = 0;
|
||||
out_4096887481508877236[3] = 1;
|
||||
out_4096887481508877236[4] = 0;
|
||||
out_4096887481508877236[5] = 0;
|
||||
out_4096887481508877236[6] = 0;
|
||||
out_4096887481508877236[7] = 0;
|
||||
out_4096887481508877236[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_5876220439485156080) {
|
||||
out_5876220439485156080[0] = 0;
|
||||
out_5876220439485156080[1] = 0;
|
||||
out_5876220439485156080[2] = 0;
|
||||
out_5876220439485156080[3] = 1;
|
||||
out_5876220439485156080[4] = 0;
|
||||
out_5876220439485156080[5] = 0;
|
||||
out_5876220439485156080[6] = 0;
|
||||
out_5876220439485156080[7] = 0;
|
||||
out_5876220439485156080[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_8528880318658063037) {
|
||||
out_8528880318658063037[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_6276545984403911908) {
|
||||
out_6276545984403911908[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_5810250208378428269) {
|
||||
out_5810250208378428269[0] = 0;
|
||||
out_5810250208378428269[1] = 1;
|
||||
out_5810250208378428269[2] = 0;
|
||||
out_5810250208378428269[3] = 0;
|
||||
out_5810250208378428269[4] = 0;
|
||||
out_5810250208378428269[5] = 0;
|
||||
out_5810250208378428269[6] = 0;
|
||||
out_5810250208378428269[7] = 0;
|
||||
out_5810250208378428269[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_3191225783370338985) {
|
||||
out_3191225783370338985[0] = 0;
|
||||
out_3191225783370338985[1] = 1;
|
||||
out_3191225783370338985[2] = 0;
|
||||
out_3191225783370338985[3] = 0;
|
||||
out_3191225783370338985[4] = 0;
|
||||
out_3191225783370338985[5] = 0;
|
||||
out_3191225783370338985[6] = 0;
|
||||
out_3191225783370338985[7] = 0;
|
||||
out_3191225783370338985[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_4837476363436818603) {
|
||||
out_4837476363436818603[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_7011338153293664918) {
|
||||
out_7011338153293664918[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_7554094848261592773) {
|
||||
out_7554094848261592773[0] = 1;
|
||||
out_7554094848261592773[1] = 0;
|
||||
out_7554094848261592773[2] = 0;
|
||||
out_7554094848261592773[3] = 0;
|
||||
out_7554094848261592773[4] = 0;
|
||||
out_7554094848261592773[5] = 0;
|
||||
out_7554094848261592773[6] = 0;
|
||||
out_7554094848261592773[7] = 0;
|
||||
out_7554094848261592773[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_8273624800439869559) {
|
||||
out_8273624800439869559[0] = 1;
|
||||
out_8273624800439869559[1] = 0;
|
||||
out_8273624800439869559[2] = 0;
|
||||
out_8273624800439869559[3] = 0;
|
||||
out_8273624800439869559[4] = 0;
|
||||
out_8273624800439869559[5] = 0;
|
||||
out_8273624800439869559[6] = 0;
|
||||
out_8273624800439869559[7] = 0;
|
||||
out_8273624800439869559[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_7350316286992083367) {
|
||||
out_7350316286992083367[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_8647698245415467134) {
|
||||
out_8647698245415467134[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_5240218141402074836) {
|
||||
out_5240218141402074836[0] = 0;
|
||||
out_5240218141402074836[1] = 0;
|
||||
out_5240218141402074836[2] = 0;
|
||||
out_5240218141402074836[3] = 0;
|
||||
out_5240218141402074836[4] = 0;
|
||||
out_5240218141402074836[5] = 0;
|
||||
out_5240218141402074836[6] = 0;
|
||||
out_5240218141402074836[7] = 0;
|
||||
out_5240218141402074836[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_7859242566410164120) {
|
||||
out_7859242566410164120[0] = 0;
|
||||
out_7859242566410164120[1] = 0;
|
||||
out_7859242566410164120[2] = 0;
|
||||
out_7859242566410164120[3] = 0;
|
||||
out_7859242566410164120[4] = 0;
|
||||
out_7859242566410164120[5] = 0;
|
||||
out_7859242566410164120[6] = 0;
|
||||
out_7859242566410164120[7] = 0;
|
||||
out_7859242566410164120[8] = 1;
|
||||
}
|
||||
#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_1840749225442711100) {
|
||||
err_fun(nom_x, delta_x, out_1840749225442711100);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447) {
|
||||
err_fun(nom_x, delta_x, out_7743502251243671447);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197) {
|
||||
inv_err_fun(nom_x, true_x, out_7887258947850824197);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669) {
|
||||
inv_err_fun(nom_x, true_x, out_8632432640030731669);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_5849575757077882696) {
|
||||
H_mod_fun(state, out_5849575757077882696);
|
||||
void car_H_mod_fun(double *state, double *out_6955983744751080330) {
|
||||
H_mod_fun(state, out_6955983744751080330);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_1306455665935687231) {
|
||||
f_fun(state, dt, out_1306455665935687231);
|
||||
void car_f_fun(double *state, double dt, double *out_1057256165937700131) {
|
||||
f_fun(state, dt, out_1057256165937700131);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_1015750461961158607) {
|
||||
F_fun(state, dt, out_1015750461961158607);
|
||||
void car_F_fun(double *state, double dt, double *out_2042254547319901591) {
|
||||
F_fun(state, dt, out_2042254547319901591);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_579481060641732431) {
|
||||
h_25(state, unused, out_579481060641732431);
|
||||
void car_h_25(double *state, double *unused, double *out_9038137973563738704) {
|
||||
h_25(state, unused, out_9038137973563738704);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_8838814511200069080) {
|
||||
H_25(state, unused, out_8838814511200069080);
|
||||
void car_H_25(double *state, double *unused, double *out_6219790086191979796) {
|
||||
H_25(state, unused, out_6219790086191979796);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_8628826922044058076) {
|
||||
h_24(state, unused, out_8628826922044058076);
|
||||
void car_h_24(double *state, double *unused, double *out_8933773235415263694) {
|
||||
h_24(state, unused, out_8933773235415263694);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7435279963503982970) {
|
||||
H_24(state, unused, out_7435279963503982970);
|
||||
void car_H_24(double *state, double *unused, double *out_2661864487389656660) {
|
||||
H_24(state, unused, out_2661864487389656660);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_8060396416076809453) {
|
||||
h_30(state, unused, out_8060396416076809453);
|
||||
void car_h_30(double *state, double *unused, double *out_1353345261140371077) {
|
||||
h_30(state, unused, out_1353345261140371077);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6320481552692820453) {
|
||||
H_30(state, unused, out_6320481552692820453);
|
||||
void car_H_30(double *state, double *unused, double *out_3701457127684731169) {
|
||||
H_30(state, unused, out_3701457127684731169);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_8663712300753286970) {
|
||||
h_26(state, unused, out_8663712300753286970);
|
||||
void car_h_26(double *state, double *unused, double *out_3791544801975276460) {
|
||||
h_26(state, unused, out_3791544801975276460);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_5866426243635426312) {
|
||||
H_26(state, unused, out_5866426243635426312);
|
||||
void car_H_26(double *state, double *unused, double *out_8485450668643515596) {
|
||||
H_26(state, unused, out_8485450668643515596);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_2233071042628120952) {
|
||||
h_27(state, unused, out_2233071042628120952);
|
||||
void car_h_27(double *state, double *unused, double *out_4406932832484967267) {
|
||||
h_27(state, unused, out_4406932832484967267);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_4096887481508877236) {
|
||||
H_27(state, unused, out_4096887481508877236);
|
||||
void car_H_27(double *state, double *unused, double *out_5876220439485156080) {
|
||||
H_27(state, unused, out_5876220439485156080);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_8528880318658063037) {
|
||||
h_29(state, unused, out_8528880318658063037);
|
||||
void car_h_29(double *state, double *unused, double *out_6276545984403911908) {
|
||||
h_29(state, unused, out_6276545984403911908);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_5810250208378428269) {
|
||||
H_29(state, unused, out_5810250208378428269);
|
||||
void car_H_29(double *state, double *unused, double *out_3191225783370338985) {
|
||||
H_29(state, unused, out_3191225783370338985);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_4837476363436818603) {
|
||||
h_28(state, unused, out_4837476363436818603);
|
||||
void car_h_28(double *state, double *unused, double *out_7011338153293664918) {
|
||||
h_28(state, unused, out_7011338153293664918);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_7554094848261592773) {
|
||||
H_28(state, unused, out_7554094848261592773);
|
||||
void car_H_28(double *state, double *unused, double *out_8273624800439869559) {
|
||||
H_28(state, unused, out_8273624800439869559);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_7350316286992083367) {
|
||||
h_31(state, unused, out_7350316286992083367);
|
||||
void car_h_31(double *state, double *unused, double *out_8647698245415467134) {
|
||||
h_31(state, unused, out_8647698245415467134);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_5240218141402074836) {
|
||||
H_31(state, unused, out_5240218141402074836);
|
||||
void car_H_31(double *state, double *unused, double *out_7859242566410164120) {
|
||||
H_31(state, unused, out_7859242566410164120);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_1840749225442711100);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_7887258947850824197);
|
||||
void car_H_mod_fun(double *state, double *out_5849575757077882696);
|
||||
void car_f_fun(double *state, double dt, double *out_1306455665935687231);
|
||||
void car_F_fun(double *state, double dt, double *out_1015750461961158607);
|
||||
void car_h_25(double *state, double *unused, double *out_579481060641732431);
|
||||
void car_H_25(double *state, double *unused, double *out_8838814511200069080);
|
||||
void car_h_24(double *state, double *unused, double *out_8628826922044058076);
|
||||
void car_H_24(double *state, double *unused, double *out_7435279963503982970);
|
||||
void car_h_30(double *state, double *unused, double *out_8060396416076809453);
|
||||
void car_H_30(double *state, double *unused, double *out_6320481552692820453);
|
||||
void car_h_26(double *state, double *unused, double *out_8663712300753286970);
|
||||
void car_H_26(double *state, double *unused, double *out_5866426243635426312);
|
||||
void car_h_27(double *state, double *unused, double *out_2233071042628120952);
|
||||
void car_H_27(double *state, double *unused, double *out_4096887481508877236);
|
||||
void car_h_29(double *state, double *unused, double *out_8528880318658063037);
|
||||
void car_H_29(double *state, double *unused, double *out_5810250208378428269);
|
||||
void car_h_28(double *state, double *unused, double *out_4837476363436818603);
|
||||
void car_H_28(double *state, double *unused, double *out_7554094848261592773);
|
||||
void car_h_31(double *state, double *unused, double *out_7350316286992083367);
|
||||
void car_H_31(double *state, double *unused, double *out_5240218141402074836);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_7743502251243671447);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_8632432640030731669);
|
||||
void car_H_mod_fun(double *state, double *out_6955983744751080330);
|
||||
void car_f_fun(double *state, double dt, double *out_1057256165937700131);
|
||||
void car_F_fun(double *state, double dt, double *out_2042254547319901591);
|
||||
void car_h_25(double *state, double *unused, double *out_9038137973563738704);
|
||||
void car_H_25(double *state, double *unused, double *out_6219790086191979796);
|
||||
void car_h_24(double *state, double *unused, double *out_8933773235415263694);
|
||||
void car_H_24(double *state, double *unused, double *out_2661864487389656660);
|
||||
void car_h_30(double *state, double *unused, double *out_1353345261140371077);
|
||||
void car_H_30(double *state, double *unused, double *out_3701457127684731169);
|
||||
void car_h_26(double *state, double *unused, double *out_3791544801975276460);
|
||||
void car_H_26(double *state, double *unused, double *out_8485450668643515596);
|
||||
void car_h_27(double *state, double *unused, double *out_4406932832484967267);
|
||||
void car_H_27(double *state, double *unused, double *out_5876220439485156080);
|
||||
void car_h_29(double *state, double *unused, double *out_6276545984403911908);
|
||||
void car_H_29(double *state, double *unused, double *out_3191225783370338985);
|
||||
void car_h_28(double *state, double *unused, double *out_7011338153293664918);
|
||||
void car_H_28(double *state, double *unused, double *out_8273624800439869559);
|
||||
void car_h_31(double *state, double *unused, double *out_8647698245415467134);
|
||||
void car_H_31(double *state, double *unused, double *out_7859242566410164120);
|
||||
void car_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);
|
||||
|
||||
@@ -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_2351947283659598660) {
|
||||
out_2351947283659598660[0] = delta_x[0] + nom_x[0];
|
||||
out_2351947283659598660[1] = delta_x[1] + nom_x[1];
|
||||
out_2351947283659598660[2] = delta_x[2] + nom_x[2];
|
||||
out_2351947283659598660[3] = delta_x[3] + nom_x[3];
|
||||
out_2351947283659598660[4] = delta_x[4] + nom_x[4];
|
||||
out_2351947283659598660[5] = delta_x[5] + nom_x[5];
|
||||
out_2351947283659598660[6] = delta_x[6] + nom_x[6];
|
||||
out_2351947283659598660[7] = delta_x[7] + nom_x[7];
|
||||
out_2351947283659598660[8] = delta_x[8] + nom_x[8];
|
||||
out_2351947283659598660[9] = delta_x[9] + nom_x[9];
|
||||
out_2351947283659598660[10] = delta_x[10] + nom_x[10];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
|
||||
out_1734633203049768488[0] = delta_x[0] + nom_x[0];
|
||||
out_1734633203049768488[1] = delta_x[1] + nom_x[1];
|
||||
out_1734633203049768488[2] = delta_x[2] + nom_x[2];
|
||||
out_1734633203049768488[3] = delta_x[3] + nom_x[3];
|
||||
out_1734633203049768488[4] = delta_x[4] + nom_x[4];
|
||||
out_1734633203049768488[5] = delta_x[5] + nom_x[5];
|
||||
out_1734633203049768488[6] = delta_x[6] + nom_x[6];
|
||||
out_1734633203049768488[7] = delta_x[7] + nom_x[7];
|
||||
out_1734633203049768488[8] = delta_x[8] + nom_x[8];
|
||||
out_1734633203049768488[9] = delta_x[9] + nom_x[9];
|
||||
out_1734633203049768488[10] = delta_x[10] + nom_x[10];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
|
||||
out_8539310053726918653[0] = -nom_x[0] + true_x[0];
|
||||
out_8539310053726918653[1] = -nom_x[1] + true_x[1];
|
||||
out_8539310053726918653[2] = -nom_x[2] + true_x[2];
|
||||
out_8539310053726918653[3] = -nom_x[3] + true_x[3];
|
||||
out_8539310053726918653[4] = -nom_x[4] + true_x[4];
|
||||
out_8539310053726918653[5] = -nom_x[5] + true_x[5];
|
||||
out_8539310053726918653[6] = -nom_x[6] + true_x[6];
|
||||
out_8539310053726918653[7] = -nom_x[7] + true_x[7];
|
||||
out_8539310053726918653[8] = -nom_x[8] + true_x[8];
|
||||
out_8539310053726918653[9] = -nom_x[9] + true_x[9];
|
||||
out_8539310053726918653[10] = -nom_x[10] + true_x[10];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
|
||||
out_7226800202829737504[0] = -nom_x[0] + true_x[0];
|
||||
out_7226800202829737504[1] = -nom_x[1] + true_x[1];
|
||||
out_7226800202829737504[2] = -nom_x[2] + true_x[2];
|
||||
out_7226800202829737504[3] = -nom_x[3] + true_x[3];
|
||||
out_7226800202829737504[4] = -nom_x[4] + true_x[4];
|
||||
out_7226800202829737504[5] = -nom_x[5] + true_x[5];
|
||||
out_7226800202829737504[6] = -nom_x[6] + true_x[6];
|
||||
out_7226800202829737504[7] = -nom_x[7] + true_x[7];
|
||||
out_7226800202829737504[8] = -nom_x[8] + true_x[8];
|
||||
out_7226800202829737504[9] = -nom_x[9] + true_x[9];
|
||||
out_7226800202829737504[10] = -nom_x[10] + true_x[10];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_2843304307356815987) {
|
||||
out_2843304307356815987[0] = 1.0;
|
||||
out_2843304307356815987[1] = 0;
|
||||
out_2843304307356815987[2] = 0;
|
||||
out_2843304307356815987[3] = 0;
|
||||
out_2843304307356815987[4] = 0;
|
||||
out_2843304307356815987[5] = 0;
|
||||
out_2843304307356815987[6] = 0;
|
||||
out_2843304307356815987[7] = 0;
|
||||
out_2843304307356815987[8] = 0;
|
||||
out_2843304307356815987[9] = 0;
|
||||
out_2843304307356815987[10] = 0;
|
||||
out_2843304307356815987[11] = 0;
|
||||
out_2843304307356815987[12] = 1.0;
|
||||
out_2843304307356815987[13] = 0;
|
||||
out_2843304307356815987[14] = 0;
|
||||
out_2843304307356815987[15] = 0;
|
||||
out_2843304307356815987[16] = 0;
|
||||
out_2843304307356815987[17] = 0;
|
||||
out_2843304307356815987[18] = 0;
|
||||
out_2843304307356815987[19] = 0;
|
||||
out_2843304307356815987[20] = 0;
|
||||
out_2843304307356815987[21] = 0;
|
||||
out_2843304307356815987[22] = 0;
|
||||
out_2843304307356815987[23] = 0;
|
||||
out_2843304307356815987[24] = 1.0;
|
||||
out_2843304307356815987[25] = 0;
|
||||
out_2843304307356815987[26] = 0;
|
||||
out_2843304307356815987[27] = 0;
|
||||
out_2843304307356815987[28] = 0;
|
||||
out_2843304307356815987[29] = 0;
|
||||
out_2843304307356815987[30] = 0;
|
||||
out_2843304307356815987[31] = 0;
|
||||
out_2843304307356815987[32] = 0;
|
||||
out_2843304307356815987[33] = 0;
|
||||
out_2843304307356815987[34] = 0;
|
||||
out_2843304307356815987[35] = 0;
|
||||
out_2843304307356815987[36] = 1.0;
|
||||
out_2843304307356815987[37] = 0;
|
||||
out_2843304307356815987[38] = 0;
|
||||
out_2843304307356815987[39] = 0;
|
||||
out_2843304307356815987[40] = 0;
|
||||
out_2843304307356815987[41] = 0;
|
||||
out_2843304307356815987[42] = 0;
|
||||
out_2843304307356815987[43] = 0;
|
||||
out_2843304307356815987[44] = 0;
|
||||
out_2843304307356815987[45] = 0;
|
||||
out_2843304307356815987[46] = 0;
|
||||
out_2843304307356815987[47] = 0;
|
||||
out_2843304307356815987[48] = 1.0;
|
||||
out_2843304307356815987[49] = 0;
|
||||
out_2843304307356815987[50] = 0;
|
||||
out_2843304307356815987[51] = 0;
|
||||
out_2843304307356815987[52] = 0;
|
||||
out_2843304307356815987[53] = 0;
|
||||
out_2843304307356815987[54] = 0;
|
||||
out_2843304307356815987[55] = 0;
|
||||
out_2843304307356815987[56] = 0;
|
||||
out_2843304307356815987[57] = 0;
|
||||
out_2843304307356815987[58] = 0;
|
||||
out_2843304307356815987[59] = 0;
|
||||
out_2843304307356815987[60] = 1.0;
|
||||
out_2843304307356815987[61] = 0;
|
||||
out_2843304307356815987[62] = 0;
|
||||
out_2843304307356815987[63] = 0;
|
||||
out_2843304307356815987[64] = 0;
|
||||
out_2843304307356815987[65] = 0;
|
||||
out_2843304307356815987[66] = 0;
|
||||
out_2843304307356815987[67] = 0;
|
||||
out_2843304307356815987[68] = 0;
|
||||
out_2843304307356815987[69] = 0;
|
||||
out_2843304307356815987[70] = 0;
|
||||
out_2843304307356815987[71] = 0;
|
||||
out_2843304307356815987[72] = 1.0;
|
||||
out_2843304307356815987[73] = 0;
|
||||
out_2843304307356815987[74] = 0;
|
||||
out_2843304307356815987[75] = 0;
|
||||
out_2843304307356815987[76] = 0;
|
||||
out_2843304307356815987[77] = 0;
|
||||
out_2843304307356815987[78] = 0;
|
||||
out_2843304307356815987[79] = 0;
|
||||
out_2843304307356815987[80] = 0;
|
||||
out_2843304307356815987[81] = 0;
|
||||
out_2843304307356815987[82] = 0;
|
||||
out_2843304307356815987[83] = 0;
|
||||
out_2843304307356815987[84] = 1.0;
|
||||
out_2843304307356815987[85] = 0;
|
||||
out_2843304307356815987[86] = 0;
|
||||
out_2843304307356815987[87] = 0;
|
||||
out_2843304307356815987[88] = 0;
|
||||
out_2843304307356815987[89] = 0;
|
||||
out_2843304307356815987[90] = 0;
|
||||
out_2843304307356815987[91] = 0;
|
||||
out_2843304307356815987[92] = 0;
|
||||
out_2843304307356815987[93] = 0;
|
||||
out_2843304307356815987[94] = 0;
|
||||
out_2843304307356815987[95] = 0;
|
||||
out_2843304307356815987[96] = 1.0;
|
||||
out_2843304307356815987[97] = 0;
|
||||
out_2843304307356815987[98] = 0;
|
||||
out_2843304307356815987[99] = 0;
|
||||
out_2843304307356815987[100] = 0;
|
||||
out_2843304307356815987[101] = 0;
|
||||
out_2843304307356815987[102] = 0;
|
||||
out_2843304307356815987[103] = 0;
|
||||
out_2843304307356815987[104] = 0;
|
||||
out_2843304307356815987[105] = 0;
|
||||
out_2843304307356815987[106] = 0;
|
||||
out_2843304307356815987[107] = 0;
|
||||
out_2843304307356815987[108] = 1.0;
|
||||
out_2843304307356815987[109] = 0;
|
||||
out_2843304307356815987[110] = 0;
|
||||
out_2843304307356815987[111] = 0;
|
||||
out_2843304307356815987[112] = 0;
|
||||
out_2843304307356815987[113] = 0;
|
||||
out_2843304307356815987[114] = 0;
|
||||
out_2843304307356815987[115] = 0;
|
||||
out_2843304307356815987[116] = 0;
|
||||
out_2843304307356815987[117] = 0;
|
||||
out_2843304307356815987[118] = 0;
|
||||
out_2843304307356815987[119] = 0;
|
||||
out_2843304307356815987[120] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_7212715412967761026) {
|
||||
out_7212715412967761026[0] = 1.0;
|
||||
out_7212715412967761026[1] = 0;
|
||||
out_7212715412967761026[2] = 0;
|
||||
out_7212715412967761026[3] = 0;
|
||||
out_7212715412967761026[4] = 0;
|
||||
out_7212715412967761026[5] = 0;
|
||||
out_7212715412967761026[6] = 0;
|
||||
out_7212715412967761026[7] = 0;
|
||||
out_7212715412967761026[8] = 0;
|
||||
out_7212715412967761026[9] = 0;
|
||||
out_7212715412967761026[10] = 0;
|
||||
out_7212715412967761026[11] = 0;
|
||||
out_7212715412967761026[12] = 1.0;
|
||||
out_7212715412967761026[13] = 0;
|
||||
out_7212715412967761026[14] = 0;
|
||||
out_7212715412967761026[15] = 0;
|
||||
out_7212715412967761026[16] = 0;
|
||||
out_7212715412967761026[17] = 0;
|
||||
out_7212715412967761026[18] = 0;
|
||||
out_7212715412967761026[19] = 0;
|
||||
out_7212715412967761026[20] = 0;
|
||||
out_7212715412967761026[21] = 0;
|
||||
out_7212715412967761026[22] = 0;
|
||||
out_7212715412967761026[23] = 0;
|
||||
out_7212715412967761026[24] = 1.0;
|
||||
out_7212715412967761026[25] = 0;
|
||||
out_7212715412967761026[26] = 0;
|
||||
out_7212715412967761026[27] = 0;
|
||||
out_7212715412967761026[28] = 0;
|
||||
out_7212715412967761026[29] = 0;
|
||||
out_7212715412967761026[30] = 0;
|
||||
out_7212715412967761026[31] = 0;
|
||||
out_7212715412967761026[32] = 0;
|
||||
out_7212715412967761026[33] = 0;
|
||||
out_7212715412967761026[34] = 0;
|
||||
out_7212715412967761026[35] = 0;
|
||||
out_7212715412967761026[36] = 1.0;
|
||||
out_7212715412967761026[37] = 0;
|
||||
out_7212715412967761026[38] = 0;
|
||||
out_7212715412967761026[39] = 0;
|
||||
out_7212715412967761026[40] = 0;
|
||||
out_7212715412967761026[41] = 0;
|
||||
out_7212715412967761026[42] = 0;
|
||||
out_7212715412967761026[43] = 0;
|
||||
out_7212715412967761026[44] = 0;
|
||||
out_7212715412967761026[45] = 0;
|
||||
out_7212715412967761026[46] = 0;
|
||||
out_7212715412967761026[47] = 0;
|
||||
out_7212715412967761026[48] = 1.0;
|
||||
out_7212715412967761026[49] = 0;
|
||||
out_7212715412967761026[50] = 0;
|
||||
out_7212715412967761026[51] = 0;
|
||||
out_7212715412967761026[52] = 0;
|
||||
out_7212715412967761026[53] = 0;
|
||||
out_7212715412967761026[54] = 0;
|
||||
out_7212715412967761026[55] = 0;
|
||||
out_7212715412967761026[56] = 0;
|
||||
out_7212715412967761026[57] = 0;
|
||||
out_7212715412967761026[58] = 0;
|
||||
out_7212715412967761026[59] = 0;
|
||||
out_7212715412967761026[60] = 1.0;
|
||||
out_7212715412967761026[61] = 0;
|
||||
out_7212715412967761026[62] = 0;
|
||||
out_7212715412967761026[63] = 0;
|
||||
out_7212715412967761026[64] = 0;
|
||||
out_7212715412967761026[65] = 0;
|
||||
out_7212715412967761026[66] = 0;
|
||||
out_7212715412967761026[67] = 0;
|
||||
out_7212715412967761026[68] = 0;
|
||||
out_7212715412967761026[69] = 0;
|
||||
out_7212715412967761026[70] = 0;
|
||||
out_7212715412967761026[71] = 0;
|
||||
out_7212715412967761026[72] = 1.0;
|
||||
out_7212715412967761026[73] = 0;
|
||||
out_7212715412967761026[74] = 0;
|
||||
out_7212715412967761026[75] = 0;
|
||||
out_7212715412967761026[76] = 0;
|
||||
out_7212715412967761026[77] = 0;
|
||||
out_7212715412967761026[78] = 0;
|
||||
out_7212715412967761026[79] = 0;
|
||||
out_7212715412967761026[80] = 0;
|
||||
out_7212715412967761026[81] = 0;
|
||||
out_7212715412967761026[82] = 0;
|
||||
out_7212715412967761026[83] = 0;
|
||||
out_7212715412967761026[84] = 1.0;
|
||||
out_7212715412967761026[85] = 0;
|
||||
out_7212715412967761026[86] = 0;
|
||||
out_7212715412967761026[87] = 0;
|
||||
out_7212715412967761026[88] = 0;
|
||||
out_7212715412967761026[89] = 0;
|
||||
out_7212715412967761026[90] = 0;
|
||||
out_7212715412967761026[91] = 0;
|
||||
out_7212715412967761026[92] = 0;
|
||||
out_7212715412967761026[93] = 0;
|
||||
out_7212715412967761026[94] = 0;
|
||||
out_7212715412967761026[95] = 0;
|
||||
out_7212715412967761026[96] = 1.0;
|
||||
out_7212715412967761026[97] = 0;
|
||||
out_7212715412967761026[98] = 0;
|
||||
out_7212715412967761026[99] = 0;
|
||||
out_7212715412967761026[100] = 0;
|
||||
out_7212715412967761026[101] = 0;
|
||||
out_7212715412967761026[102] = 0;
|
||||
out_7212715412967761026[103] = 0;
|
||||
out_7212715412967761026[104] = 0;
|
||||
out_7212715412967761026[105] = 0;
|
||||
out_7212715412967761026[106] = 0;
|
||||
out_7212715412967761026[107] = 0;
|
||||
out_7212715412967761026[108] = 1.0;
|
||||
out_7212715412967761026[109] = 0;
|
||||
out_7212715412967761026[110] = 0;
|
||||
out_7212715412967761026[111] = 0;
|
||||
out_7212715412967761026[112] = 0;
|
||||
out_7212715412967761026[113] = 0;
|
||||
out_7212715412967761026[114] = 0;
|
||||
out_7212715412967761026[115] = 0;
|
||||
out_7212715412967761026[116] = 0;
|
||||
out_7212715412967761026[117] = 0;
|
||||
out_7212715412967761026[118] = 0;
|
||||
out_7212715412967761026[119] = 0;
|
||||
out_7212715412967761026[120] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_774769938980785647) {
|
||||
out_774769938980785647[0] = dt*state[3] + state[0];
|
||||
out_774769938980785647[1] = dt*state[4] + state[1];
|
||||
out_774769938980785647[2] = dt*state[5] + state[2];
|
||||
out_774769938980785647[3] = state[3];
|
||||
out_774769938980785647[4] = state[4];
|
||||
out_774769938980785647[5] = state[5];
|
||||
out_774769938980785647[6] = dt*state[7] + state[6];
|
||||
out_774769938980785647[7] = dt*state[8] + state[7];
|
||||
out_774769938980785647[8] = state[8];
|
||||
out_774769938980785647[9] = state[9];
|
||||
out_774769938980785647[10] = state[10];
|
||||
void f_fun(double *state, double dt, double *out_4622219999608348212) {
|
||||
out_4622219999608348212[0] = dt*state[3] + state[0];
|
||||
out_4622219999608348212[1] = dt*state[4] + state[1];
|
||||
out_4622219999608348212[2] = dt*state[5] + state[2];
|
||||
out_4622219999608348212[3] = state[3];
|
||||
out_4622219999608348212[4] = state[4];
|
||||
out_4622219999608348212[5] = state[5];
|
||||
out_4622219999608348212[6] = dt*state[7] + state[6];
|
||||
out_4622219999608348212[7] = dt*state[8] + state[7];
|
||||
out_4622219999608348212[8] = state[8];
|
||||
out_4622219999608348212[9] = state[9];
|
||||
out_4622219999608348212[10] = state[10];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_4467768114936024642) {
|
||||
out_4467768114936024642[0] = 1;
|
||||
out_4467768114936024642[1] = 0;
|
||||
out_4467768114936024642[2] = 0;
|
||||
out_4467768114936024642[3] = dt;
|
||||
out_4467768114936024642[4] = 0;
|
||||
out_4467768114936024642[5] = 0;
|
||||
out_4467768114936024642[6] = 0;
|
||||
out_4467768114936024642[7] = 0;
|
||||
out_4467768114936024642[8] = 0;
|
||||
out_4467768114936024642[9] = 0;
|
||||
out_4467768114936024642[10] = 0;
|
||||
out_4467768114936024642[11] = 0;
|
||||
out_4467768114936024642[12] = 1;
|
||||
out_4467768114936024642[13] = 0;
|
||||
out_4467768114936024642[14] = 0;
|
||||
out_4467768114936024642[15] = dt;
|
||||
out_4467768114936024642[16] = 0;
|
||||
out_4467768114936024642[17] = 0;
|
||||
out_4467768114936024642[18] = 0;
|
||||
out_4467768114936024642[19] = 0;
|
||||
out_4467768114936024642[20] = 0;
|
||||
out_4467768114936024642[21] = 0;
|
||||
out_4467768114936024642[22] = 0;
|
||||
out_4467768114936024642[23] = 0;
|
||||
out_4467768114936024642[24] = 1;
|
||||
out_4467768114936024642[25] = 0;
|
||||
out_4467768114936024642[26] = 0;
|
||||
out_4467768114936024642[27] = dt;
|
||||
out_4467768114936024642[28] = 0;
|
||||
out_4467768114936024642[29] = 0;
|
||||
out_4467768114936024642[30] = 0;
|
||||
out_4467768114936024642[31] = 0;
|
||||
out_4467768114936024642[32] = 0;
|
||||
out_4467768114936024642[33] = 0;
|
||||
out_4467768114936024642[34] = 0;
|
||||
out_4467768114936024642[35] = 0;
|
||||
out_4467768114936024642[36] = 1;
|
||||
out_4467768114936024642[37] = 0;
|
||||
out_4467768114936024642[38] = 0;
|
||||
out_4467768114936024642[39] = 0;
|
||||
out_4467768114936024642[40] = 0;
|
||||
out_4467768114936024642[41] = 0;
|
||||
out_4467768114936024642[42] = 0;
|
||||
out_4467768114936024642[43] = 0;
|
||||
out_4467768114936024642[44] = 0;
|
||||
out_4467768114936024642[45] = 0;
|
||||
out_4467768114936024642[46] = 0;
|
||||
out_4467768114936024642[47] = 0;
|
||||
out_4467768114936024642[48] = 1;
|
||||
out_4467768114936024642[49] = 0;
|
||||
out_4467768114936024642[50] = 0;
|
||||
out_4467768114936024642[51] = 0;
|
||||
out_4467768114936024642[52] = 0;
|
||||
out_4467768114936024642[53] = 0;
|
||||
out_4467768114936024642[54] = 0;
|
||||
out_4467768114936024642[55] = 0;
|
||||
out_4467768114936024642[56] = 0;
|
||||
out_4467768114936024642[57] = 0;
|
||||
out_4467768114936024642[58] = 0;
|
||||
out_4467768114936024642[59] = 0;
|
||||
out_4467768114936024642[60] = 1;
|
||||
out_4467768114936024642[61] = 0;
|
||||
out_4467768114936024642[62] = 0;
|
||||
out_4467768114936024642[63] = 0;
|
||||
out_4467768114936024642[64] = 0;
|
||||
out_4467768114936024642[65] = 0;
|
||||
out_4467768114936024642[66] = 0;
|
||||
out_4467768114936024642[67] = 0;
|
||||
out_4467768114936024642[68] = 0;
|
||||
out_4467768114936024642[69] = 0;
|
||||
out_4467768114936024642[70] = 0;
|
||||
out_4467768114936024642[71] = 0;
|
||||
out_4467768114936024642[72] = 1;
|
||||
out_4467768114936024642[73] = dt;
|
||||
out_4467768114936024642[74] = 0;
|
||||
out_4467768114936024642[75] = 0;
|
||||
out_4467768114936024642[76] = 0;
|
||||
out_4467768114936024642[77] = 0;
|
||||
out_4467768114936024642[78] = 0;
|
||||
out_4467768114936024642[79] = 0;
|
||||
out_4467768114936024642[80] = 0;
|
||||
out_4467768114936024642[81] = 0;
|
||||
out_4467768114936024642[82] = 0;
|
||||
out_4467768114936024642[83] = 0;
|
||||
out_4467768114936024642[84] = 1;
|
||||
out_4467768114936024642[85] = dt;
|
||||
out_4467768114936024642[86] = 0;
|
||||
out_4467768114936024642[87] = 0;
|
||||
out_4467768114936024642[88] = 0;
|
||||
out_4467768114936024642[89] = 0;
|
||||
out_4467768114936024642[90] = 0;
|
||||
out_4467768114936024642[91] = 0;
|
||||
out_4467768114936024642[92] = 0;
|
||||
out_4467768114936024642[93] = 0;
|
||||
out_4467768114936024642[94] = 0;
|
||||
out_4467768114936024642[95] = 0;
|
||||
out_4467768114936024642[96] = 1;
|
||||
out_4467768114936024642[97] = 0;
|
||||
out_4467768114936024642[98] = 0;
|
||||
out_4467768114936024642[99] = 0;
|
||||
out_4467768114936024642[100] = 0;
|
||||
out_4467768114936024642[101] = 0;
|
||||
out_4467768114936024642[102] = 0;
|
||||
out_4467768114936024642[103] = 0;
|
||||
out_4467768114936024642[104] = 0;
|
||||
out_4467768114936024642[105] = 0;
|
||||
out_4467768114936024642[106] = 0;
|
||||
out_4467768114936024642[107] = 0;
|
||||
out_4467768114936024642[108] = 1;
|
||||
out_4467768114936024642[109] = 0;
|
||||
out_4467768114936024642[110] = 0;
|
||||
out_4467768114936024642[111] = 0;
|
||||
out_4467768114936024642[112] = 0;
|
||||
out_4467768114936024642[113] = 0;
|
||||
out_4467768114936024642[114] = 0;
|
||||
out_4467768114936024642[115] = 0;
|
||||
out_4467768114936024642[116] = 0;
|
||||
out_4467768114936024642[117] = 0;
|
||||
out_4467768114936024642[118] = 0;
|
||||
out_4467768114936024642[119] = 0;
|
||||
out_4467768114936024642[120] = 1;
|
||||
void F_fun(double *state, double dt, double *out_1489675256084431222) {
|
||||
out_1489675256084431222[0] = 1;
|
||||
out_1489675256084431222[1] = 0;
|
||||
out_1489675256084431222[2] = 0;
|
||||
out_1489675256084431222[3] = dt;
|
||||
out_1489675256084431222[4] = 0;
|
||||
out_1489675256084431222[5] = 0;
|
||||
out_1489675256084431222[6] = 0;
|
||||
out_1489675256084431222[7] = 0;
|
||||
out_1489675256084431222[8] = 0;
|
||||
out_1489675256084431222[9] = 0;
|
||||
out_1489675256084431222[10] = 0;
|
||||
out_1489675256084431222[11] = 0;
|
||||
out_1489675256084431222[12] = 1;
|
||||
out_1489675256084431222[13] = 0;
|
||||
out_1489675256084431222[14] = 0;
|
||||
out_1489675256084431222[15] = dt;
|
||||
out_1489675256084431222[16] = 0;
|
||||
out_1489675256084431222[17] = 0;
|
||||
out_1489675256084431222[18] = 0;
|
||||
out_1489675256084431222[19] = 0;
|
||||
out_1489675256084431222[20] = 0;
|
||||
out_1489675256084431222[21] = 0;
|
||||
out_1489675256084431222[22] = 0;
|
||||
out_1489675256084431222[23] = 0;
|
||||
out_1489675256084431222[24] = 1;
|
||||
out_1489675256084431222[25] = 0;
|
||||
out_1489675256084431222[26] = 0;
|
||||
out_1489675256084431222[27] = dt;
|
||||
out_1489675256084431222[28] = 0;
|
||||
out_1489675256084431222[29] = 0;
|
||||
out_1489675256084431222[30] = 0;
|
||||
out_1489675256084431222[31] = 0;
|
||||
out_1489675256084431222[32] = 0;
|
||||
out_1489675256084431222[33] = 0;
|
||||
out_1489675256084431222[34] = 0;
|
||||
out_1489675256084431222[35] = 0;
|
||||
out_1489675256084431222[36] = 1;
|
||||
out_1489675256084431222[37] = 0;
|
||||
out_1489675256084431222[38] = 0;
|
||||
out_1489675256084431222[39] = 0;
|
||||
out_1489675256084431222[40] = 0;
|
||||
out_1489675256084431222[41] = 0;
|
||||
out_1489675256084431222[42] = 0;
|
||||
out_1489675256084431222[43] = 0;
|
||||
out_1489675256084431222[44] = 0;
|
||||
out_1489675256084431222[45] = 0;
|
||||
out_1489675256084431222[46] = 0;
|
||||
out_1489675256084431222[47] = 0;
|
||||
out_1489675256084431222[48] = 1;
|
||||
out_1489675256084431222[49] = 0;
|
||||
out_1489675256084431222[50] = 0;
|
||||
out_1489675256084431222[51] = 0;
|
||||
out_1489675256084431222[52] = 0;
|
||||
out_1489675256084431222[53] = 0;
|
||||
out_1489675256084431222[54] = 0;
|
||||
out_1489675256084431222[55] = 0;
|
||||
out_1489675256084431222[56] = 0;
|
||||
out_1489675256084431222[57] = 0;
|
||||
out_1489675256084431222[58] = 0;
|
||||
out_1489675256084431222[59] = 0;
|
||||
out_1489675256084431222[60] = 1;
|
||||
out_1489675256084431222[61] = 0;
|
||||
out_1489675256084431222[62] = 0;
|
||||
out_1489675256084431222[63] = 0;
|
||||
out_1489675256084431222[64] = 0;
|
||||
out_1489675256084431222[65] = 0;
|
||||
out_1489675256084431222[66] = 0;
|
||||
out_1489675256084431222[67] = 0;
|
||||
out_1489675256084431222[68] = 0;
|
||||
out_1489675256084431222[69] = 0;
|
||||
out_1489675256084431222[70] = 0;
|
||||
out_1489675256084431222[71] = 0;
|
||||
out_1489675256084431222[72] = 1;
|
||||
out_1489675256084431222[73] = dt;
|
||||
out_1489675256084431222[74] = 0;
|
||||
out_1489675256084431222[75] = 0;
|
||||
out_1489675256084431222[76] = 0;
|
||||
out_1489675256084431222[77] = 0;
|
||||
out_1489675256084431222[78] = 0;
|
||||
out_1489675256084431222[79] = 0;
|
||||
out_1489675256084431222[80] = 0;
|
||||
out_1489675256084431222[81] = 0;
|
||||
out_1489675256084431222[82] = 0;
|
||||
out_1489675256084431222[83] = 0;
|
||||
out_1489675256084431222[84] = 1;
|
||||
out_1489675256084431222[85] = dt;
|
||||
out_1489675256084431222[86] = 0;
|
||||
out_1489675256084431222[87] = 0;
|
||||
out_1489675256084431222[88] = 0;
|
||||
out_1489675256084431222[89] = 0;
|
||||
out_1489675256084431222[90] = 0;
|
||||
out_1489675256084431222[91] = 0;
|
||||
out_1489675256084431222[92] = 0;
|
||||
out_1489675256084431222[93] = 0;
|
||||
out_1489675256084431222[94] = 0;
|
||||
out_1489675256084431222[95] = 0;
|
||||
out_1489675256084431222[96] = 1;
|
||||
out_1489675256084431222[97] = 0;
|
||||
out_1489675256084431222[98] = 0;
|
||||
out_1489675256084431222[99] = 0;
|
||||
out_1489675256084431222[100] = 0;
|
||||
out_1489675256084431222[101] = 0;
|
||||
out_1489675256084431222[102] = 0;
|
||||
out_1489675256084431222[103] = 0;
|
||||
out_1489675256084431222[104] = 0;
|
||||
out_1489675256084431222[105] = 0;
|
||||
out_1489675256084431222[106] = 0;
|
||||
out_1489675256084431222[107] = 0;
|
||||
out_1489675256084431222[108] = 1;
|
||||
out_1489675256084431222[109] = 0;
|
||||
out_1489675256084431222[110] = 0;
|
||||
out_1489675256084431222[111] = 0;
|
||||
out_1489675256084431222[112] = 0;
|
||||
out_1489675256084431222[113] = 0;
|
||||
out_1489675256084431222[114] = 0;
|
||||
out_1489675256084431222[115] = 0;
|
||||
out_1489675256084431222[116] = 0;
|
||||
out_1489675256084431222[117] = 0;
|
||||
out_1489675256084431222[118] = 0;
|
||||
out_1489675256084431222[119] = 0;
|
||||
out_1489675256084431222[120] = 1;
|
||||
}
|
||||
void h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
|
||||
out_8206867657584711155[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_7567908033892933921) {
|
||||
out_7567908033892933921[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + state[6];
|
||||
}
|
||||
void H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
|
||||
out_2748205046550560630[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_2748205046550560630[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_2748205046550560630[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_2748205046550560630[3] = 0;
|
||||
out_2748205046550560630[4] = 0;
|
||||
out_2748205046550560630[5] = 0;
|
||||
out_2748205046550560630[6] = 1;
|
||||
out_2748205046550560630[7] = 0;
|
||||
out_2748205046550560630[8] = 0;
|
||||
out_2748205046550560630[9] = 0;
|
||||
out_2748205046550560630[10] = 0;
|
||||
void H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
|
||||
out_7455993273502557413[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_7455993273502557413[3] = 0;
|
||||
out_7455993273502557413[4] = 0;
|
||||
out_7455993273502557413[5] = 0;
|
||||
out_7455993273502557413[6] = 1;
|
||||
out_7455993273502557413[7] = 0;
|
||||
out_7455993273502557413[8] = 0;
|
||||
out_7455993273502557413[9] = 0;
|
||||
out_7455993273502557413[10] = 0;
|
||||
}
|
||||
void h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
|
||||
out_3144072620339266513[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_3841468768416351937) {
|
||||
out_3841468768416351937[0] = sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2)) + sat_pos[3]*state[10] + state[6] + state[9];
|
||||
}
|
||||
void H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
|
||||
out_8303647801952025828[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_8303647801952025828[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_8303647801952025828[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_8303647801952025828[3] = 0;
|
||||
out_8303647801952025828[4] = 0;
|
||||
out_8303647801952025828[5] = 0;
|
||||
out_8303647801952025828[6] = 1;
|
||||
out_8303647801952025828[7] = 0;
|
||||
out_8303647801952025828[8] = 0;
|
||||
out_8303647801952025828[9] = 1;
|
||||
out_8303647801952025828[10] = sat_pos[3];
|
||||
void H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
|
||||
out_5623941665124898301[0] = (-sat_pos[0] + state[0])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[1] = (-sat_pos[1] + state[1])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[2] = (-sat_pos[2] + state[2])/sqrt(pow(-sat_pos[0] + state[0], 2) + pow(-sat_pos[1] + state[1], 2) + pow(-sat_pos[2] + state[2], 2));
|
||||
out_5623941665124898301[3] = 0;
|
||||
out_5623941665124898301[4] = 0;
|
||||
out_5623941665124898301[5] = 0;
|
||||
out_5623941665124898301[6] = 1;
|
||||
out_5623941665124898301[7] = 0;
|
||||
out_5623941665124898301[8] = 0;
|
||||
out_5623941665124898301[9] = 1;
|
||||
out_5623941665124898301[10] = sat_pos[3];
|
||||
}
|
||||
void h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
out_7730590468597126339[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_1211039377275791210) {
|
||||
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
}
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
out_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[6] = 0;
|
||||
out_1168235388167943403[7] = 1;
|
||||
out_1168235388167943403[8] = 0;
|
||||
out_1168235388167943403[9] = 0;
|
||||
out_1168235388167943403[10] = 0;
|
||||
void H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[6] = 0;
|
||||
out_2159917500592958723[7] = 1;
|
||||
out_2159917500592958723[8] = 0;
|
||||
out_2159917500592958723[9] = 0;
|
||||
out_2159917500592958723[10] = 0;
|
||||
}
|
||||
void h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
out_7730590468597126339[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_1211039377275791210) {
|
||||
out_1211039377275791210[0] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + (sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2)) + state[7];
|
||||
}
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
out_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[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_1168235388167943403[6] = 0;
|
||||
out_1168235388167943403[7] = 1;
|
||||
out_1168235388167943403[8] = 0;
|
||||
out_1168235388167943403[9] = 0;
|
||||
out_1168235388167943403[10] = 0;
|
||||
void H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
out_2159917500592958723[0] = pow(sat_pos_vel[0] - state[0], 2)*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[3] - state[3])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[1] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[1] - state[1])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[1] - state[1], 2)*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[4] - state[4])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[2] = (sat_pos_vel[0] - state[0])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[3] - state[3])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + (sat_pos_vel[1] - state[1])*(sat_pos_vel[2] - state[2])*(sat_pos_vel[4] - state[4])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) + pow(sat_pos_vel[2] - state[2], 2)*(sat_pos_vel[5] - state[5])/pow(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2), 3.0/2.0) - (sat_pos_vel[5] - state[5])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[3] = -(sat_pos_vel[0] - state[0])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[4] = -(sat_pos_vel[1] - state[1])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[5] = -(sat_pos_vel[2] - state[2])/sqrt(pow(sat_pos_vel[0] - state[0], 2) + pow(sat_pos_vel[1] - state[1], 2) + pow(sat_pos_vel[2] - state[2], 2));
|
||||
out_2159917500592958723[6] = 0;
|
||||
out_2159917500592958723[7] = 1;
|
||||
out_2159917500592958723[8] = 0;
|
||||
out_2159917500592958723[9] = 0;
|
||||
out_2159917500592958723[10] = 0;
|
||||
}
|
||||
#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_2351947283659598660) {
|
||||
err_fun(nom_x, delta_x, out_2351947283659598660);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488) {
|
||||
err_fun(nom_x, delta_x, out_1734633203049768488);
|
||||
}
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653) {
|
||||
inv_err_fun(nom_x, true_x, out_8539310053726918653);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504) {
|
||||
inv_err_fun(nom_x, true_x, out_7226800202829737504);
|
||||
}
|
||||
void gnss_H_mod_fun(double *state, double *out_2843304307356815987) {
|
||||
H_mod_fun(state, out_2843304307356815987);
|
||||
void gnss_H_mod_fun(double *state, double *out_7212715412967761026) {
|
||||
H_mod_fun(state, out_7212715412967761026);
|
||||
}
|
||||
void gnss_f_fun(double *state, double dt, double *out_774769938980785647) {
|
||||
f_fun(state, dt, out_774769938980785647);
|
||||
void gnss_f_fun(double *state, double dt, double *out_4622219999608348212) {
|
||||
f_fun(state, dt, out_4622219999608348212);
|
||||
}
|
||||
void gnss_F_fun(double *state, double dt, double *out_4467768114936024642) {
|
||||
F_fun(state, dt, out_4467768114936024642);
|
||||
void gnss_F_fun(double *state, double dt, double *out_1489675256084431222) {
|
||||
F_fun(state, dt, out_1489675256084431222);
|
||||
}
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155) {
|
||||
h_6(state, sat_pos, out_8206867657584711155);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_7567908033892933921) {
|
||||
h_6(state, sat_pos, out_7567908033892933921);
|
||||
}
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630) {
|
||||
H_6(state, sat_pos, out_2748205046550560630);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_7455993273502557413) {
|
||||
H_6(state, sat_pos, out_7455993273502557413);
|
||||
}
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513) {
|
||||
h_20(state, sat_pos, out_3144072620339266513);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3841468768416351937) {
|
||||
h_20(state, sat_pos, out_3841468768416351937);
|
||||
}
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828) {
|
||||
H_20(state, sat_pos, out_8303647801952025828);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_5623941665124898301) {
|
||||
H_20(state, sat_pos, out_5623941665124898301);
|
||||
}
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
h_7(state, sat_pos_vel, out_7730590468597126339);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
h_7(state, sat_pos_vel, out_1211039377275791210);
|
||||
}
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
H_7(state, sat_pos_vel, out_1168235388167943403);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
H_7(state, sat_pos_vel, out_2159917500592958723);
|
||||
}
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339) {
|
||||
h_21(state, sat_pos_vel, out_7730590468597126339);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210) {
|
||||
h_21(state, sat_pos_vel, out_1211039377275791210);
|
||||
}
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403) {
|
||||
H_21(state, sat_pos_vel, out_1168235388167943403);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723) {
|
||||
H_21(state, sat_pos_vel, out_2159917500592958723);
|
||||
}
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -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_2351947283659598660);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_8539310053726918653);
|
||||
void gnss_H_mod_fun(double *state, double *out_2843304307356815987);
|
||||
void gnss_f_fun(double *state, double dt, double *out_774769938980785647);
|
||||
void gnss_F_fun(double *state, double dt, double *out_4467768114936024642);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_8206867657584711155);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_2748205046550560630);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3144072620339266513);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_8303647801952025828);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_7730590468597126339);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_1168235388167943403);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_7730590468597126339);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_1168235388167943403);
|
||||
void gnss_err_fun(double *nom_x, double *delta_x, double *out_1734633203049768488);
|
||||
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_7226800202829737504);
|
||||
void gnss_H_mod_fun(double *state, double *out_7212715412967761026);
|
||||
void gnss_f_fun(double *state, double dt, double *out_4622219999608348212);
|
||||
void gnss_F_fun(double *state, double dt, double *out_1489675256084431222);
|
||||
void gnss_h_6(double *state, double *sat_pos, double *out_7567908033892933921);
|
||||
void gnss_H_6(double *state, double *sat_pos, double *out_7455993273502557413);
|
||||
void gnss_h_20(double *state, double *sat_pos, double *out_3841468768416351937);
|
||||
void gnss_H_20(double *state, double *sat_pos, double *out_5623941665124898301);
|
||||
void gnss_h_7(double *state, double *sat_pos_vel, double *out_1211039377275791210);
|
||||
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2159917500592958723);
|
||||
void gnss_h_21(double *state, double *sat_pos_vel, double *out_1211039377275791210);
|
||||
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2159917500592958723);
|
||||
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -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_8687680363762493539);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_8136490426412747881);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_1524655933590429885);
|
||||
void live_H_mod_fun(double *state, double *out_7453067886472422609);
|
||||
void live_f_fun(double *state, double dt, double *out_3547963935686363060);
|
||||
void live_F_fun(double *state, double dt, double *out_3254137990445528781);
|
||||
void live_h_4(double *state, double *unused, double *out_4410787759120580449);
|
||||
void live_H_4(double *state, double *unused, double *out_5512660098689845475);
|
||||
void live_h_9(double *state, double *unused, double *out_4264504783842197092);
|
||||
void live_H_9(double *state, double *unused, double *out_5271470452060254830);
|
||||
void live_h_10(double *state, double *unused, double *out_1276705416788479855);
|
||||
void live_H_10(double *state, double *unused, double *out_6589591562994162559);
|
||||
void live_h_12(double *state, double *unused, double *out_4489758644873872208);
|
||||
void live_H_12(double *state, double *unused, double *out_493203690657883680);
|
||||
void live_h_35(double *state, double *unused, double *out_8696122944070004010);
|
||||
void live_H_35(double *state, double *unused, double *out_2252359341667130029);
|
||||
void live_h_32(double *state, double *unused, double *out_2592146495080937647);
|
||||
void live_H_32(double *state, double *unused, double *out_8522971196901590084);
|
||||
void live_h_13(double *state, double *unused, double *out_5320318911182793356);
|
||||
void live_H_13(double *state, double *unused, double *out_6260235854627074248);
|
||||
void live_h_14(double *state, double *unused, double *out_4264504783842197092);
|
||||
void live_H_14(double *state, double *unused, double *out_5271470452060254830);
|
||||
void live_h_33(double *state, double *unused, double *out_57093419937807770);
|
||||
void live_H_33(double *state, double *unused, double *out_5402916346305987633);
|
||||
void live_H(double *in_vec, double *out_4500389823279432985);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_1913535744177293566);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_2851117358798162907);
|
||||
void live_H_mod_fun(double *state, double *out_2697157995522224710);
|
||||
void live_f_fun(double *state, double dt, double *out_258969813900144001);
|
||||
void live_F_fun(double *state, double dt, double *out_3982048512447115253);
|
||||
void live_h_4(double *state, double *unused, double *out_3062002944620705961);
|
||||
void live_H_4(double *state, double *unused, double *out_3011098296458653357);
|
||||
void live_h_9(double *state, double *unused, double *out_5633073678966500073);
|
||||
void live_H_9(double *state, double *unused, double *out_3252287943088244002);
|
||||
void live_h_10(double *state, double *unused, double *out_4956664653644560279);
|
||||
void live_H_10(double *state, double *unused, double *out_4513091182693755627);
|
||||
void live_h_12(double *state, double *unused, double *out_5780595779529914179);
|
||||
void live_H_12(double *state, double *unused, double *out_3632197321506247024);
|
||||
void live_h_35(double *state, double *unused, double *out_190113666816480701);
|
||||
void live_H_35(double *state, double *unused, double *out_6377760353831260733);
|
||||
void live_h_32(double *state, double *unused, double *out_1962437249848893520);
|
||||
void live_H_32(double *state, double *unused, double *out_6267679347670678325);
|
||||
void live_h_13(double *state, double *unused, double *out_1422098576590259012);
|
||||
void live_H_13(double *state, double *unused, double *out_979728642438066826);
|
||||
void live_h_14(double *state, double *unused, double *out_5633073678966500073);
|
||||
void live_H_14(double *state, double *unused, double *out_3252287943088244002);
|
||||
void live_h_33(double *state, double *unused, double *out_4297372334290366217);
|
||||
void live_H_33(double *state, double *unused, double *out_8918426715239433279);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -92,7 +92,7 @@ class ParamsLearner:
|
||||
self.speed = msg.vEgo
|
||||
|
||||
in_linear_region = abs(self.steering_angle) < 45 or not self.steering_pressed
|
||||
self.active = self.speed > 5 and in_linear_region
|
||||
self.active = self.speed > 1 and in_linear_region
|
||||
|
||||
if self.active:
|
||||
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[math.radians(msg.steeringAngleDeg)]]))
|
||||
|
||||
@@ -16,7 +16,9 @@ from selfdrive.controls.lib.vehicle_model import ACCELERATION_DUE_TO_GRAVITY
|
||||
HISTORY = 5 # secs
|
||||
POINTS_PER_BUCKET = 1500
|
||||
MIN_POINTS_TOTAL = 4000
|
||||
MIN_POINTS_TOTAL_QLOG = 600
|
||||
FIT_POINTS_TOTAL = 2000
|
||||
FIT_POINTS_TOTAL_QLOG = 600
|
||||
MIN_VEL = 15 # m/s
|
||||
FRICTION_FACTOR = 1.5 # ~85% of data coverage
|
||||
FACTOR_SANITY = 0.3
|
||||
@@ -26,7 +28,7 @@ MIN_FILTER_DECAY = 50
|
||||
MAX_FILTER_DECAY = 250
|
||||
LAT_ACC_THRESHOLD = 1
|
||||
STEER_BUCKET_BOUNDS = [(-0.5, -0.3), (-0.3, -0.2), (-0.2, -0.1), (-0.1, 0), (0, 0.1), (0.1, 0.2), (0.2, 0.3), (0.3, 0.5)]
|
||||
MIN_BUCKET_POINTS = [100, 300, 500, 500, 500, 500, 300, 100]
|
||||
MIN_BUCKET_POINTS = np.array([100, 300, 500, 500, 500, 500, 300, 100])
|
||||
MAX_RESETS = 5.0
|
||||
MAX_INVALID_THRESHOLD = 10
|
||||
MIN_ENGAGE_BUFFER = 2 # secs
|
||||
@@ -58,10 +60,11 @@ class NPQueue:
|
||||
|
||||
|
||||
class PointBuckets:
|
||||
def __init__(self, x_bounds, min_points):
|
||||
def __init__(self, x_bounds, min_points, min_points_total):
|
||||
self.x_bounds = x_bounds
|
||||
self.buckets = {bounds: NPQueue(maxlen=POINTS_PER_BUCKET, rowsize=3) for bounds in x_bounds}
|
||||
self.buckets_min_points = {bounds: min_point for bounds, min_point in zip(x_bounds, min_points)}
|
||||
self.min_points_total = min_points_total
|
||||
|
||||
def bucket_lengths(self):
|
||||
return [len(v) for v in self.buckets.values()]
|
||||
@@ -70,7 +73,7 @@ class PointBuckets:
|
||||
return sum(self.bucket_lengths())
|
||||
|
||||
def is_valid(self):
|
||||
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= MIN_POINTS_TOTAL)
|
||||
return all(len(v) >= min_pts for v, min_pts in zip(self.buckets.values(), self.buckets_min_points.values())) and (self.__len__() >= self.min_points_total)
|
||||
|
||||
def add_point(self, x, y):
|
||||
for bound_min, bound_max in self.x_bounds:
|
||||
@@ -90,9 +93,17 @@ class PointBuckets:
|
||||
|
||||
|
||||
class TorqueEstimator:
|
||||
def __init__(self, CP):
|
||||
def __init__(self, CP, decimated=False):
|
||||
self.hist_len = int(HISTORY / DT_MDL)
|
||||
self.lag = CP.steerActuatorDelay + .2 # from controlsd
|
||||
if decimated:
|
||||
self.min_bucket_points = MIN_BUCKET_POINTS / 10
|
||||
self.min_points_total = MIN_POINTS_TOTAL_QLOG
|
||||
self.fit_points = FIT_POINTS_TOTAL_QLOG
|
||||
else:
|
||||
self.min_bucket_points = MIN_BUCKET_POINTS
|
||||
self.min_points_total = MIN_POINTS_TOTAL
|
||||
self.fit_points = FIT_POINTS_TOTAL
|
||||
|
||||
self.offline_friction = 0.0
|
||||
self.offline_latAccelFactor = 0.0
|
||||
@@ -157,10 +168,10 @@ class TorqueEstimator:
|
||||
self.invalid_values_tracker = 0.0
|
||||
self.decay = MIN_FILTER_DECAY
|
||||
self.raw_points = defaultdict(lambda: deque(maxlen=self.hist_len))
|
||||
self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=MIN_BUCKET_POINTS)
|
||||
self.filtered_points = PointBuckets(x_bounds=STEER_BUCKET_BOUNDS, min_points=self.min_bucket_points, min_points_total=self.min_points_total)
|
||||
|
||||
def estimate_params(self):
|
||||
points = self.filtered_points.get_points(FIT_POINTS_TOTAL)
|
||||
points = self.filtered_points.get_points(self.fit_points)
|
||||
# total least square solution as both x and y are noisy observations
|
||||
# this is empirically the slope of the hysteresis parallelogram as opposed to the line through the diagonals
|
||||
try:
|
||||
|
||||
Binary file not shown.
Reference in New Issue
Block a user