|  |  | @ -191,9 +191,9 @@ class LocKalman(): | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     # Observation matrix modifier |  |  |  |     # Observation matrix modifier | 
			
		
	
		
		
			
				
					
					|  |  |  |     H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) |  |  |  |     H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err))) | 
			
		
	
		
		
			
				
					
					|  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) |  |  |  |       H_mod_sym[p_idx[0]:p_idx[1], p_err_idx[0]:p_err_idx[1]] = np.eye(p_idx[1] - p_idx[0]) | 
			
		
	
		
		
			
				
					
					|  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:] |  |  |  |       H_mod_sym[q_idx[0]:q_idx[1], q_err_idx[0]:q_err_idx[1]] = 0.5 * quat_matrix_r(state[q_idx[0]:q_idx[1]])[:, 1:] | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     # these error functions are defined so that say there |  |  |  |     # these error functions are defined so that say there | 
			
		
	
	
		
		
			
				
					|  |  | @ -205,17 +205,17 @@ class LocKalman(): | 
			
		
	
		
		
			
				
					
					|  |  |  |     delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) |  |  |  |     delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) |  |  |  |     err_function_sym = sp.Matrix(np.zeros((dim_state, 1))) | 
			
		
	
		
		
			
				
					
					|  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       delta_quat = sp.Matrix(np.ones(4)) |  |  |  |       delta_quat = sp.Matrix(np.ones(4)) | 
			
		
	
		
		
			
				
					
					|  |  |  |       delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :]) |  |  |  |       delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[q_err_idx[0]: q_err_idx[1], :]) | 
			
		
	
		
		
			
				
					
					|  |  |  |       err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat |  |  |  |       err_function_sym[q_idx[0]:q_idx[1], 0] = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]) * delta_quat | 
			
		
	
		
		
			
				
					
					|  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) |  |  |  |       err_function_sym[p_idx[0]:p_idx[1], :] = sp.Matrix(nom_x[p_idx[0]:p_idx[1], :] + delta_x[p_err_idx[0]:p_err_idx[1], :]) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
		
		
			
				
					
					|  |  |  |     inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) |  |  |  |     inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1))) | 
			
		
	
		
		
			
				
					
					|  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs): |  |  |  |     for p_idx, p_err_idx in zip(p_idxs, p_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) |  |  |  |       inv_err_function_sym[p_err_idx[0]:p_err_idx[1], 0] = sp.Matrix(-nom_x[p_idx[0]:p_idx[1], 0] + true_x[p_idx[0]:p_idx[1], 0]) | 
			
		
	
		
		
			
				
					
					|  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs): |  |  |  |     for q_idx, q_err_idx in zip(q_idxs, q_err_idxs, strict=True): | 
			
				
				
			
		
	
		
		
	
		
		
			
				
					
					|  |  |  |       delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] |  |  |  |       delta_quat = quat_matrix_r(nom_x[q_idx[0]:q_idx[1], 0]).T * true_x[q_idx[0]:q_idx[1], 0] | 
			
		
	
		
		
			
				
					
					|  |  |  |       inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) |  |  |  |       inv_err_function_sym[q_err_idx[0]:q_err_idx[1], 0] = sp.Matrix(2 * delta_quat[1:]) | 
			
		
	
		
		
			
				
					
					|  |  |  | 
 |  |  |  | 
 | 
			
		
	
	
		
		
			
				
					|  |  | 
 |