From 16fca911cf18992d2c26725ee2b6d6decc4d3313 Mon Sep 17 00:00:00 2001
From: jacobsj14 <jacobstjohn07@gmail.com>
Date: Mon, 25 May 2026 19:20:15 -0230
Subject: [PATCH] Rotation Matrix

Rotation Matrix for mapping the reference frame to body frame. I am not sure if this is correct but please have a look
---
 RotationMatrix | 78 ++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 78 insertions(+)
 create mode 100644 RotationMatrix

diff --git a/RotationMatrix b/RotationMatrix
new file mode 100644
index 0000000..46e2a80
--- /dev/null
+++ b/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='')