@ -250,12 +250,12 @@ void Localizer::input_fake_gps_observations(double current_time) {
// Steps : first predict -> observe current obs with reasonable STD
// Steps : first predict -> observe current obs with reasonable STD
this - > kf - > predict ( current_time ) ;
this - > kf - > predict ( current_time ) ;
VectorXd current_x = this - > kf - > get_x ( ) ;
VectorXd current_x = this - > kf - > get_x ( ) ;
VectorXd ecef_pos = current_x . segment < STATE_ECEF_POS_LEN > ( STATE_ECEF_POS_START ) ;
VectorXd ecef_pos = current_x . segment < STATE_ECEF_POS_LEN > ( STATE_ECEF_POS_START ) ;
VectorXd ecef_vel = current_x . segment < STATE_ECEF_VELOCITY_LEN > ( STATE_ECEF_VELOCITY_START ) ;
VectorXd ecef_vel = current_x . segment < STATE_ECEF_VELOCITY_LEN > ( STATE_ECEF_VELOCITY_START ) ;
MatrixXdr ecef_pos_R = this - > kf - > get_fake_gps_pos_cov ( ) ;
MatrixXdr ecef_pos_R = this - > kf - > get_fake_gps_pos_cov ( ) ;
MatrixXdr ecef_vel_R = this - > kf - > get_fake_gps_vel_cov ( ) ;
MatrixXdr ecef_vel_R = this - > kf - > get_fake_gps_vel_cov ( ) ;
this - > kf - > predict_and_observe ( current_time , OBSERVATION_ECEF_POS , { ecef_pos } , { ecef_pos_R } ) ;
this - > kf - > predict_and_observe ( current_time , OBSERVATION_ECEF_POS , { ecef_pos } , { ecef_pos_R } ) ;
this - > kf - > predict_and_observe ( current_time , OBSERVATION_ECEF_VEL , { ecef_vel } , { ecef_vel_R } ) ;
this - > kf - > predict_and_observe ( current_time , OBSERVATION_ECEF_VEL , { ecef_vel } , { ecef_vel_R } ) ;
}
}
@ -283,7 +283,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
VectorXd ecef_vel = this - > converter - > ned2ecef ( { log . getVNED ( ) [ 0 ] , log . getVNED ( ) [ 1 ] , log . getVNED ( ) [ 2 ] } ) . to_vector ( ) - ecef_pos ;
VectorXd ecef_vel = this - > converter - > ned2ecef ( { log . getVNED ( ) [ 0 ] , log . getVNED ( ) [ 1 ] , log . getVNED ( ) [ 2 ] } ) . to_vector ( ) - ecef_pos ;
MatrixXdr ecef_pos_R = Vector3d : : Constant ( std : : pow ( 10.0 * log . getAccuracy ( ) , 2 ) + std : : pow ( 10.0 * log . getVerticalAccuracy ( ) , 2 ) ) . asDiagonal ( ) ;
MatrixXdr ecef_pos_R = Vector3d : : Constant ( std : : pow ( 10.0 * log . getAccuracy ( ) , 2 ) + std : : pow ( 10.0 * log . getVerticalAccuracy ( ) , 2 ) ) . asDiagonal ( ) ;
MatrixXdr ecef_vel_R = Vector3d : : Constant ( std : : pow ( log . getSpeedAccuracy ( ) * 10.0 , 2 ) ) . asDiagonal ( ) ;
MatrixXdr ecef_vel_R = Vector3d : : Constant ( std : : pow ( log . getSpeedAccuracy ( ) * 10.0 , 2 ) ) . asDiagonal ( ) ;
this - > unix_timestamp_millis = log . getTimestamp ( ) ;
this - > unix_timestamp_millis = log . getTimestamp ( ) ;
double gps_est_error = ( this - > kf - > get_x ( ) . segment < STATE_ECEF_POS_LEN > ( STATE_ECEF_POS_START ) - ecef_pos ) . norm ( ) ;
double gps_est_error = ( this - > kf - > get_x ( ) . segment < STATE_ECEF_POS_LEN > ( STATE_ECEF_POS_START ) - ecef_pos ) . norm ( ) ;
@ -419,7 +419,7 @@ void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd
init_P . block < STATE_ECEF_ORIENTATION_ERR_LEN , STATE_ECEF_ORIENTATION_ERR_LEN > ( STATE_ECEF_ORIENTATION_ERR_START , STATE_ECEF_ORIENTATION_ERR_START ) . diagonal ( ) = reset_orientation_P . diagonal ( ) ;
init_P . block < STATE_ECEF_ORIENTATION_ERR_LEN , STATE_ECEF_ORIENTATION_ERR_LEN > ( STATE_ECEF_ORIENTATION_ERR_START , STATE_ECEF_ORIENTATION_ERR_START ) . diagonal ( ) = reset_orientation_P . diagonal ( ) ;
init_P . block < STATE_ECEF_VELOCITY_ERR_LEN , STATE_ECEF_VELOCITY_ERR_LEN > ( STATE_ECEF_VELOCITY_ERR_START , STATE_ECEF_VELOCITY_ERR_START ) . diagonal ( ) = init_vel_R . diagonal ( ) ;
init_P . block < STATE_ECEF_VELOCITY_ERR_LEN , STATE_ECEF_VELOCITY_ERR_LEN > ( STATE_ECEF_VELOCITY_ERR_START , STATE_ECEF_VELOCITY_ERR_START ) . diagonal ( ) = init_vel_R . diagonal ( ) ;
init_P . block ( STATE_ANGULAR_VELOCITY_ERR_START , STATE_ANGULAR_VELOCITY_ERR_START , non_ecef_state_err_len , non_ecef_state_err_len ) . diagonal ( ) = current_P . block ( STATE_ANGULAR_VELOCITY_ERR_START , STATE_ANGULAR_VELOCITY_ERR_START , non_ecef_state_err_len , non_ecef_state_err_len ) . diagonal ( ) ;
init_P . block ( STATE_ANGULAR_VELOCITY_ERR_START , STATE_ANGULAR_VELOCITY_ERR_START , non_ecef_state_err_len , non_ecef_state_err_len ) . diagonal ( ) = current_P . block ( STATE_ANGULAR_VELOCITY_ERR_START , STATE_ANGULAR_VELOCITY_ERR_START , non_ecef_state_err_len , non_ecef_state_err_len ) . diagonal ( ) ;
this - > reset_kalman ( current_time , current_x , init_P ) ;
this - > reset_kalman ( current_time , current_x , init_P ) ;
}
}
@ -494,7 +494,7 @@ int Localizer::locationd_thread() {
PubMaster pm ( { " liveLocationKalman " } ) ;
PubMaster pm ( { " liveLocationKalman " } ) ;
// TODO: remove carParams once we're always sending at 100Hz
// TODO: remove carParams once we're always sending at 100Hz
SubMaster sm ( service_list , nullptr , { " gpsLocationExternal " , " carParams " } ) ;
SubMaster sm ( service_list , { } , nullptr , { " gpsLocationExternal " , " carParams " } ) ;
uint64_t cnt = 0 ;
uint64_t cnt = 0 ;
bool filterInitialized = false ;
bool filterInitialized = false ;