@ -250,12 +250,12 @@ void Localizer::input_fake_gps_observations(double current_time) { 
			
		
	
		
			
				
					  // Steps : first predict -> observe current obs with reasonable STD
   
			
		
	
		
			
				
					  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_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_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_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 ;   
			
		
	
		
			
				
					  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 ( ) ;   
			
		
	
		
			
				
					   
 
			
		
	
		
			
				
					
 
			
		
	
		
			
				
					  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 ( ) ;   
			
		
	
		
			
				
					
 
			
		
	
	
		
			
				
					
						
							
								 
						
						
							
								 
						
						
					 
				
				@ -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_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 ( ) ;   
			
		
	
		
			
				
					   
 
			
		
	
		
			
				
					
 
			
		
	
		
			
				
					  this - > reset_kalman ( current_time ,  current_x ,  init_P ) ;   
			
		
	
		
			
				
					}  
			
		
	
		
			
				
					
 
			
		
	
	
		
			
				
					
						
							
								 
						
						
							
								 
						
						
					 
				
				@ -494,7 +494,7 @@ int Localizer::locationd_thread() { 
			
		
	
		
			
				
					  PubMaster  pm ( { " liveLocationKalman " } ) ;   
			
		
	
		
			
				
					
 
			
		
	
		
			
				
					  // 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 ;   
			
		
	
		
			
				
					  bool  filterInitialized  =  false ;