mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-08-23 17:23:51 +08:00
Code Issues Packages Projects Releases Wiki Activity

dragonpilot v2023.07.05

Browse Source 
version: dragonpilot v0.9.4 release
date: 2023-07-05T18:59:41
dp-dev(priv) master commit: 7b0489feab40283a422d2201ef95a9cb8c06f6cd
This commit is contained in:
Dragonpilot Team
2023-07-05 18:45:50 -07:00
commit d0beb4d392
1549 changed files with 511297 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/models/generated/car.h
+40
View File
@@ -0,0 +1,40 @@
#pragma once
#include "rednose/helpers/common_ekf.h"
extern "C" {
void car_update_25(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_24(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_30(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_26(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
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_7398202595670281399);
void car_inv_err_fun(double *nom_x, double *true_x, double *out_1202273728262154148);
void car_H_mod_fun(double *state, double *out_3953678512844074941);
void car_f_fun(double *state, double dt, double *out_4919033046586703302);
void car_F_fun(double *state, double dt, double *out_4107182711379579553);
void car_h_25(double *state, double *unused, double *out_1567680244849663411);
void car_H_25(double *state, double *unused, double *out_5225987524986084127);
void car_h_24(double *state, double *unused, double *out_2364262104948025745);
void car_H_24(double *state, double *unused, double *out_6913803960855188318);
void car_h_30(double *state, double *unused, double *out_2025881607008225073);
void car_H_30(double *state, double *unused, double *out_6304066207231850734);
void car_h_26(double *state, double *unused, double *out_7624553002435329403);
void car_H_26(double *state, double *unused, double *out_1484484206112027903);
void car_h_27(double *state, double *unused, double *out_7786159700633563417);
void car_H_27(double *state, double *unused, double *out_8478829519032275645);
void car_h_29(double *state, double *unused, double *out_2897319116255315758);
void car_H_29(double *state, double *unused, double *out_8254551827807724938);
void car_h_28(double *state, double *unused, double *out_1907874213074848329);
void car_H_28(double *state, double *unused, double *out_3172152810738194364);
void car_h_31(double *state, double *unused, double *out_6195585011971009030);
void car_H_31(double *state, double *unused, double *out_5256633486863044555);
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);
void car_set_center_to_front(double x);
void car_set_center_to_rear(double x);
void car_set_stiffness_front(double x);
void car_set_stiffness_rear(double x);
}
selfdrive/locationd/models/generated/gnss.h
+22
View File
@@ -0,0 +1,22 @@
#pragma once
#include "rednose/helpers/common_ekf.h"
extern "C" {
void gnss_update_6(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
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_1753087778211117269);
void gnss_inv_err_fun(double *nom_x, double *true_x, double *out_2956927510017451437);
void gnss_H_mod_fun(double *state, double *out_3719518099167795508);
void gnss_f_fun(double *state, double dt, double *out_897954495300312922);
void gnss_F_fun(double *state, double dt, double *out_6443902817737307729);
void gnss_h_6(double *state, double *sat_pos, double *out_245286193991190596);
void gnss_H_6(double *state, double *sat_pos, double *out_1271139816703817215);
void gnss_h_20(double *state, double *sat_pos, double *out_7714962011380569024);
void gnss_H_20(double *state, double *sat_pos, double *out_7189362631491019695);
void gnss_h_7(double *state, double *sat_pos_vel, double *out_3341484752321584106);
void gnss_H_7(double *state, double *sat_pos_vel, double *out_2769680937173047531);
void gnss_h_21(double *state, double *sat_pos_vel, double *out_3341484752321584106);
void gnss_H_21(double *state, double *sat_pos_vel, double *out_2769680937173047531);
void gnss_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/libkf.so
Executable
BIN
View File
Binary file not shown.
selfdrive/locationd/models/generated/live.h
+38
View File
@@ -0,0 +1,38 @@
#pragma once
#include "rednose/helpers/common_ekf.h"
extern "C" {
void live_update_4(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_9(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_12(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_35(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
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_5128157265026678182);
void live_err_fun(double *nom_x, double *delta_x, double *out_2798632635382813919);
void live_inv_err_fun(double *nom_x, double *true_x, double *out_5339335174414579736);
void live_H_mod_fun(double *state, double *out_1284023194967019530);
void live_f_fun(double *state, double dt, double *out_1421591046507185130);
void live_F_fun(double *state, double dt, double *out_7704460823213363823);
void live_h_4(double *state, double *unused, double *out_602047715749074833);
void live_H_4(double *state, double *unused, double *out_9042268972257776925);
void live_h_9(double *state, double *unused, double *out_4954724991031095057);
void live_H_9(double *state, double *unused, double *out_2117256166187327221);
void live_h_10(double *state, double *unused, double *out_8523310708961865851);
void live_H_10(double *state, double *unused, double *out_4826563757172353065);
void live_h_12(double *state, double *unused, double *out_2499034109806295132);
void live_H_12(double *state, double *unused, double *out_4385018693419812896);
void live_h_35(double *state, double *unused, double *out_4692529026086718755);
void live_H_35(double *state, double *unused, double *out_1639455661094799187);
void live_h_32(double *state, double *unused, double *out_7068888198573815517);
void live_H_32(double *state, double *unused, double *out_7238866073961881420);
void live_h_13(double *state, double *unused, double *out_7559512868723735515);
void live_H_13(double *state, double *unused, double *out_8231147310952013425);
void live_h_14(double *state, double *unused, double *out_4954724991031095057);
void live_H_14(double *state, double *unused, double *out_2117256166187327221);
void live_h_33(double *state, double *unused, double *out_1217344797616727358);
void live_H_33(double *state, double *unused, double *out_1511101343544058417);
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
}
selfdrive/locationd/models/generated/live_kf_constants.h
+102
View File
@@ -0,0 +1,102 @@
#pragma once
#include <unordered_map>
#include <eigen3/Eigen/Dense>
#define STATE_ACCELERATION_START 16
#define STATE_ACCELERATION_END 19
#define STATE_ACCELERATION_LEN 3
#define STATE_ACCELERATION_ERR_START 15
#define STATE_ACCELERATION_ERR_END 18
#define STATE_ACCELERATION_ERR_LEN 3
#define STATE_ACC_BIAS_START 19
#define STATE_ACC_BIAS_END 22
#define STATE_ACC_BIAS_LEN 3
#define STATE_ACC_BIAS_ERR_START 18
#define STATE_ACC_BIAS_ERR_END 21
#define STATE_ACC_BIAS_ERR_LEN 3
#define STATE_ANGULAR_VELOCITY_START 10
#define STATE_ANGULAR_VELOCITY_END 13
#define STATE_ANGULAR_VELOCITY_LEN 3
#define STATE_ANGULAR_VELOCITY_ERR_START 9
#define STATE_ANGULAR_VELOCITY_ERR_END 12
#define STATE_ANGULAR_VELOCITY_ERR_LEN 3
#define STATE_ECEF_ORIENTATION_START 3
#define STATE_ECEF_ORIENTATION_END 7
#define STATE_ECEF_ORIENTATION_LEN 4
#define STATE_ECEF_ORIENTATION_ERR_START 3
#define STATE_ECEF_ORIENTATION_ERR_END 6
#define STATE_ECEF_ORIENTATION_ERR_LEN 3
#define STATE_ECEF_POS_START 0
#define STATE_ECEF_POS_END 3
#define STATE_ECEF_POS_LEN 3
#define STATE_ECEF_POS_ERR_START 0
#define STATE_ECEF_POS_ERR_END 3
#define STATE_ECEF_POS_ERR_LEN 3
#define STATE_ECEF_VELOCITY_START 7
#define STATE_ECEF_VELOCITY_END 10
#define STATE_ECEF_VELOCITY_LEN 3
#define STATE_ECEF_VELOCITY_ERR_START 6
#define STATE_ECEF_VELOCITY_ERR_END 9
#define STATE_ECEF_VELOCITY_ERR_LEN 3
#define STATE_GYRO_BIAS_START 13
#define STATE_GYRO_BIAS_END 16
#define STATE_GYRO_BIAS_LEN 3
#define STATE_GYRO_BIAS_ERR_START 12
#define STATE_GYRO_BIAS_ERR_END 15
#define STATE_GYRO_BIAS_ERR_LEN 3
#define OBSERVATION_ANGLE_OFFSET_FAST 27
#define OBSERVATION_CAMERA_ODO_ROTATION 14
#define OBSERVATION_CAMERA_ODO_TRANSLATION 13
#define OBSERVATION_ECEF_ORIENTATION_FROM_GPS 32
#define OBSERVATION_ECEF_POS 12
#define OBSERVATION_ECEF_VEL 35
#define OBSERVATION_FEATURE_TRACK_TEST 17
#define OBSERVATION_GPS_NED 2
#define OBSERVATION_GPS_VEL 5
#define OBSERVATION_IMU_FRAME 19
#define OBSERVATION_LANE_PT 18
#define OBSERVATION_MSCKF_TEST 16
#define OBSERVATION_NO_ACCEL 33
#define OBSERVATION_NO_OBSERVATION 1
#define OBSERVATION_NO_ROT 9
#define OBSERVATION_ODOMETRIC_SPEED 3
#define OBSERVATION_ORB_FEATURES 15
#define OBSERVATION_ORB_FEATURES_WIDE 34
#define OBSERVATION_ORB_POINT 11
#define OBSERVATION_PHONE_ACCEL 10
#define OBSERVATION_PHONE_GYRO 4
#define OBSERVATION_PSEUDORANGE 22
#define OBSERVATION_PSEUDORANGE_GLONASS 20
#define OBSERVATION_PSEUDORANGE_GPS 6
#define OBSERVATION_PSEUDORANGE_RATE 23
#define OBSERVATION_PSEUDORANGE_RATE_GLONASS 21
#define OBSERVATION_PSEUDORANGE_RATE_GPS 7
#define OBSERVATION_ROAD_FRAME_XY_SPEED 24
#define OBSERVATION_ROAD_FRAME_X_SPEED 30
#define OBSERVATION_ROAD_FRAME_YAW_RATE 25
#define OBSERVATION_ROAD_ROLL 31
#define OBSERVATION_SPEED 8
#define OBSERVATION_STEER_ANGLE 26
#define OBSERVATION_STEER_RATIO 29
#define OBSERVATION_STIFFNESS 28
#define OBSERVATION_UNKNOWN 0
static const Eigen::VectorXd live_initial_x = (Eigen::VectorXd(22) << 3.8800000e+06,-3.3700000e+06,3.7600000e+06,4.2254641e-01,-3.1238054e-01,-8.3602975e-01,-1.5788347e-01,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00,0.0000000e+00).finished();
static const Eigen::VectorXd live_initial_P_diag = (Eigen::VectorXd(21) << 1.e+02,1.e+02,1.e+02,1.e-04,1.e-04,1.e-04,1.e+02,1.e+02,1.e+02,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+00,1.e+04,1.e+04,1.e+04,1.e-04,1.e-04,1.e-04).finished();
static const Eigen::VectorXd live_fake_gps_pos_cov_diag = (Eigen::VectorXd(3) << 1000000,1000000,1000000).finished();
static const Eigen::VectorXd live_fake_gps_vel_cov_diag = (Eigen::VectorXd(3) << 100,100,100).finished();
static const Eigen::VectorXd live_reset_orientation_diag = (Eigen::VectorXd(3) << 1,1,1).finished();
static const Eigen::VectorXd live_Q_diag = (Eigen::VectorXd(21) << 8.9999999999999998e-04,8.9999999999999998e-04,8.9999999999999998e-04,9.9999999999999995e-07,9.9999999999999995e-07,9.9999999999999995e-07,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000000e-04,1.0000000000000002e-02,1.0000000000000002e-02,1.0000000000000002e-02,2.5000000000000001e-09,2.5000000000000001e-09,2.5000000000000001e-09,9.0000000000000000e+00,9.0000000000000000e+00,9.0000000000000000e+00,2.5000000000000001e-05,2.5000000000000001e-05,2.5000000000000001e-05).finished();
static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {
{ 4, (Eigen::VectorXd(3) << 0.0006250000000000001,0.0006250000000000001,0.0006250000000000001).finished() },
{ 10, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
{ 14, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
{ 9, (Eigen::VectorXd(3) << 2.5e-05,2.5e-05,2.5e-05).finished() },
{ 33, (Eigen::VectorXd(3) << 0.0025000000000000005,0.0025000000000000005,0.0025000000000000005).finished() },
{ 12, (Eigen::VectorXd(3) << 25,25,25).finished() },
{ 35, (Eigen::VectorXd(3) << 0.25,0.25,0.25).finished() },
{ 32, (Eigen::VectorXd(4) << 0.04000000000000001,0.04000000000000001,0.04000000000000001,0.04000000000000001).finished() },
};