# -*- coding: utf-8 -*- """ Created Mar 2025 @author: Nortek Support Transform Nortek Signature AD2CP data from BEAM to ENU coordinates. - Compatible with .mat files exported from Signature Deployment. - Supports detection of AHRS Edit file path, 'mode' and 'declination' as needed before running!! """ import numpy as np from scipy.io import loadmat # Load the .mat file mat_data = loadmat(r"C:\Users\FranziskaOgg\OneDrive - Nortek AS\Documents\Notes\FAQ draft\Coordinate Systems\test_plan.mat") #mat_data = loadmat("") # Extract 'Data' and 'Config' as dictionaries Data = mat_data['Data'][0, 0] Config = mat_data['Config'][0, 0] Data = {field: Data[field] for field in Data.dtype.names} Config = {field: Config[field] for field in Config.dtype.names} # ------------------------------- # Parameters mode = 'burst' declination = 0 # ------------------------------- # Mode mapping mode_map = { 'avg': ('Average', 'Average'), 'avg1': ('Alt_Average', 'Alt_Average'), 'burst1': ('Alt_Burst', 'Alt_Burst'), 'burst': ('Burst', 'Burst') } dataModeWord, configModeWord = mode_map[mode] # Indexing entire time series index = np.arange(len(Data[f'{dataModeWord}_Time'].flatten())) # Determine if two verticals exist twoZs = int( f'{dataModeWord}_VelUp2' in Data.keys() or f'{dataModeWord}_VelBeam4' in Data.keys() or f'{dataModeWord}_VelZ2' in Data.keys()) # Confirm sufficient amount of beams used beam_map = Data[f'{dataModeWord}_BeamToChannelMapping'][0] + 1 active_beams = beam_map[beam_map>0] number_of_beams = len( beam_map[beam_map>0]) if number_of_beams <= 2: print("Transformations require at least 3 active beams.") T_beam2xyz = np.nan exit() # ------------------------------- # Get transformation matrix T T_beam2xyz = Config[f'{configModeWord}_Beam2xyz'] # Special case of Signature55 if Config['InstrumentName'] == 'Signature55kHz': T_beam2xyz = np.array([ [1.9492, -0.9746, -0.9746], [0, -1.6881, 1.6881], [0.3547, 0.3547, 0.3547] ]) # Correct bug in older FW versions if Config['FWVersion'] <= 2163 and number_of_beams == 4: T_beam2xyz[1, :] = -T_beam2xyz[1, :] # Coordiante transformation BEAM -> XYZ # Coordinate system check coord_sys = Config[f'{configModeWord}_CoordSystem'][0] if coord_sys == 'BEAM': print("Velocity data is in BEAM coordinate system.") n_cells = Config[f'{configModeWord}_NCells'][0][0] L = len(index) x_all = np.zeros((L, n_cells)) y_all = np.zeros((L, n_cells)) z_all = np.zeros((L, n_cells)) if twoZs: z2_all = np.zeros((L, n_cells)) for n_cell in range(n_cells): beam = np.zeros((number_of_beams, L)) for i, beam_channel in enumerate(active_beams): field_name = f'{dataModeWord}_VelBeam{int(beam_channel)}' beam[i, :] = Data[field_name][:, n_cell].flatten() # Apply T matrix to beam data xyz = T_beam2xyz @ beam x_all[:, n_cell] = xyz[0, :] y_all[:, n_cell] = xyz[1, :] z_all[:, n_cell] = xyz[2, :] if twoZs and xyz.shape[0] > 3: z2_all[:, n_cell] = xyz[3, :] # Save back to Data dictionary Data[f'{dataModeWord}_VelX'] = x_all Data[f'{dataModeWord}_VelY'] = y_all if twoZs: Data[f'{dataModeWord}_VelZ1'] = z_all Data[f'{dataModeWord}_VelZ2'] = z2_all else: Data[f'{dataModeWord}_VelZ'] = z_all # Mark updated coordinate system Config[f'{configModeWord}_CoordSystem'] = 'XYZ' # Just use plain string here print("BEAM transformed to XYZ") # ------------------------------------------------------------------- # Coordinate transformation XYZ ➝ ENU # ------------------------------------------------------------------- ahrsUsed = False hprUsed = False coord_sys = Config[f'{configModeWord}_CoordSystem'] if coord_sys == 'ENU': print("Velocity data is already in ENU coordinate system.") else: if coord_sys == 'XYZ': print("Velocity data is in XYZ coordinate system.") K = 4 if twoZs else 3 n_cells = Config[f'{configModeWord}_NCells'][0][0] L = len(index) # Pre-allocate ENU velocity arrays in Data Data[f'{dataModeWord}_VelEast'] = np.zeros((L, n_cells)) Data[f'{dataModeWord}_VelNorth'] = np.zeros((L, n_cells)) if not twoZs: Data[f'{dataModeWord}_VelUp'] = np.zeros((L, n_cells)) else: Data[f'{dataModeWord}_VelUp1'] = np.zeros((L, n_cells)) Data[f'{dataModeWord}_VelUp2'] = np.zeros((L, n_cells)) if twoZs: Name = ['X', 'Y', 'Z1', 'Z2'] else: Name = ['X', 'Y', 'Z'] ENU = np.zeros((K, n_cells)) xyz = np.zeros((K, n_cells)) for sampleIndex in index: # identify orientation or usage of AHRS orientation = ( Data[f'{dataModeWord}_Status'][sampleIndex].item() >> 25) & 0b111 signXYZ = np.array([1, -1, -1, -1]) if orientation == 5 else np.array([1, 1, 1, 1]) if orientation == 7: if not ahrsUsed: print("AHRS matrix is used") ahrsUsed = True rot = Data[f'{dataModeWord}_AHRSRotationMatrix'][sampleIndex].reshape(3, 3) H = np.array([[np.cos(np.deg2rad(declination)), np.sin(np.deg2rad(declination)), 0], [-np.sin(np.deg2rad(declination)), np.cos(np.deg2rad(declination)), 0], [0, 0, 1]]) xyz2enu = H @ rot #Data[f'{dataModeWord}_AHRSRotationMatrix'][sampleIndex] = xyz2enu.T.reshape(-1) heading = 90 - np.arctan2(xyz2enu[1, 0], xyz2enu[0, 0]) * 180 / np.pi heading %= 360 Data[f'{dataModeWord}_Heading'][sampleIndex] = heading else: if not hprUsed: print("HPR matrix is used") hprUsed = True heading = Data[f'{dataModeWord}_Heading'][sampleIndex] + declination heading %= 360 Data[f'{dataModeWord}_Heading'][sampleIndex] = heading hh = np.deg2rad(heading - 90) pp = np.deg2rad(Data[f'{dataModeWord}_Pitch'][sampleIndex]) rr = np.deg2rad(Data[f'{dataModeWord}_Roll'][sampleIndex]) H = np.array([ [np.cos(hh[0]), np.sin(hh[0]), 0], [-np.sin(hh[0]), np.cos(hh[0]), 0], [0, 0, 1] ]) P = np.array([ [np.cos(pp[0]), -np.sin(pp[0])*np.sin(rr[0]), -np.cos(rr[0])*np.sin(pp[0])], [0, np.cos(rr[0]), -np.sin(rr[0])], [np.sin(pp[0]), np.sin(rr[0])*np.cos(pp[0]), np.cos(pp[0])*np.cos(rr[0])] ]) xyz2enu = H @ P if twoZs: print(xyz2enu) xyz2enu_full = np.zeros((4, 4)) xyz2enu_full[:3, :3] = xyz2enu # Apply the same operations xyz2enu_full[0, 2] /= 2 xyz2enu_full[0, 3] = xyz2enu_full[0, 2] xyz2enu_full[1, 2] /= 2 xyz2enu_full[1, 3] = xyz2enu_full[1, 2] # Duplicate row 3 into row 4 xyz2enu_full[3, :] = xyz2enu_full[2, :] # Modify 3rd and 4th rows as described xyz2enu_full[2, 3] = 0 xyz2enu_full[3, 3] = xyz2enu_full[2, 2] xyz2enu_full[3, 2] = 0 xyz2enu=xyz2enu_full for i in range(K): axs = Name[i] xyz[i, :] = signXYZ[i] * Data[f'{dataModeWord}_Vel{axs}'][sampleIndex, :n_cells] # Transform XYZ to ENU ENU = xyz2enu @ xyz # Save to Data Data[f'{dataModeWord}_VelEast'][sampleIndex, :n_cells] = ENU[0, :] Data[f'{dataModeWord}_VelNorth'][sampleIndex, :n_cells] = ENU[1, :] if not twoZs: Data[f'{dataModeWord}_VelUp'][sampleIndex, :n_cells] = ENU[2, :] else: Data[f'{dataModeWord}_VelUp1'][sampleIndex, :n_cells] = ENU[2, :] Data[f'{dataModeWord}_VelUp2'][sampleIndex, :n_cells] = ENU[3, :] # Update coordinate system Config[f'{configModeWord}_CoordSystem'] = 'ENU' print("XYZ transformed to ENU")