NavState Autonomous Surface Vessel IMU EKF

import numpy as np
import plotly.graph_objects as go
from plotly.subplots import make_subplots

import gtsam
from gtsam import NavState, Point3, Pose3, Rot3


# Helper to display yaw/pitch/roll in degrees
def rot3_ypr_rad(R: Rot3):
    """Return yaw, pitch, roll (degrees) for a Rot3."""
    return np.degrees(R.ypr())

# Single function to convert CSV into as similar as input used in NavStateImuExample
def parse_data(filename):
    # read csv file as np
    data_np = np.genfromtxt(filename, delimiter=",", dtype=None, skip_header=1)  # type: ignore
    start = data_np[1, 7:]
    imu_meas = [
        (row[1:4], row[4:7]) for row in data_np[1:]
    ]  # skipping first row because of gt initialization
    data_gt = data_np[:, 7:]

    dts = np.diff(
        data_np[:, 0]
    )  # I found that not all dt values are the same, therefore I'm using an array.
    position = start[:3]
    quat = start[3:7]  # x,y,z,w
    velocity_body = start[7:10]

    rot = Rot3.Quaternion(quat[3], quat[0], quat[1], quat[2])

    pose = Pose3(rot, Point3(*position))
    velocity_world = rot.rotate(velocity_body)
    X0 = NavState(pose, velocity_world)

    pos_true = data_gt[:, 0:3]
    quat_true = data_gt[:, 3:7]
    vel_true_body = data_gt[:, 7:10]
    vel_true_world = np.empty_like(vel_true_body)
    ypr_true = np.empty((vel_true_body.shape[0], 3))
    for i in range(vel_true_body.shape[0]):
        rot = Rot3.Quaternion(
            quat_true[i, 3], quat_true[i, 0], quat_true[i, 1], quat_true[i, 2]
        )
        vel_true_world[i] = rot.rotate(vel_true_body[i])
        ypr_true[i] = rot3_ypr_rad(rot)

    return X0, imu_meas, pos_true, ypr_true, vel_true_world, dts

# Find the maximum delta
max_delta = np.max(dts)

# Create a Plotly figure
fig = go.Figure()

# Add a line plot for deltas
fig.add_trace(
    go.Scatter(y=dts, mode="lines", name="Delta (s)", line=dict(color="blue"))
)

# Update layout for the plot
fig.update_layout(
    title="Delta between Consecutive Timestamps",
    xaxis_title="Index",
    yaxis_title="Delta (s)",
    template="plotly_white",
)

# Show the plot
fig.show()

# Find the indices where the delta is greater than 0.01
large_deltas_idx = np.where(dts > 0.010001)[0]  # In some cases dt is 0.0100000000001
print(f"Indices of deltas greater than 0.01: {large_deltas_idx}")

print(f"The maximum delta between consecutive timestamps is: {max_delta}")

# Create 3-row subplot: one for each of Yaw, Pitch, Roll
fig = make_subplots(rows=3, cols=1, shared_xaxes=True, subplot_titles=("Yaw (deg)", "Pitch (deg)", "Roll (deg)"))
color_rgb = {0: ("rgba(255,0,0,0.15)", "#ff0000"), 1: ("rgba(0,128,0,0.15)", "#008000"), 2: ("rgba(0,0,255,0.15)", "#0000ff")}
names = {0: "Yaw", 1: "Pitch", 2: "Roll"}
for i in range(3):
    fill_rgba, line_color = color_rgb[i]; mean = ypr_est[:, i]; std2p = 2.0 * rot_std_deg[:, i]; upper = mean + std2p; lower = mean - std2p; row = i + 1
    fig.add_trace(go.Scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=lower, fill="tonexty", fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=mean, name=f"{names[i]} est", line=dict(color=line_color, width=2)), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=ypr_true[:, i], name=f"{names[i]} true", line=dict(color=line_color, dash="dash", width=2)), row=row, col=1)
fig.update_layout(height=900, title_text="Yaw, Pitch, Roll (deg) with ±2σ bounds", xaxis3_title="Time (s)", yaxis_title="Degrees", showlegend=True)
fig.show()

# 3 subplots for vx, vy, vz
fig = make_subplots(rows=3, cols=1, shared_xaxes=True, subplot_titles=("Velocity X (m/s)", "Velocity Y (m/s)", "Velocity Z (m/s)"))
color_rgb = {0: ("rgba(255,0,0,0.15)", "#ff0000"), 1: ("rgba(0,128,0,0.15)", "#008000"), 2: ("rgba(0,0,255,0.15)", "#0000ff")}
for i, comp in enumerate(["x", "y", "z"]):
    fill_rgba, line_color = color_rgb[i]; mean = vel_est[:, i]; std2p = 2.0 * vel_std[:, i]; upper = mean + std2p; lower = mean - std2p; row = i + 1
    fig.add_trace(go.Scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=lower, fill="tonexty", fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=mean, name=f"v{comp} est", line=dict(color=line_color, width=2)), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=vel_true[:, i], name=f"v{comp} true", line=dict(color=line_color, dash="dash", width=2)), row=row, col=1)
fig.update_layout(height=900, title_text="Velocity Components in World Frame with ±2σ Bounds", xaxis3_title="Time (s)", yaxis_title="Velocity (m/s)", showlegend=True)
fig.show()

# 3 subplots for px, py, pz
fig = make_subplots(rows=3, cols=1, shared_xaxes=True, subplot_titles=("Position X (m)", "Position Y (m)", "Position Z (m)"))
color_rgb = {0: ("rgba(255,0,0,0.15)", "#ff0000"), 1: ("rgba(0,128,0,0.15)", "#008000"), 2: ("rgba(0,0,255,0.15)", "#0000ff")}
for i, comp in enumerate(["x", "y", "z"]):
    fill_rgba, line_color = color_rgb[i]; mean = pos_est[:, i]; std2p = 2.0 * pos_std[:, i]; upper = mean + std2p; lower = mean - std2p; row = i + 1
    fig.add_trace(go.Scatter(x=times, y=upper, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=lower, fill="tonexty", fillcolor=fill_rgba, line=dict(color=line_color, width=0), showlegend=False), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=mean, name=f"p{comp} est", line=dict(color=line_color, width=2)), row=row, col=1)
    fig.add_trace(go.Scatter(x=times, y=pos_true[:, i], name=f"p{comp} true", line=dict(color=line_color, dash="dash", width=2)), row=row, col=1)
fig.update_layout(height=900, title_text="Position Components with ±2σ Bounds", xaxis3_title="Time (s)", yaxis_title="Position (m)", showlegend=True)
fig.show()