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
@@ -0,0 +1 @@
|
||||
generated/
|
||||
Whitespace-only changes.
+180
@@ -0,0 +1,180 @@
|
||||
#!/usr/bin/env python3
|
||||
import math
|
||||
import sys
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
|
||||
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
from openpilot.common.swaglog import cloudlog
|
||||
|
||||
from rednose.helpers.kalmanfilter import KalmanFilter
|
||||
|
||||
if __name__ == '__main__': # Generating sympy
|
||||
import sympy as sp
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
else:
|
||||
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
|
||||
|
||||
|
||||
i = 0
|
||||
|
||||
def _slice(n):
|
||||
global i
|
||||
s = slice(i, i + n)
|
||||
i += n
|
||||
|
||||
return s
|
||||
|
||||
|
||||
class States:
|
||||
# Vehicle model params
|
||||
STIFFNESS = _slice(1) # [-]
|
||||
STEER_RATIO = _slice(1) # [-]
|
||||
ANGLE_OFFSET = _slice(1) # [rad]
|
||||
ANGLE_OFFSET_FAST = _slice(1) # [rad]
|
||||
|
||||
VELOCITY = _slice(2) # (x, y) [m/s]
|
||||
YAW_RATE = _slice(1) # [rad/s]
|
||||
STEER_ANGLE = _slice(1) # [rad]
|
||||
ROAD_ROLL = _slice(1) # [rad]
|
||||
|
||||
|
||||
class CarKalman(KalmanFilter):
|
||||
name = 'car'
|
||||
|
||||
initial_x = np.array([
|
||||
1.0,
|
||||
15.0,
|
||||
0.0,
|
||||
0.0,
|
||||
|
||||
10.0, 0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
])
|
||||
|
||||
# process noise
|
||||
Q = np.diag([
|
||||
(.05 / 100)**2,
|
||||
.01**2,
|
||||
math.radians(0.02)**2,
|
||||
math.radians(0.25)**2,
|
||||
|
||||
.1**2, .01**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(0.1)**2,
|
||||
math.radians(1)**2,
|
||||
])
|
||||
P_initial = Q.copy()
|
||||
|
||||
obs_noise: dict[int, Any] = {
|
||||
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
|
||||
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
|
||||
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
|
||||
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
|
||||
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
|
||||
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
|
||||
}
|
||||
|
||||
global_vars = [
|
||||
'mass',
|
||||
'rotational_inertia',
|
||||
'center_to_front',
|
||||
'center_to_rear',
|
||||
'stiffness_front',
|
||||
'stiffness_rear',
|
||||
]
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
dim_state = CarKalman.initial_x.shape[0]
|
||||
name = CarKalman.name
|
||||
|
||||
# Linearized single-track lateral dynamics, equations 7.211-7.213
|
||||
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
|
||||
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
|
||||
|
||||
# globals
|
||||
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
|
||||
m, j, aF, aR, cF_orig, cR_orig = global_vars
|
||||
|
||||
# make functions and jacobians with sympy
|
||||
# state variables
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
|
||||
# Vehicle model constants
|
||||
sf = state[States.STIFFNESS, :][0, 0]
|
||||
|
||||
cF, cR = sf * cF_orig, sf * cR_orig
|
||||
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
|
||||
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
|
||||
theta = state[States.ROAD_ROLL, :][0, 0]
|
||||
sa = state[States.STEER_ANGLE, :][0, 0]
|
||||
|
||||
sR = state[States.STEER_RATIO, :][0, 0]
|
||||
u, v = state[States.VELOCITY, :]
|
||||
r = state[States.YAW_RATE, :][0, 0]
|
||||
|
||||
A = sp.Matrix(np.zeros((2, 2)))
|
||||
A[0, 0] = -(cF + cR) / (m * u)
|
||||
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
|
||||
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
|
||||
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
|
||||
|
||||
B = sp.Matrix(np.zeros((2, 1)))
|
||||
B[0, 0] = cF / m / sR
|
||||
B[1, 0] = (cF * aF) / j / sR
|
||||
|
||||
C = sp.Matrix(np.zeros((2, 1)))
|
||||
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
|
||||
C[1, 0] = 0
|
||||
|
||||
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
|
||||
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
|
||||
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
|
||||
|
||||
# Basic descretization, 1st order integrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
obs_eqs = [
|
||||
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
|
||||
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
|
||||
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
|
||||
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
|
||||
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
|
||||
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
|
||||
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
|
||||
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
|
||||
]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
|
||||
|
||||
def __init__(self, generated_dir):
|
||||
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
|
||||
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
|
||||
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
|
||||
|
||||
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
|
||||
self.filter.set_global("mass", mass)
|
||||
self.filter.set_global("rotational_inertia", rotational_inertia)
|
||||
self.filter.set_global("center_to_front", center_to_front)
|
||||
self.filter.set_global("center_to_rear", center_to_rear)
|
||||
self.filter.set_global("stiffness_front", stiffness_front)
|
||||
self.filter.set_global("stiffness_rear", stiffness_rear)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
CarKalman.generate_code(generated_dir)
|
||||
@@ -0,0 +1,92 @@
|
||||
import os
|
||||
|
||||
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
|
||||
|
||||
class ObservationKind:
|
||||
UNKNOWN = 0
|
||||
NO_OBSERVATION = 1
|
||||
GPS_NED = 2
|
||||
ODOMETRIC_SPEED = 3
|
||||
PHONE_GYRO = 4
|
||||
GPS_VEL = 5
|
||||
PSEUDORANGE_GPS = 6
|
||||
PSEUDORANGE_RATE_GPS = 7
|
||||
SPEED = 8
|
||||
NO_ROT = 9
|
||||
PHONE_ACCEL = 10
|
||||
ORB_POINT = 11
|
||||
ECEF_POS = 12
|
||||
CAMERA_ODO_TRANSLATION = 13
|
||||
CAMERA_ODO_ROTATION = 14
|
||||
ORB_FEATURES = 15
|
||||
MSCKF_TEST = 16
|
||||
FEATURE_TRACK_TEST = 17
|
||||
LANE_PT = 18
|
||||
IMU_FRAME = 19
|
||||
PSEUDORANGE_GLONASS = 20
|
||||
PSEUDORANGE_RATE_GLONASS = 21
|
||||
PSEUDORANGE = 22
|
||||
PSEUDORANGE_RATE = 23
|
||||
ECEF_VEL = 35
|
||||
ECEF_ORIENTATION_FROM_GPS = 32
|
||||
NO_ACCEL = 33
|
||||
ORB_FEATURES_WIDE = 34
|
||||
|
||||
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
|
||||
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
|
||||
STEER_ANGLE = 26 # [rad]
|
||||
ANGLE_OFFSET_FAST = 27 # [rad]
|
||||
STIFFNESS = 28 # [-]
|
||||
STEER_RATIO = 29 # [-]
|
||||
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
|
||||
ROAD_ROLL = 31 # [rad]
|
||||
|
||||
names = [
|
||||
'Unknown',
|
||||
'No observation',
|
||||
'GPS NED',
|
||||
'Odometric speed',
|
||||
'Phone gyro',
|
||||
'GPS velocity',
|
||||
'GPS pseudorange',
|
||||
'GPS pseudorange rate',
|
||||
'Speed',
|
||||
'No rotation',
|
||||
'Phone acceleration',
|
||||
'ORB point',
|
||||
'ECEF pos',
|
||||
'camera odometric translation',
|
||||
'camera odometric rotation',
|
||||
'ORB features',
|
||||
'MSCKF test',
|
||||
'Feature track test',
|
||||
'Lane ecef point',
|
||||
'imu frame eulers',
|
||||
'GLONASS pseudorange',
|
||||
'GLONASS pseudorange rate',
|
||||
'pseudorange',
|
||||
'pseudorange rate',
|
||||
|
||||
'Road Frame x,y speed',
|
||||
'Road Frame yaw rate',
|
||||
'Steer Angle',
|
||||
'Fast Angle Offset',
|
||||
'Stiffness',
|
||||
'Steer Ratio',
|
||||
'Road Frame x speed',
|
||||
'Road Roll',
|
||||
'ECEF orientation from GPS',
|
||||
'NO accel',
|
||||
'ORB features wide camera',
|
||||
'ECEF_VEL',
|
||||
]
|
||||
|
||||
@classmethod
|
||||
def to_string(cls, kind):
|
||||
return cls.names[kind]
|
||||
|
||||
|
||||
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GPS,
|
||||
ObservationKind.PSEUDORANGE_GLONASS,
|
||||
ObservationKind.PSEUDORANGE_RATE_GLONASS]
|
||||
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() },
|
||||
};
|
||||
|
||||
@@ -0,0 +1,122 @@
|
||||
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
|
||||
|
||||
using namespace EKFS;
|
||||
using namespace Eigen;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
|
||||
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
|
||||
}
|
||||
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
|
||||
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> res;
|
||||
for (const Eigen::VectorXd &vec : vec_vec) {
|
||||
res.push_back(get_mapvec(vec));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
|
||||
std::vector<Eigen::Map<MatrixXdr>> res;
|
||||
for (const MatrixXdr &mat : mat_vec) {
|
||||
res.push_back(get_mapmat(mat));
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
LiveKalman::LiveKalman() {
|
||||
this->dim_state = live_initial_x.rows();
|
||||
this->dim_state_err = live_initial_P_diag.rows();
|
||||
|
||||
this->initial_x = live_initial_x;
|
||||
this->initial_P = live_initial_P_diag.asDiagonal();
|
||||
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
|
||||
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
|
||||
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
|
||||
this->Q = live_Q_diag.asDiagonal();
|
||||
for (auto& pair : live_obs_noise_diag) {
|
||||
this->obs_noise[pair.first] = pair.second.asDiagonal();
|
||||
}
|
||||
|
||||
// init filter
|
||||
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
|
||||
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
|
||||
std::vector<int>{3}, std::vector<std::string>(), 0.8);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
|
||||
MatrixXdr covs = covs_diag.asDiagonal();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
|
||||
MatrixXdr covs = this->filter->covs();
|
||||
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
|
||||
}
|
||||
|
||||
VectorXd LiveKalman::get_x() {
|
||||
return this->filter->state();
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::get_P() {
|
||||
return this->filter->covs();
|
||||
}
|
||||
|
||||
double LiveKalman::get_filter_time() {
|
||||
return this->filter->get_filter_time();
|
||||
}
|
||||
|
||||
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
|
||||
std::vector<MatrixXdr> R;
|
||||
for (int i = 0; i < n; i++) {
|
||||
R.push_back(this->obs_noise[kind]);
|
||||
}
|
||||
return R;
|
||||
}
|
||||
|
||||
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
|
||||
std::optional<Estimate> r;
|
||||
if (R.size() == 0) {
|
||||
R = this->get_R(kind, meas.size());
|
||||
}
|
||||
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
|
||||
return r;
|
||||
}
|
||||
|
||||
void LiveKalman::predict(double t) {
|
||||
this->filter->predict(t);
|
||||
}
|
||||
|
||||
const Eigen::VectorXd &LiveKalman::get_initial_x() {
|
||||
return this->initial_x;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_initial_P() {
|
||||
return this->initial_P;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
|
||||
return this->fake_gps_pos_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
|
||||
return this->fake_gps_vel_cov;
|
||||
}
|
||||
|
||||
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
|
||||
return this->reset_orientation_P;
|
||||
}
|
||||
|
||||
MatrixXdr LiveKalman::H(const VectorXd &in) {
|
||||
assert(in.size() == 6);
|
||||
Matrix<double, 3, 6, Eigen::RowMajor> res;
|
||||
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
|
||||
return res;
|
||||
}
|
||||
@@ -0,0 +1,66 @@
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
|
||||
#include <eigen3/Eigen/Core>
|
||||
#include <eigen3/Eigen/Dense>
|
||||
|
||||
#include "generated/live_kf_constants.h"
|
||||
#include "rednose/helpers/ekf_sym.h"
|
||||
|
||||
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
using namespace EKFS;
|
||||
|
||||
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
|
||||
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
|
||||
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
|
||||
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
|
||||
|
||||
class LiveKalman {
|
||||
public:
|
||||
LiveKalman();
|
||||
|
||||
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
|
||||
void init_state(const Eigen::VectorXd &state, double filter_time);
|
||||
|
||||
Eigen::VectorXd get_x();
|
||||
MatrixXdr get_P();
|
||||
double get_filter_time();
|
||||
std::vector<MatrixXdr> get_R(int kind, int n);
|
||||
|
||||
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
|
||||
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
|
||||
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
|
||||
void predict(double t);
|
||||
|
||||
const Eigen::VectorXd &get_initial_x();
|
||||
const MatrixXdr &get_initial_P();
|
||||
const MatrixXdr &get_fake_gps_pos_cov();
|
||||
const MatrixXdr &get_fake_gps_vel_cov();
|
||||
const MatrixXdr &get_reset_orientation_P();
|
||||
|
||||
MatrixXdr H(const Eigen::VectorXd &in);
|
||||
|
||||
private:
|
||||
std::string name = "live";
|
||||
|
||||
std::shared_ptr<EKFSym> filter;
|
||||
|
||||
int dim_state;
|
||||
int dim_state_err;
|
||||
|
||||
Eigen::VectorXd initial_x;
|
||||
MatrixXdr initial_P;
|
||||
MatrixXdr fake_gps_pos_cov;
|
||||
MatrixXdr fake_gps_vel_cov;
|
||||
MatrixXdr reset_orientation_P;
|
||||
MatrixXdr Q; // process noise
|
||||
std::unordered_map<int, MatrixXdr> obs_noise;
|
||||
};
|
||||
+242
@@ -0,0 +1,242 @@
|
||||
#!/usr/bin/env python3
|
||||
|
||||
import sys
|
||||
import os
|
||||
import numpy as np
|
||||
|
||||
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
|
||||
|
||||
import sympy as sp
|
||||
import inspect
|
||||
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
|
||||
from rednose.helpers.ekf_sym import gen_code
|
||||
|
||||
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
|
||||
|
||||
|
||||
def numpy2eigenstring(arr):
|
||||
assert(len(arr.shape) == 1)
|
||||
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
|
||||
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
|
||||
|
||||
|
||||
class States:
|
||||
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
|
||||
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
|
||||
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
|
||||
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
|
||||
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
|
||||
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
|
||||
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
|
||||
|
||||
# Error-state has different slices because it is an ESKF
|
||||
ECEF_POS_ERR = slice(0, 3)
|
||||
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
|
||||
ECEF_VELOCITY_ERR = slice(6, 9)
|
||||
ANGULAR_VELOCITY_ERR = slice(9, 12)
|
||||
GYRO_BIAS_ERR = slice(12, 15)
|
||||
ACCELERATION_ERR = slice(15, 18)
|
||||
ACC_BIAS_ERR = slice(18, 21)
|
||||
|
||||
|
||||
class LiveKalman:
|
||||
name = 'live'
|
||||
|
||||
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
|
||||
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0,
|
||||
0, 0, 0])
|
||||
|
||||
# state covariance
|
||||
initial_P_diag = np.array([10**2, 10**2, 10**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
10**2, 10**2, 10**2,
|
||||
1**2, 1**2, 1**2,
|
||||
1**2, 1**2, 1**2,
|
||||
100**2, 100**2, 100**2,
|
||||
0.01**2, 0.01**2, 0.01**2])
|
||||
|
||||
# state covariance when resetting midway in a segment
|
||||
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
|
||||
|
||||
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
|
||||
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
|
||||
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
|
||||
|
||||
# process noise
|
||||
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
|
||||
0.001**2, 0.001**2, 0.001**2,
|
||||
0.01**2, 0.01**2, 0.01**2,
|
||||
0.1**2, 0.1**2, 0.1**2,
|
||||
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
|
||||
3**2, 3**2, 3**2,
|
||||
0.005**2, 0.005**2, 0.005**2])
|
||||
|
||||
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
|
||||
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
|
||||
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
|
||||
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
|
||||
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
|
||||
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
|
||||
|
||||
@staticmethod
|
||||
def generate_code(generated_dir):
|
||||
name = LiveKalman.name
|
||||
dim_state = LiveKalman.initial_x.shape[0]
|
||||
dim_state_err = LiveKalman.initial_P_diag.shape[0]
|
||||
|
||||
state_sym = sp.MatrixSymbol('state', dim_state, 1)
|
||||
state = sp.Matrix(state_sym)
|
||||
x, y, z = state[States.ECEF_POS, :]
|
||||
q = state[States.ECEF_ORIENTATION, :]
|
||||
v = state[States.ECEF_VELOCITY, :]
|
||||
vx, vy, vz = v
|
||||
omega = state[States.ANGULAR_VELOCITY, :]
|
||||
vroll, vpitch, vyaw = omega
|
||||
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
|
||||
acceleration = state[States.ACCELERATION, :]
|
||||
acc_bias = state[States.ACC_BIAS, :]
|
||||
|
||||
dt = sp.Symbol('dt')
|
||||
|
||||
# calibration and attitude rotation matrices
|
||||
quat_rot = quat_rotate(*q)
|
||||
|
||||
# Got the quat predict equations from here
|
||||
# A New Quaternion-Based Kalman Filter for
|
||||
# Real-Time Attitude Estimation Using the Two-Step
|
||||
# Geometrically-Intuitive Correction Algorithm
|
||||
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
|
||||
[vroll, 0, vyaw, -vpitch],
|
||||
[vpitch, -vyaw, 0, vroll],
|
||||
[vyaw, vpitch, -vroll, 0]])
|
||||
q_dot = A * q
|
||||
|
||||
# Time derivative of the state as a function of state
|
||||
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
state_dot[States.ECEF_POS, :] = v
|
||||
state_dot[States.ECEF_ORIENTATION, :] = q_dot
|
||||
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
|
||||
|
||||
# Basic descretization, 1st order intergrator
|
||||
# Can be pretty bad if dt is big
|
||||
f_sym = state + dt * state_dot
|
||||
|
||||
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
|
||||
state_err = sp.Matrix(state_err_sym)
|
||||
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
|
||||
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
|
||||
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
|
||||
acceleration_err = state_err[States.ACCELERATION_ERR, :]
|
||||
|
||||
# Time derivative of the state error as a function of state error and state
|
||||
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
|
||||
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
|
||||
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
state_err_dot[States.ECEF_POS_ERR, :] = v_err
|
||||
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
|
||||
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
|
||||
f_err_sym = state_err + dt * state_err_dot
|
||||
|
||||
# Observation matrix modifier
|
||||
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
|
||||
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
|
||||
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
|
||||
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
|
||||
|
||||
# these error functions are defined so that say there
|
||||
# is a nominal x and true x:
|
||||
# true x = err_function(nominal x, delta x)
|
||||
# delta x = inv_err_function(nominal x, true x)
|
||||
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
|
||||
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
|
||||
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
|
||||
|
||||
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
|
||||
delta_quat = sp.Matrix(np.ones(4))
|
||||
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
|
||||
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
|
||||
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
|
||||
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
|
||||
|
||||
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
|
||||
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
|
||||
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
|
||||
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
|
||||
|
||||
eskf_params = [[err_function_sym, nom_x, delta_x],
|
||||
[inv_err_function_sym, nom_x, true_x],
|
||||
H_mod_sym, f_err_sym, state_err_sym]
|
||||
#
|
||||
# Observation functions
|
||||
#
|
||||
h_gyro_sym = sp.Matrix([
|
||||
vroll + roll_bias,
|
||||
vpitch + pitch_bias,
|
||||
vyaw + yaw_bias])
|
||||
|
||||
pos = sp.Matrix([x, y, z])
|
||||
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
|
||||
h_acc_sym = (gravity + acceleration + acc_bias)
|
||||
h_acc_stationary_sym = acceleration
|
||||
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
|
||||
h_pos_sym = sp.Matrix([x, y, z])
|
||||
h_vel_sym = sp.Matrix([vx, vy, vz])
|
||||
h_orientation_sym = q
|
||||
h_relative_motion = sp.Matrix(quat_rot.T * v)
|
||||
|
||||
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
|
||||
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
|
||||
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
|
||||
[h_pos_sym, ObservationKind.ECEF_POS, None],
|
||||
[h_vel_sym, ObservationKind.ECEF_VEL, None],
|
||||
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
|
||||
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
|
||||
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
|
||||
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
|
||||
|
||||
# this returns a sympy routine for the jacobian of the observation function of the local vel
|
||||
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
|
||||
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
|
||||
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
|
||||
|
||||
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
|
||||
|
||||
# write constants to extra header file for use in cpp
|
||||
live_kf_header = "#pragma once\n\n"
|
||||
live_kf_header += "#include <unordered_map>\n"
|
||||
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
|
||||
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
|
||||
assert(slc.step is None) # unsupported
|
||||
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
|
||||
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
|
||||
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
|
||||
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
|
||||
live_kf_header += "\n"
|
||||
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
|
||||
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
|
||||
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
|
||||
for kind, noise in LiveKalman.obs_noise_diag.items():
|
||||
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
|
||||
live_kf_header += "};\n\n"
|
||||
|
||||
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
generated_dir = sys.argv[2]
|
||||
LiveKalman.generate_code(generated_dir)
|
||||
Reference in new issue
Block a user