From d3a3274f458dfef763499f599be7dcedc80ec9ba Mon Sep 17 00:00:00 2001
From: Chris Choi <chutsu@gmail.com>
Date: Sat, 28 Sep 2024 16:45:36 +0100
Subject: [PATCH] Add plot script for ImuError test

---
 scripts/plot_test_inertial_error_solve.py | 17 +++++++++++++++++
 1 file changed, 17 insertions(+)
 create mode 100644 scripts/plot_test_inertial_error_solve.py

diff --git a/scripts/plot_test_inertial_error_solve.py b/scripts/plot_test_inertial_error_solve.py
new file mode 100644
index 0000000..bdbc90c
--- /dev/null
+++ b/scripts/plot_test_inertial_error_solve.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+import numpy as np
+import matplotlib.pylab as plt
+
+data = np.genfromtxt("/tmp/imu_solve.csv", delimiter=",", skip_header=True)
+
+time = data[:, 0] * 1e-9
+gnd = data[:, 1:4]
+init = data[:, 4:7]
+est = data[:, 7:10]
+
+plt.plot(gnd[:, 0], gnd[:, 1], "k--", label="Ground-Truth")
+plt.plot(init[:, 0], init[:, 1], "r.", label="Initial")
+plt.plot(est[:, 0], est[:, 1], "b-", label="Estimate")
+
+plt.legend(loc=0)
+plt.show()