From ead725a7723502065f2174db871674bb625be5e4 Mon Sep 17 00:00:00 2001
From: Pavel Mazniker <60963077+goodengineer@users.noreply.github.com>
Date: Tue, 7 Jan 2025 21:02:32 -0900
Subject: [PATCH] Update simAndObserve.m

---
 MPC virtual lab/helperfunctions/simAndObserve.m | 16 ++++++++++------
 1 file changed, 10 insertions(+), 6 deletions(-)

diff --git a/MPC virtual lab/helperfunctions/simAndObserve.m b/MPC virtual lab/helperfunctions/simAndObserve.m
index 9755637..5554c75 100644
--- a/MPC virtual lab/helperfunctions/simAndObserve.m	
+++ b/MPC virtual lab/helperfunctions/simAndObserve.m	
@@ -4,20 +4,24 @@ function simAndObserve(Vx, out, reference)
 XRef = out.simout.time*Vx;
 waypoints = [XRef YRef zeros(length(YRef),1)];
 simulationTime = 10;
-%figure(1)
+figure
 simulateScenario(simulationTime,waypoints,Vx);
-
-
-%figure;
-%add labels and fix not having own figure issue
+figure
 subplot(2,1,1)
 hold on
 plot(reference(:,1),reference(:,2))
 plot(out.tout,YRef);
+legend('Ref. 1 vc 2','YRef vs tout sim.')
+xlabel('Time (s)')
+ylabel('Position (m)')
 hold off
 subplot(2,1,2)
 hold on
 plot(reference(:,1),reference(:,3))
 plot(out.tout,yawRef);
+legend('Ref. 1 vs 3','yawRef vs tout sim.')
+xlabel('Time (s)')
+ylabel('Yaw (rad)')
 hold off
-end
\ No newline at end of file
+set(gcf, 'Visible','on')
+end