sunnypilot v2026.002.000
version: sunnypilot v2026.002.000 (staging) date: 2026-07-21T07:44:02 master commit: 2c334ede443d7391d27575af8c854a095ba702a8
This commit is contained in:
github-actions[bot] committed
commit
de77c89cf6
3995 files changed
+1260688
No files matched your search
Binary file not shown.
File diff suppressed because it is too large.
Load diff
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
#include "rednose/helpers/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_6746369553526897517);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_2356689388845686605);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_7416532235122030660);
|
||||
void live_H_mod_fun(double *state, double *out_7744797074742978621);
|
||||
void live_f_fun(double *state, double dt, double *out_8063025216892748525);
|
||||
void live_F_fun(double *state, double dt, double *out_1924980074135776075);
|
||||
void live_h_4(double *state, double *unused, double *out_7366302340587907635);
|
||||
void live_H_4(double *state, double *unused, double *out_8678952100085115055);
|
||||
void live_h_9(double *state, double *unused, double *out_1116014595619586497);
|
||||
void live_H_9(double *state, double *unused, double *out_2480573038359989091);
|
||||
void live_h_10(double *state, double *unused, double *out_15153629602772458);
|
||||
void live_H_10(double *state, double *unused, double *out_4583653000591918572);
|
||||
void live_h_12(double *state, double *unused, double *out_9071809923999704409);
|
||||
void live_H_12(double *state, double *unused, double *out_4748335565592474766);
|
||||
void live_h_35(double *state, double *unused, double *out_7011637724721731008);
|
||||
void live_H_35(double *state, double *unused, double *out_2002772533267461057);
|
||||
void live_h_32(double *state, double *unused, double *out_8005805873262674095);
|
||||
void live_H_32(double *state, double *unused, double *out_6269965131311088716);
|
||||
void live_h_13(double *state, double *unused, double *out_7868428076142185420);
|
||||
void live_H_13(double *state, double *unused, double *out_4177806446556115282);
|
||||
void live_h_14(double *state, double *unused, double *out_1116014595619586497);
|
||||
void live_H_14(double *state, double *unused, double *out_2480573038359989091);
|
||||
void live_h_33(double *state, double *unused, double *out_5108012817780654932);
|
||||
void live_H_33(double *state, double *unused, double *out_1147784471371396547);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -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() },
|
||||
};
|
||||
|
||||
Reference in new issue
Block a user