| 
						
						
							
								
							
						
						
					 | 
					 | 
					@ -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:]) | 
				
			
			
		
	
		
		
			
				
					
					 | 
					 | 
					 | 
					
 | 
					 | 
					 | 
					 | 
					
 | 
				
			
			
		
	
	
		
		
			
				
					| 
						
							
								
							
						
						
						
					 | 
					 | 
					
  |