mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-15 03:53:58 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.8.11 release

Browse Source
This commit is contained in:
Vehicle Researcher
2021-11-19 15:24:51 -08:00
parent 93e8dbb8b5
commit bf338bf5e9
328 changed files with 9240 additions and 3951 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.cc
+12 -12
View File
@@ -130,16 +130,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
Vector3d nans = Vector3d(NAN, NAN, NAN);
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true);
init_measurement(fix.initVelocityNED(), ned_vel, nans, true);
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, true);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0);
init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
@@ -305,6 +305,7 @@ void Localizer::handle_car_state(double current_time, const cereal::CarState::Re
this->car_speed = std::abs(log.getVEgo());
if (log.getStandstill()) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
@@ -335,7 +336,6 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
rot_calib_std *= 10.0;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
@@ -344,12 +344,12 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry
void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) {
if (log.getRpyCalib().size() > 0) {
auto calib = floatlist2vector(log.getRpyCalib());
if ((calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
return;
}
this->calib = calib;
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == 1;
selfdrive/locationd/models/constants.py
+1
View File
@@ -29,6 +29,7 @@ class ObservationKind:
PSEUDORANGE_RATE = 23
ECEF_VEL = 31
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
selfdrive/locationd/models/live_kf.cc
+2 -2
View File
@@ -28,8 +28,8 @@ std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(std::vector<MatrixXdr>& mat_ve
}
LiveKalman::LiveKalman() {
this->dim_state = 23;
this->dim_state_err = 22;
this->dim_state = 26;
this->dim_state_err = 25;
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
selfdrive/locationd/models/live_kf.py
+25 -15
View File
@@ -29,6 +29,7 @@ class States():
ODO_SCALE = slice(16, 17) # odometer scale
ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
IMU_OFFSET = slice(20, 23) # imu offset angles in radians
ACC_BIAS = slice(23, 26)
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@@ -39,29 +40,32 @@ class States():
ODO_SCALE_ERR = slice(15, 16)
ACCELERATION_ERR = slice(16, 19)
IMU_OFFSET_ERR = slice(19, 22)
ACC_BIAS_ERR = slice(22, 25)
class LiveKalman():
name = 'live'
initial_x = np.array([-2.7e6, 4.2e6, 3.8e6,
1, 0, 0, 0,
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,
1,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([1e16, 1e16, 1e16,
10**2, 10**2, 10**2,
initial_P_diag = np.array([1e3**2, 1e3**2, 1e3**2,
0.5**2, 0.5**2, 0.5**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
0.02**2,
1**2, 1**2, 1**2,
(0.01)**2, (0.01)**2, (0.01)**2])
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2,
0.01**2, 0.01**2, 0.01**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
@@ -71,7 +75,8 @@ class LiveKalman():
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
(0.02 / 100)**2,
3**2, 3**2, 3**2,
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2])
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]),
ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
@@ -79,6 +84,7 @@ class LiveKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: 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])}
@@ -98,9 +104,9 @@ class LiveKalman():
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
odo_scale = state[States.ODO_SCALE, :][0, :]
acceleration = state[States.ACCELERATION, :]
imu_angles = state[States.IMU_OFFSET, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
@@ -134,6 +140,7 @@ class LiveKalman():
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)
@@ -176,18 +183,20 @@ class LiveKalman():
#
# Observation functions
#
#imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = sp.Matrix([vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
# imu_rot = euler_rotate(*imu_angles)
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)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
h_speed_sym = sp.Matrix([speed * odo_scale])
h_speed_sym = sp.Matrix([speed])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
@@ -205,7 +214,8 @@ class LiveKalman():
[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_imu_frame_sym, ObservationKind.IMU_FRAME, None]]
[h_imu_frame_sym, ObservationKind.IMU_FRAME, 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
selfdrive/locationd/ublox_msg.cc
+7 -5
View File
@@ -172,12 +172,14 @@ kj::Array<capnp::word> UbloxMsgParser::gen_rxm_sfrbx(ubx_t::rxm_sfrbx_t *msg) {
}
// Collect subframes in map and parse when we have all the parts
kaitai::kstream stream(subframe_data);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
{
kaitai::kstream stream(subframe_data);
gps_t subframe(&stream);
int subframe_id = subframe.how()->subframe_id();
if (subframe_id == 1) gps_subframes[msg->sv_id()].clear();
gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
if (subframe_id == 1) gps_subframes[msg->sv_id()].clear();
gps_subframes[msg->sv_id()][subframe_id] = subframe_data;
}
if (gps_subframes[msg->sv_id()].size() == 5) {
MessageBuilder msg_builder;
selfdrive/locationd/ubloxd.cc
+4 -4
View File
@@ -45,10 +45,10 @@ int main() {
if(parser.add_data(data + bytes_consumed, (uint32_t)(len - bytes_consumed), bytes_consumed_this_time)) {
try {
auto msg = parser.gen_msg();
if (msg.second.size() > 0) {
auto bytes = msg.second.asBytes();
pm.send(msg.first.c_str(), bytes.begin(), bytes.size());
auto ublox_msg = parser.gen_msg();
if (ublox_msg.second.size() > 0) {
auto bytes = ublox_msg.second.asBytes();
pm.send(ublox_msg.first.c_str(), bytes.begin(), bytes.size());
}
} catch (const std::exception& e) {
LOGE("Error parsing ublox message %s", e.what());