From d4a8a59a506c2c13458d33627e765eb89fe3cf23 Mon Sep 17 00:00:00 2001
From: urpok23 <70227979+urpok23@users.noreply.github.com>
Date: Wed, 8 Nov 2023 20:53:59 +0300
Subject: [PATCH] vectorize cost calculation

---
 control/optimal.py | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/control/optimal.py b/control/optimal.py
index 811c8e518..81372705c 100644
--- a/control/optimal.py
+++ b/control/optimal.py
@@ -319,11 +319,9 @@ def _cost_function(self, coeffs):
             dt = np.diff(self.timepts)
 
             # Integrate the cost
-            # TODO: vectorize
-            cost = 0
-            for i in range(self.timepts.size-1):
-                # Approximate the integral using trapezoidal rule
-                cost += 0.5 * (costs[i] + costs[i+1]) * dt[i]
+            costs = np.array(costs)
+           # Approximate the integral using trapezoidal rule
+            cost = np.sum(0.5 * (costs[:-1] + costs[1:]) * dt)
 
         else:
             # Sum the integral cost over the time (second) indices