Rotation Matrix

RotationMatrix

@@ -0,0 +1,78 @@
+#RotationMatrixASV
+import numpy as np
+import csv
+
+def rotation_matrix(yaw_deg, pitch_deg=0.0, roll_deg=0.0):
+    psi = np.radians(yaw_deg)
+    theta = np.radians(pitch_deg)
+    phi = np.radians(roll_deg)  
+
+    Rz = np.array([
+        [np.cos(psi), -np.sin(psi), 0],
+        [np.sin(psi), np.cos(psi), 0],
+        [0, 0, 1]
+    ])
+
+    Ry = np.array([
+        [np.cos(theta), 0, np.sin(theta)],
+        [1, 0, 0],
+        [-np.sin(theta), 0, np.cos(theta)]
+    ])
+
+    Rx = np.array([
+        [1, 0, 0],
+        [0, np.cos(phi), -np.sin(phi)],
+        [0, np.sin(phi), np.cos(phi)]
+    ])
+
+    return Rz @ Ry @ Rx
+
+#Algorithm for taking real data samples would go here:
+#example algorithm here
+'''
+timesteps = 10
+dt = 0.1
+
+samples = []
+for i in range(timestamps):
+    t = round(i * dt, 2)
+    yaw = 30.0 + i * 1.0
+    pitch = 0.0
+    roll = 0.0
+    v_body = np.array([3.00, 0.05, 0.00])
+    R = rotation_matrix(yaw, pitch, roll)
+    v_ned = R @ v_body
+
+    samples.append({
+    'timestamp': t,
+    'yaw_deg': round(yaw, 2),
+    'pitch_deg' : round(pitch, 2)
+    'roll_deg' : round(roll, 2)
+    "vx_body":    round(v_body[0], 4),
+    "vy_body":    round(v_body[1], 4),
+    "vz_body":    round(v_body[2], 4),
+    "v_north":    round(v_ned[0], 4),
+    "v_east":     round(v_ned[1], 4),
+    "v_down":     round(v_ned[2], 4),
+    })
+
+'''
+
+
+#Writing to CSV file
+
+output_file = 'icebergasv_ned_log.csv'
+fieldnames = ['timestamp', 'yaw_deg', 'pitch_deg', 'roll_deg', 'vx_body', 'vy_body', 'vz_body', 'v_north',
+'v_east','v_down']
+
+with open(output_file, 'w', newline='') as f:
+    writer = csv.DictWriter(f, fieldnames=fieldnames)
+    writer.writeheader()
+    writer.writerows(samples)
+
+print(f"wrote {len(samples)} rows to {output_file}")
+
+print('\nPreview')
+with open(output_file, 'r') as f:
+    for line in f:
+        print(line, end='')