|  |  |  | @ -40,7 +40,6 @@ class GNSSKalman(): | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |   maha_test_kinds = []  # ObservationKind.PSEUDORANGE_RATE, ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_GLONASS] | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |   @staticmethod | 
			
		
	
		
			
				
					|  |  |  |  |   def generate_code(): | 
			
		
	
		
			
				
					|  |  |  |  |     dim_state = GNSSKalman.x_initial.shape[0] | 
			
		
	
	
		
			
				
					|  |  |  | @ -68,7 +67,6 @@ class GNSSKalman(): | 
			
		
	
		
			
				
					|  |  |  |  |     # Can be pretty bad if dt is big | 
			
		
	
		
			
				
					|  |  |  |  |     f_sym = state + dt * state_dot | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |     # | 
			
		
	
		
			
				
					|  |  |  |  |     # Observation functions | 
			
		
	
		
			
				
					|  |  |  |  |     # | 
			
		
	
	
		
			
				
					|  |  |  | @ -85,17 +83,21 @@ class GNSSKalman(): | 
			
		
	
		
			
				
					|  |  |  |  |     los_x, los_y, los_z = sat_los_sym | 
			
		
	
		
			
				
					|  |  |  |  |     orb_x, orb_y, orb_z = orb_epos_sym | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |     h_pseudorange_sym = sp.Matrix([sp.sqrt( | 
			
		
	
		
			
				
					|  |  |  |  |     h_pseudorange_sym = sp.Matrix([ | 
			
		
	
		
			
				
					|  |  |  |  |       sp.sqrt( | 
			
		
	
		
			
				
					|  |  |  |  |         (x - sat_x)**2 + | 
			
		
	
		
			
				
					|  |  |  |  |         (y - sat_y)**2 + | 
			
		
	
		
			
				
					|  |  |  |  |                                     (z - sat_z)**2) + | 
			
		
	
		
			
				
					|  |  |  |  |                                     cb]) | 
			
		
	
		
			
				
					|  |  |  |  |         (z - sat_z)**2 | 
			
		
	
		
			
				
					|  |  |  |  |       ) + cb | 
			
		
	
		
			
				
					|  |  |  |  |     ]) | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |     h_pseudorange_glonass_sym = sp.Matrix([sp.sqrt( | 
			
		
	
		
			
				
					|  |  |  |  |     h_pseudorange_glonass_sym = sp.Matrix([ | 
			
		
	
		
			
				
					|  |  |  |  |       sp.sqrt( | 
			
		
	
		
			
				
					|  |  |  |  |         (x - sat_x)**2 + | 
			
		
	
		
			
				
					|  |  |  |  |         (y - sat_y)**2 + | 
			
		
	
		
			
				
					|  |  |  |  |                                     (z - sat_z)**2) + | 
			
		
	
		
			
				
					|  |  |  |  |                                     cb + glonass_bias + glonass_freq_slope*glonass_freq]) | 
			
		
	
		
			
				
					|  |  |  |  |         (z - sat_z)**2 | 
			
		
	
		
			
				
					|  |  |  |  |       ) + cb + glonass_bias + glonass_freq_slope * glonass_freq | 
			
		
	
		
			
				
					|  |  |  |  |     ]) | 
			
		
	
		
			
				
					|  |  |  |  | 
 | 
			
		
	
		
			
				
					|  |  |  |  |     los_vector = (sp.Matrix(sat_pos_vel_sym[0:3]) - sp.Matrix([x, y, z])) | 
			
		
	
		
			
				
					|  |  |  |  |     los_vector = los_vector / sp.sqrt(los_vector[0]**2 + los_vector[1]**2 + los_vector[2]**2) | 
			
		
	
	
		
			
				
					|  |  |  | 
 |