diff --git a/.gitignore b/.gitignore
index ca50a2365..623c45d79 100644
--- a/.gitignore
+++ b/.gitignore
@@ -25,11 +25,12 @@ clcache
board/obj/
selfdrive/boardd/boardd
-selfdrive/visiond/visiond
selfdrive/logcatd/logcatd
+selfdrive/mapd/default_speeds_by_region.json
selfdrive/proclogd/proclogd
selfdrive/ui/ui
selfdrive/test/tests/plant/out
+selfdrive/visiond/visiond
/src/
one
diff --git a/.travis.yml b/.travis.yml
index 74411b5de..3dfe33823 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -7,6 +7,8 @@ install:
- docker build -t tmppilot -f Dockerfile.openpilot .
script:
- - docker run --rm
+ - docker run
-v "$(pwd)"/selfdrive/test/tests/plant/out:/tmp/openpilot/selfdrive/test/tests/plant/out
tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/tests/plant && OPTEST=1 ./test_longitudinal.py'
+ - docker run
+ tmppilot /bin/sh -c 'cd /tmp/openpilot/selfdrive/test/ && ./test_fingerprints.py'
diff --git a/README.md b/README.md
index b1ed6a1a6..f8052b285 100644
--- a/README.md
+++ b/README.md
@@ -114,12 +114,11 @@ Community Maintained Cars
| Make | Model | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | Giraffe |
| ---------------------| -------------------------| ---------------------| --------| ---------------| -----------------| ---------------|-------------------|
| Honda | Fit 2018 | Honda Sensing | Yes | Yes | 25mph1| 12mph | Inverted Nidec |
-| Tesla | Model S 2012 | All | Yes | Not yet | Not applicable | 0mph | Custom9|
-| Tesla | Model S 2013 | All | Yes | Not yet | Not applicable | 0mph | Custom9|
+| Tesla | Model S 2012-13 | All | Yes | Not yet | Not applicable | 0mph | Custom8|
[[Honda Fit Pull Request]](https://github.com/commaai/openpilot/pull/266).
[[Tesla Model S Pull Request]](https://github.com/commaai/openpilot/pull/246)
-9Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
+8Community built Giraffe, find more information here [Community Tesla Giraffe](https://github.com/jeankalud/neo/tree/tesla_giraffe/giraffe/tesla)
Community Maintained Cars are not confirmed by comma.ai to meet our [safety model](https://github.com/commaai/openpilot/blob/devel/SAFETY.md). Be extra cautious using them.
diff --git a/RELEASES.md b/RELEASES.md
index ae4c8f14a..ef96e09e5 100644
--- a/RELEASES.md
+++ b/RELEASES.md
@@ -1,3 +1,15 @@
+Version 0.5.10 (2019-03-19)
+========================
+ * Self-tuning vehicle parameters: steering offset, tires stiffness and steering ratio
+ * Improve longitudinal control at low speed when lead vehicle harshly decelerates
+ * Fix panda bug going unexpectedly in DCP mode when EON is connected
+ * Reduce white panda power consumption by 500mW when EON is disconnected by turning off WIFI
+ * New Driver Monitoring Model
+ * Support QR codes for login using comma connect
+ * Refactor comma pedal FW and use CRC-8 checksum algorithm for safety. Reflashing pedal is required.
+ Please see `#hw-pedal` on [discord](discord.comma.ai) for assistance updating comma pedal.
+ * Additional speed limit rules for Germany thanks to arne182
+
Version 0.5.9 (2019-02-10)
========================
* Improve calibration using a dedicated neural network
@@ -8,7 +20,7 @@ Version 0.5.9 (2019-02-10)
* Kia Optima support thanks to emmertex!
* Buick Regal 2018 support thanks to HOYS!
* Comma pedal support for Toyota thanks to wocsor! Note: tuning needed and not maintained by comma
- * Chrysler Pacifica and Jeep Grand Cherokee suppor thanks to adhintz!
+ * Chrysler Pacifica and Jeep Grand Cherokee support thanks to adhintz!
Version 0.5.8 (2019-01-17)
========================
diff --git a/SAFETY.md b/SAFETY.md
index a2f77a550..b5800a0d5 100644
--- a/SAFETY.md
+++ b/SAFETY.md
@@ -89,7 +89,7 @@ GM/Chevrolet
and openpilot to 350. This is approximately 0.3g of braking.
- Steering torque is controlled through the 0x180 CAN message and it's limited by the panda firmware and by
- openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will not fault when
+ openpilot to a value between -300 and 300. In addition, the vehicle EPS unit will fault for
commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by
openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
0.75s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
@@ -105,5 +105,34 @@ GM/Chevrolet
- GM CAN uses both a counter and a checksum to ensure integrity and prevent
replay of the same message.
-**Extra note"**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
+Hyundai/Kia (Lateral only)
+------
+
+ - While the system is engaged, steer commands are subject to the same limits used by
+ the stock system.
+
+ - Steering torque is controlled through the 0x340 CAN message and it's limited by the panda firmware and by
+ openpilot to a value between -255 and 255. In addition, the vehicle EPS unit will fault for
+ commands outside the values of -409 and 409. A steering torque rate limit is enforced by the panda firmware and by
+ openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
+ 0.85s. Commanded steering torque is gradually limited by the panda firmware and by openpilot if the driver's
+ torque exceeds 50 units in the opposite dicrection to ensure limited applied torque against the
+ driver's will.
+
+Chrysler/Jeep/Fiat (Lateral only)
+------
+
+ - While the system is engaged, steer commands are subject to the same limits used by
+ the stock system.
+
+ - Steering torque is controlled through the 0x292 CAN message and it's limited by the panda firmware and by
+ openpilot to a value between -261 and 261. In addition, the vehicle EPS unit will fault for
+ commands outside these limits. A steering torque rate limit is enforced by the panda firmware and by
+ openpilot, so that the commanded steering torque must rise from 0 to max value no faster than
+ 0.87s. Commanded steering torque is limited by the panda firmware and by openpilot to be no more than 80
+ units above the actual EPS generated motor torque to ensure limited differences between
+ commanded and actual torques.
+
+
+**Extra note**: comma.ai strongly discourages the use of openpilot forks with safety code either missing or
not fully meeting the above requirements.
diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk
index 0890624be..342f832c2 100644
Binary files a/apk/ai.comma.plus.offroad.apk and b/apk/ai.comma.plus.offroad.apk differ
diff --git a/cereal/car.capnp b/cereal/car.capnp
index fbcbb1065..8fb312867 100644
--- a/cereal/car.capnp
+++ b/cereal/car.capnp
@@ -72,6 +72,7 @@ struct CarEvent @0x9b1657f34caf3ad3 {
calibrationProgress @47;
lowBattery @48;
invalidGiraffeHonda @49;
+ vehicleModelInvalid @50;
}
}
@@ -317,6 +318,7 @@ struct CarParams {
hyundai @8;
chrysler @9;
tesla @10;
+ subaru @11;
}
# things about the car in the manual
diff --git a/cereal/log.capnp b/cereal/log.capnp
index eb6ea7632..3f79deefe 100644
--- a/cereal/log.capnp
+++ b/cereal/log.capnp
@@ -419,7 +419,7 @@ struct Live100Data {
alertType @44 :Text;
alertSound @45 :Text;
awarenessStatus @26 :Float32;
- angleOffset @27 :Float32;
+ angleModelBias @27 :Float32;
gpsPlannerActive @40 :Bool;
engageable @41 :Bool; # can OP be engaged?
driverMonitoringOn @43 :Bool;
@@ -582,6 +582,9 @@ struct Plan {
vCurvature @21 :Float32;
decelForTurn @22 :Bool;
mapValid @25 :Bool;
+ radarValid @28 :Bool;
+
+ processingDelay @29 :Float32;
struct GpsTrajectory {
@@ -608,8 +611,12 @@ struct PathPlan {
rPoly @6 :List(Float32);
rProb @7 :Float32;
- angleSteers @8 :Float32;
+ angleSteers @8 :Float32; # deg
+ rateSteers @13 :Float32; # deg/s
valid @9 :Bool;
+ paramsValid @10 :Bool;
+ modelValid @12 :Bool;
+ angleOffset @11 :Float32;
}
struct LiveLocationData {
@@ -1602,6 +1609,9 @@ struct LiveParametersData {
valid @0 :Bool;
gyroBias @1 :Float32;
angleOffset @2 :Float32;
+ angleOffsetAverage @3 :Float32;
+ stiffnessFactor @4 :Float32;
+ steerRatio @5 :Float32;
}
struct LiveMapData {
diff --git a/common/ffi_wrapper.py b/common/ffi_wrapper.py
index 65b790a80..fd6b2bf7b 100644
--- a/common/ffi_wrapper.py
+++ b/common/ffi_wrapper.py
@@ -6,7 +6,11 @@ from cffi import FFI
TMPDIR = "/tmp/ccache"
-def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=[]):
+
+def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=None):
+ if libraries is None:
+ libraries = []
+
cache = name + "_" + hashlib.sha1(c_code).hexdigest()
try:
os.mkdir(tmpdir)
@@ -28,7 +32,11 @@ def ffi_wrap(name, c_code, c_header, tmpdir=TMPDIR, cflags="", libraries=[]):
return mod.ffi, mod.lib
-def compile_code(name, c_code, c_header, directory, cflags="", libraries=[]):
+
+def compile_code(name, c_code, c_header, directory, cflags="", libraries=None):
+ if libraries is None:
+ libraries = []
+
ffibuilder = FFI()
ffibuilder.set_source(name, c_code, source_extension='.cpp', libraries=libraries)
ffibuilder.cdef(c_header)
@@ -36,6 +44,7 @@ def compile_code(name, c_code, c_header, directory, cflags="", libraries=[]):
os.environ['CFLAGS'] = cflags
ffibuilder.compile(verbose=True, debug=False, tmpdir=directory)
+
def wrap_compiled(name, directory):
sys.path.append(directory)
mod = __import__(name)
diff --git a/common/params.py b/common/params.py
index de0e7a141..30ebaa060 100755
--- a/common/params.py
+++ b/common/params.py
@@ -29,6 +29,7 @@ import fcntl
import tempfile
from enum import Enum
+
def mkdirs_exists_ok(path):
try:
os.makedirs(path)
@@ -36,52 +37,47 @@ def mkdirs_exists_ok(path):
if not os.path.isdir(path):
raise
+
class TxType(Enum):
PERSISTENT = 1
CLEAR_ON_MANAGER_START = 2
CLEAR_ON_CAR_START = 3
+
class UnknownKeyName(Exception):
pass
+
keys = {
-# written: manager
-# read: loggerd, uploaderd, offroad
- "DongleId": TxType.PERSISTENT,
"AccessToken": TxType.PERSISTENT,
- "Version": TxType.PERSISTENT,
- "TrainingVersion": TxType.PERSISTENT,
- "GitCommit": TxType.PERSISTENT,
+ "CalibrationParams": TxType.PERSISTENT,
+ "CarParams": TxType.CLEAR_ON_CAR_START,
+ "CompletedTrainingVersion": TxType.PERSISTENT,
+ "ControlsParams": TxType.PERSISTENT,
+ "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
+ "DongleId": TxType.PERSISTENT,
"GitBranch": TxType.PERSISTENT,
+ "GitCommit": TxType.PERSISTENT,
"GitRemote": TxType.PERSISTENT,
-# written: baseui
-# read: ui, controls
- "IsMetric": TxType.PERSISTENT,
- "IsFcwEnabled": TxType.PERSISTENT,
"HasAcceptedTerms": TxType.PERSISTENT,
- "CompletedTrainingVersion": TxType.PERSISTENT,
- "IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"IsDriverMonitoringEnabled": TxType.PERSISTENT,
+ "IsFcwEnabled": TxType.PERSISTENT,
"IsGeofenceEnabled": TxType.PERSISTENT,
- "SpeedLimitOffset": TxType.PERSISTENT,
-# written: visiond
-# read: visiond, controlsd
- "CalibrationParams": TxType.PERSISTENT,
- "ControlsParams": TxType.PERSISTENT,
-# written: controlsd
-# read: radard
- "CarParams": TxType.CLEAR_ON_CAR_START,
-
- "Passive": TxType.PERSISTENT,
- "DoUninstall": TxType.CLEAR_ON_MANAGER_START,
- "ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
+ "IsMetric": TxType.PERSISTENT,
"IsUpdateAvailable": TxType.PERSISTENT,
- "LongitudinalControl": TxType.PERSISTENT,
+ "IsUploadVideoOverCellularEnabled": TxType.PERSISTENT,
"LimitSetSpeed": TxType.PERSISTENT,
-
+ "LiveParameters": TxType.PERSISTENT,
+ "LongitudinalControl": TxType.PERSISTENT,
+ "Passive": TxType.PERSISTENT,
"RecordFront": TxType.PERSISTENT,
+ "ShouldDoUpdate": TxType.CLEAR_ON_MANAGER_START,
+ "SpeedLimitOffset": TxType.PERSISTENT,
+ "TrainingVersion": TxType.PERSISTENT,
+ "Version": TxType.PERSISTENT,
}
+
def fsync_dir(path):
fd = os.open(path, os.O_RDONLY)
try:
diff --git a/common/sympy_helpers.py b/common/sympy_helpers.py
new file mode 100644
index 000000000..879eb71bb
--- /dev/null
+++ b/common/sympy_helpers.py
@@ -0,0 +1,81 @@
+#!/usr/bin/env python
+import sympy as sp
+import numpy as np
+
+def cross(x):
+ ret = sp.Matrix(np.zeros((3,3)))
+ ret[0,1], ret[0,2] = -x[2], x[1]
+ ret[1,0], ret[1,2] = x[2], -x[0]
+ ret[2,0], ret[2,1] = -x[1], x[0]
+ return ret
+
+def euler_rotate(roll, pitch, yaw):
+ # make symbolic rotation matrix from eulers
+ matrix_roll = sp.Matrix([[1, 0, 0],
+ [0, sp.cos(roll), -sp.sin(roll)],
+ [0, sp.sin(roll), sp.cos(roll)]])
+ matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
+ [0, 1, 0],
+ [-sp.sin(pitch), 0, sp.cos(pitch)]])
+ matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
+ [sp.sin(yaw), sp.cos(yaw), 0],
+ [0, 0, 1]])
+ return matrix_yaw*matrix_pitch*matrix_roll
+
+def quat_rotate(q0, q1, q2, q3):
+ # make symbolic rotation matrix from quat
+ return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)],
+ [2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)],
+ [2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
+
+def quat_matrix_l(p):
+ return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], -p[3], p[2]],
+ [p[2], p[3], p[0], -p[1]],
+ [p[3], -p[2], p[1], p[0]]])
+
+def quat_matrix_r(p):
+ return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
+ [p[1], p[0], p[3], -p[2]],
+ [p[2], -p[3], p[0], p[1]],
+ [p[3], p[2], -p[1], p[0]]])
+
+
+def sympy_into_c(sympy_functions):
+ from sympy.utilities import codegen
+ routines = []
+ for name, expr, args in sympy_functions:
+ r = codegen.make_routine(name, expr, language="C99")
+
+ # argument ordering input to sympy is broken with function with output arguments
+ nargs = []
+ # reorder the input arguments
+ for aa in args:
+ if aa is None:
+ nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1]))
+ continue
+ found = False
+ for a in r.arguments:
+ if str(aa.name) == str(a.name):
+ nargs.append(a)
+ found = True
+ break
+ if not found:
+ # [1,1] is a hack for Matrices
+ nargs.append(codegen.InputArgument(aa, dimensions=[1,1]))
+ # add the output arguments
+ for a in r.arguments:
+ if type(a) == codegen.OutputArgument:
+ nargs.append(a)
+
+ #assert len(r.arguments) == len(args)+1
+ r.arguments = nargs
+
+ # add routine to list
+ routines.append(r)
+
+ [(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
+ c_code = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_code.split("\n")))
+ c_header = '\n'.join(filter(lambda x: len(x) > 0 and x[0] != '#', c_header.split("\n")))
+
+ return c_header, c_code
diff --git a/common/transformations/camera.py b/common/transformations/camera.py
index 348152d79..42432a7bb 100644
--- a/common/transformations/camera.py
+++ b/common/transformations/camera.py
@@ -112,6 +112,7 @@ def img_from_device(pt_device):
return pt_img.reshape(input_shape)[:,:2]
+#TODO please use generic img transform below
def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
size = img.shape[:2]
rot = orient.rot_from_euler(eulers)
@@ -138,3 +139,36 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
img_warped = cv2.warpPerspective(img, M, size[::-1])
return img_warped[H_border: size[0] - H_border,
W_border: size[1] - W_border]
+
+
+def transform_img(base_img,
+ augment_trans=np.array([0,0,0]),
+ augment_eulers=np.array([0,0,0]),
+ from_intr=eon_intrinsics,
+ to_intr=eon_intrinsics,
+ calib_rot_view=None,
+ output_size=None):
+ cy = from_intr[1,2]
+ size = base_img.shape[:2]
+ if not output_size:
+ output_size = size[::-1]
+ h = 1.22
+ quadrangle = np.array([[0, cy + 20],
+ [size[1]-1, cy + 20],
+ [0, size[0]-1],
+ [size[1]-1, size[0]-1]], dtype=np.float32)
+ quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1))))
+ quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1],
+ h*np.ones(4),
+ h/quadrangle_norm[:,1]))
+ rot = orient.rot_from_euler(augment_eulers)
+ if calib_rot_view is not None:
+ rot = calib_rot_view.dot(rot)
+ to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
+ to_KE = to_intr.dot(to_extrinsics)
+ warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
+ warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
+ warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
+ M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32))
+ augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE)
+ return augmented_rgb
diff --git a/common/transformations/model.py b/common/transformations/model.py
index 424bd6158..2d41d9ceb 100644
--- a/common/transformations/model.py
+++ b/common/transformations/model.py
@@ -31,6 +31,18 @@ model_intrinsics = np.array(
[ 0. , 0. , 1.]])
+# MED model
+MEDMODEL_INPUT_SIZE = (640, 240)
+MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
+MEDMODEL_CY = 47.6
+
+medmodel_zoom = 1.
+medmodel_intrinsics = np.array(
+ [[ eon_focal_length / medmodel_zoom, 0. , 0.5 * MEDMODEL_INPUT_SIZE[0]],
+ [ 0. , eon_focal_length / medmodel_zoom, MEDMODEL_CY],
+ [ 0. , 0. , 1.]])
+
+
# BIG model
BIGMODEL_INPUT_SIZE = (864, 288)
@@ -57,6 +69,9 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
get_view_frame_from_road_frame(0, 0, 0, model_height))
+medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics,
+ get_view_frame_from_road_frame(0, 0, 0, model_height))
+
model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics))
# 'camera from model camera'
@@ -110,3 +125,17 @@ def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
camera_frame_from_bigmodel_frame = np.dot(camera_frame_from_ground, ground_from_bigmodel_frame)
return camera_frame_from_bigmodel_frame
+
+
+def get_model_frame(snu_full, camera_frame_from_model_frame, size):
+ idxs = camera_frame_from_model_frame.dot(np.column_stack([np.tile(np.arange(size[0]), size[1]),
+ np.tile(np.arange(size[1]), (size[0],1)).T.flatten(),
+ np.ones(size[0] * size[1])]).T).T.astype(int)
+ calib_flat = snu_full[idxs[:,1], idxs[:,0]]
+ if len(snu_full.shape) == 3:
+ calib = calib_flat.reshape((size[1], size[0], 3))
+ elif len(snu_full.shape) == 2:
+ calib = calib_flat.reshape((size[1], size[0]))
+ else:
+ raise ValueError("shape of input img is weird")
+ return calib
diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc
index de5bdbb1f..0b9f8060d 100644
Binary files a/models/monitoring_model.dlc and b/models/monitoring_model.dlc differ
diff --git a/requirements_openpilot.txt b/requirements_openpilot.txt
index 8cadf936e..48b2ddb31 100644
--- a/requirements_openpilot.txt
+++ b/requirements_openpilot.txt
@@ -15,7 +15,7 @@ cffi==1.11.5
contextlib2==0.5.4
crc16==0.1.1
crcmod==1.7
-cryptography==2.1.4
+cryptography==1.4
cycler==0.10.0
decorator==4.0.10
docopt==0.6.2
@@ -24,6 +24,7 @@ evdev==0.6.1
fastcluster==1.1.20
filterpy==1.2.4
ipaddress==1.0.16
+json-rpc==1.12.1
libusb1==1.5.0
lmdb==0.92
mpmath==1.0.0
@@ -31,7 +32,7 @@ nose==1.3.7
numpy==1.11.1
pause==0.1.2
py==1.4.31
-pyOpenSSL==17.5.0
+pyOpenSSL==16.0.0
pyasn1-modules==0.0.8
pyasn1==0.1.9
pycapnp==0.6.3
@@ -64,3 +65,4 @@ sympy==1.1.1
tqdm==4.23.1
ujson==1.35
v4l2==0.2
+websocket_client==0.55.0
diff --git a/selfdrive/athena/__init__.py b/selfdrive/athena/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
new file mode 100755
index 000000000..f6f9ef231
--- /dev/null
+++ b/selfdrive/athena/athenad.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python2.7
+import json
+import os
+import random
+import time
+import threading
+import traceback
+import zmq
+import Queue
+from jsonrpc import JSONRPCResponseManager, dispatcher
+from websocket import create_connection, WebSocketTimeoutException
+
+import selfdrive.crash as crash
+import selfdrive.messaging as messaging
+from common.params import Params
+from selfdrive.services import service_list
+from selfdrive.swaglog import cloudlog
+from selfdrive.version import version, dirty
+
+ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
+HANDLER_THREADS = os.getenv('HANDLER_THREADS', 4)
+
+dispatcher["echo"] = lambda s: s
+payload_queue = Queue.Queue()
+response_queue = Queue.Queue()
+
+def handle_long_poll(ws):
+ end_event = threading.Event()
+
+ threads = [
+ threading.Thread(target=ws_recv, args=(ws, end_event)),
+ threading.Thread(target=ws_send, args=(ws, end_event))
+ ] + [
+ threading.Thread(target=jsonrpc_handler, args=(end_event,))
+ for x in xrange(HANDLER_THREADS)
+ ]
+
+ map(lambda thread: thread.start(), threads)
+ try:
+ while not end_event.is_set():
+ time.sleep(0.1)
+ except (KeyboardInterrupt, SystemExit):
+ end_event.set()
+ raise
+ finally:
+ for i, thread in enumerate(threads):
+ thread.join()
+
+def jsonrpc_handler(end_event):
+ while not end_event.is_set():
+ try:
+ data = payload_queue.get(timeout=1)
+ response = JSONRPCResponseManager.handle(data, dispatcher)
+ response_queue.put_nowait(response)
+ except Queue.Empty:
+ pass
+ except Exception as e:
+ cloudlog.exception("athena jsonrpc handler failed")
+ traceback.print_exc()
+ response_queue.put_nowait(json.dumps({"error": str(e)}))
+
+# security: user should be able to request any message from their car
+# TODO: add service to, for example, start visiond and take a picture
+@dispatcher.add_method
+def getMessage(service=None, timeout=1000):
+ context = zmq.Context()
+ if service is None or service not in service_list:
+ raise Exception("invalid service")
+ socket = messaging.sub_sock(context, service_list[service].port)
+ socket.setsockopt(zmq.RCVTIMEO, timeout)
+ ret = messaging.recv_one(socket)
+ return ret.to_dict()
+
+def ws_recv(ws, end_event):
+ while not end_event.is_set():
+ try:
+ data = ws.recv()
+ payload_queue.put_nowait(data)
+ except WebSocketTimeoutException:
+ pass
+ except Exception:
+ traceback.print_exc()
+ end_event.set()
+
+def ws_send(ws, end_event):
+ while not end_event.is_set():
+ try:
+ response = response_queue.get(timeout=1)
+ ws.send(response.json)
+ except Queue.Empty:
+ pass
+ except Exception:
+ traceback.print_exc()
+ end_event.set()
+
+def backoff(retries):
+ return random.randrange(0, min(128, int(2 ** retries)))
+
+def main(gctx=None):
+ params = Params()
+ dongle_id = params.get("DongleId")
+ access_token = params.get("AccessToken")
+ ws_uri = ATHENA_HOST + "/ws/" + dongle_id
+
+ crash.bind_user(id=dongle_id)
+ crash.bind_extra(version=version, dirty=dirty, is_eon=True)
+ crash.install()
+
+ conn_retries = 0
+ while 1:
+ try:
+ print("connecting to %s" % ws_uri)
+ ws = create_connection(ws_uri,
+ cookie="jwt=" + access_token,
+ enable_multithread=True)
+ ws.settimeout(1)
+ conn_retries = 0
+ handle_long_poll(ws)
+ except (KeyboardInterrupt, SystemExit):
+ break
+ except Exception:
+ conn_retries += 1
+ traceback.print_exc()
+
+ time.sleep(backoff(conn_retries))
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index 74b1f4c5d..dd89d4d6d 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -42,6 +42,7 @@
#define SAFETY_HYUNDAI 7
#define SAFETY_TESLA 8
#define SAFETY_CHRYSLER 9
+#define SAFETY_SUBARU 10
#define SAFETY_TOYOTA_IPAS 0x1335
#define SAFETY_TOYOTA_NOLIMITS 0x1336
#define SAFETY_ALLOUTPUT 0x1337
@@ -124,6 +125,9 @@ void *safety_setter_thread(void *s) {
case (int)cereal::CarParams::SafetyModels::CHRYSLER:
safety_setting = SAFETY_CHRYSLER;
break;
+ case (int)cereal::CarParams::SafetyModels::SUBARU:
+ safety_setting = SAFETY_SUBARU;
+ break;
default:
LOGE("unknown safety model: %d", safety_model);
}
@@ -331,6 +335,11 @@ void can_send(void *s) {
capnp::FlatArrayMessageReader cmsg(amsg);
cereal::Event::Reader event = cmsg.getRoot();
+ if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
+ //Older than 1 second. Dont send.
+ zmq_msg_close(&msg);
+ return;
+ }
int msg_count = event.getCan().size();
uint32_t *send = (uint32_t*)malloc(msg_count*0x10);
diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h
index f8d07ae68..4f0972284 100644
--- a/selfdrive/can/common.h
+++ b/selfdrive/can/common.h
@@ -10,6 +10,7 @@
unsigned int honda_checksum(unsigned int address, uint64_t d, int l);
unsigned int toyota_checksum(unsigned int address, uint64_t d, int l);
+unsigned int pedal_checksum(unsigned int address, uint64_t d, int l);
struct SignalPackValue {
const char* name;
@@ -41,6 +42,8 @@ enum SignalType {
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
+ PEDAL_CHECKSUM,
+ PEDAL_COUNTER,
};
struct Signal {
diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc
index 2d4fb9570..776403b22 100644
--- a/selfdrive/can/dbc_template.cc
+++ b/selfdrive/can/dbc_template.cc
@@ -27,6 +27,10 @@ const Signal sigs_{{address}}[] = {
.type = SignalType::HONDA_COUNTER,
{% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %}
.type = SignalType::TOYOTA_CHECKSUM,
+ {% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %}
+ .type = SignalType::PEDAL_CHECKSUM,
+ {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %}
+ .type = SignalType::PEDAL_COUNTER,
{% else %}
.type = SignalType::DEFAULT,
{% endif %}
diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py
index 09f5e5121..cf712cf63 100644
--- a/selfdrive/can/libdbc_py.py
+++ b/selfdrive/can/libdbc_py.py
@@ -39,6 +39,8 @@ typedef enum {
HONDA_CHECKSUM,
HONDA_COUNTER,
TOYOTA_CHECKSUM,
+ PEDAL_CHECKSUM,
+ PEDAL_COUNTER,
} SignalType;
typedef struct {
diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc
index 7bb4ef26f..b6845b47d 100644
--- a/selfdrive/can/parser.cc
+++ b/selfdrive/can/parser.cc
@@ -51,6 +51,30 @@ unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) {
return s & 0xFF;
}
+unsigned int pedal_checksum(unsigned int address, uint64_t d, int l) {
+ uint8_t crc = 0xFF;
+ uint8_t poly = 0xD5; // standard crc8
+
+ d >>= ((8-l)*8); // remove padding
+ d >>= 8; // remove checksum
+
+ uint8_t *dat = (uint8_t *)&d;
+
+ int i, j;
+ for (i = 0; i < l - 1; i++) {
+ crc ^= dat[i];
+ for (j = 0; j < 8; j++) {
+ if ((crc & 0x80) != 0) {
+ crc = (uint8_t)((crc << 1) ^ poly);
+ }
+ else {
+ crc <<= 1;
+ }
+ }
+ }
+ return crc;
+}
+
namespace {
uint64_t read_u64_be(const uint8_t* v) {
@@ -113,16 +137,23 @@ struct MessageState {
return false;
}
} else if (sig.type == SignalType::HONDA_COUNTER) {
- if (!honda_update_counter(tmp)) {
+ if (!update_counter_generic(tmp, sig.b2)) {
return false;
}
} else if (sig.type == SignalType::TOYOTA_CHECKSUM) {
- // INFO("CHECKSUM %d %d %018llX - %lld vs %d\n", address, size, dat, tmp, toyota_checksum(address, dat, size));
-
if (toyota_checksum(address, dat, size) != tmp) {
INFO("%X CHECKSUM FAIL\n", address);
return false;
}
+ } else if (sig.type == SignalType::PEDAL_CHECKSUM) {
+ if (pedal_checksum(address, dat, size) != tmp) {
+ INFO("%X PEDAL CHECKSUM FAIL\n", address);
+ return false;
+ }
+ } else if (sig.type == SignalType::PEDAL_COUNTER) {
+ if (!update_counter_generic(tmp, sig.b2)) {
+ return false;
+ }
}
vals[i] = tmp * sig.factor + sig.offset;
@@ -134,10 +165,10 @@ struct MessageState {
}
- bool honda_update_counter(int64_t v) {
+ bool update_counter_generic(int64_t v, int cnt_size) {
uint8_t old_counter = counter;
counter = v;
- if (((old_counter+1) & 3) != v) {
+ if (((old_counter+1) & ((1 << cnt_size) -1)) != v) {
counter_fail += 1;
if (counter_fail > 1) {
INFO("%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v);
diff --git a/selfdrive/can/parser.py b/selfdrive/can/parser.py
index 165673cea..8960953ed 100644
--- a/selfdrive/can/parser.py
+++ b/selfdrive/can/parser.py
@@ -4,8 +4,12 @@ import numbers
from selfdrive.can.libdbc_py import libdbc, ffi
+
class CANParser(object):
- def __init__(self, dbc_name, signals, checks=[], bus=0, sendcan=False, tcp_addr="127.0.0.1"):
+ def __init__(self, dbc_name, signals, checks=None, bus=0, sendcan=False, tcp_addr="127.0.0.1"):
+ if checks is None:
+ checks = []
+
self.can_valid = True
self.vl = defaultdict(dict)
self.ts = defaultdict(dict)
diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py
index 331e0a313..810947c8e 100755
--- a/selfdrive/can/process_dbc.py
+++ b/selfdrive/can/process_dbc.py
@@ -50,7 +50,11 @@ for address, msg_name, msg_size, sigs in msgs:
sys.exit("COUNTER is not 2 bits longs %s" % msg_name)
if sig.start_bit % 8 != 5:
sys.exit("COUNTER starts at wrong bit %s" % msg_name)
-
+ if address in [0x200, 0x201]:
+ if sig.name == "COUNTER_PEDAL" and sig.size != 4:
+ sys.exit("PEDAL COUNTER is not 4 bits longs %s" % msg_name)
+ if sig.name == "CHECKSUM_PEDAL" and sig.size != 8:
+ sys.exit("PEDAL CHECKSUM is not 8 bits longs %s" % msg_name)
# Fail on duplicate message names
c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs])
diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py
index f5c30921a..255b20806 100644
--- a/selfdrive/car/__init__.py
+++ b/selfdrive/car/__init__.py
@@ -45,3 +45,39 @@ def apply_toyota_steer_torque_limits(apply_torque, apply_torque_last, motor_torq
min(apply_torque_last + LIMITS.STEER_DELTA_DOWN, LIMITS.STEER_DELTA_UP))
return int(round(apply_torque))
+
+
+def crc8_pedal(data):
+ crc = 0xFF # standard init value
+ poly = 0xD5 # standard crc8: x8+x7+x6+x4+x2+1
+ size = len(data)
+ for i in range(size-1, -1, -1):
+ crc ^= data[i]
+ for j in range(8):
+ if ((crc & 0x80) != 0):
+ crc = ((crc << 1) ^ poly) & 0xFF
+ else:
+ crc <<= 1
+ return crc
+
+
+def create_gas_command(packer, gas_amount, idx):
+ # Common gas pedal msg generator
+ enable = gas_amount > 0.001
+
+ values = {
+ "ENABLE": enable,
+ "COUNTER_PEDAL": idx & 0xF,
+ }
+
+ if enable:
+ values["GAS_COMMAND"] = gas_amount * 255.
+ values["GAS_COMMAND2"] = gas_amount * 255.
+
+ dat = packer.make_can_msg("GAS_COMMAND", 0, values)[2]
+
+ dat = [ord(i) for i in dat]
+ checksum = crc8_pedal(dat[:-1])
+ values["CHECKSUM_PEDAL"] = checksum
+
+ return packer.make_can_msg("GAS_COMMAND", 0, values)
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index 72f602d9a..a6344027d 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -111,17 +111,17 @@ class CarState(object):
self.v_wheel_rr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RR']
self.v_wheel_rl = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_RL']
self.v_wheel_fr = cp.vl['WHEEL_SPEEDS']['WHEEL_SPEED_FR']
- self.v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
+ v_wheel = (cp.vl['SPEED_1']['SPEED_LEFT'] + cp.vl['SPEED_1']['SPEED_RIGHT']) / 2.
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEERING"]['STEER_ANGLE']
self.angle_steers_rate = cp.vl["STEERING"]['STEERING_RATE']
diff --git a/selfdrive/car/chrysler/chryslercan_test.py b/selfdrive/car/chrysler/chryslercan_test.py
index 85a849661..2edfff9f2 100644
--- a/selfdrive/car/chrysler/chryslercan_test.py
+++ b/selfdrive/car/chrysler/chryslercan_test.py
@@ -1,6 +1,5 @@
import chryslercan
from values import CAR
-from carcontroller import CarController
from selfdrive.can.packer import CANPacker
from cereal import car
diff --git a/selfdrive/car/chrysler/values.py b/selfdrive/car/chrysler/values.py
index 703ba6a51..6e3a24b8f 100644
--- a/selfdrive/car/chrysler/values.py
+++ b/selfdrive/car/chrysler/values.py
@@ -34,7 +34,7 @@ FINGERPRINTS = {
{168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 660: 8, 669: 3, 671: 8, 672: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770:8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1538: 8},
# Based on 0607d2516fc2148f|2019-02-13--23-03-16
{
- 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8
+ 168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8
}
],
CAR.JEEP_CHEROKEE: [
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index c860cfdee..5a8f1f142 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -65,17 +65,17 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WheelSpeed_CG1"]['WhlRl_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rl = cp.vl["WheelSpeed_CG1"]['WhlFr_W_Meas'] * WHEEL_RADIUS
self.v_wheel_rr = cp.vl["WheelSpeed_CG1"]['WhlFl_W_Meas'] * WHEEL_RADIUS
- self.v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
self.v_cruise_pcm = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index d5fa975de..ae97a38b9 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -79,10 +79,13 @@ class CarState(object):
self.v_wheel_fr = pt_cp.vl["EBCMWheelSpdFront"]['FRWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rl = pt_cp.vl["EBCMWheelSpdRear"]['RLWheelSpd'] * CV.KPH_TO_MS
self.v_wheel_rr = pt_cp.vl["EBCMWheelSpdRear"]['RRWheelSpd'] * CV.KPH_TO_MS
- speed_estimate = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
- self.v_ego_raw = speed_estimate
- v_ego_x = self.v_ego_kf.update(speed_estimate)
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = [[v_wheel], [0.0]]
+
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py
index f87f56e49..08c0235fd 100644
--- a/selfdrive/car/honda/carcontroller.py
+++ b/selfdrive/car/honda/carcontroller.py
@@ -3,6 +3,7 @@ from common.realtime import sec_since_boot
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.controls.lib.drive_helpers import rate_limit
from common.numpy_fast import clip
+from selfdrive.car import create_gas_command
from selfdrive.car.honda import hondacan
from selfdrive.car.honda.values import AH, CruiseButtons, CAR
from selfdrive.can.packer import CANPacker
@@ -170,7 +171,7 @@ class CarController(object):
else:
# Send gas and brake commands.
if (frame % 2) == 0:
- idx = (frame / 2) % 4
+ idx = frame / 2
pump_on, self.last_pump_ts = brake_pump_hysteresys(apply_brake, self.apply_brake_last, self.last_pump_ts)
can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on,
pcm_override, pcm_cancel_cmd, hud.chime, hud.fcw, idx))
@@ -179,6 +180,6 @@ class CarController(object):
if CS.CP.enableGasInterceptor:
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
- can_sends.append(hondacan.create_gas_command(self.packer, apply_gas, idx))
+ can_sends.append(create_gas_command(self.packer, apply_gas, idx))
sendcan.send(can_list_to_can_capnp(can_sends, msgtype='sendcan').to_bytes())
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 00aa358d8..f4c7cade8 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -215,15 +215,15 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS * speed_factor
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS * speed_factor
- self.v_wheel = (self.v_wheel_fl+self.v_wheel_fr+self.v_wheel_rl+self.v_wheel_rr)/4.
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr)/4.
# blend in transmission speed at low speed, since it has more low speed accuracy
- self.v_weight = interp(self.v_wheel, v_weight_bp, v_weight_v)
+ self.v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
speed = (1. - self.v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + \
- self.v_weight * self.v_wheel
+ self.v_weight * v_wheel
if abs(speed - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = [[speed], [0.0]]
+ self.v_ego_kf.x = [[speed], [0.0]]
self.v_ego_raw = speed
v_ego_x = self.v_ego_kf.update(speed)
diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py
index 3d92d24a7..435360d65 100644
--- a/selfdrive/car/honda/hondacan.py
+++ b/selfdrive/car/honda/hondacan.py
@@ -41,18 +41,6 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_
return packer.make_can_msg("BRAKE_COMMAND", 0, values, idx)
-def create_gas_command(packer, gas_amount, idx):
- enable = gas_amount > 0.001
-
- values = {"ENABLE": enable}
-
- if enable:
- values["GAS_COMMAND"] = gas_amount * 255.
- values["GAS_COMMAND2"] = gas_amount * 255.
-
- return packer.make_can_msg("GAS_COMMAND", 0, values, idx)
-
-
def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, idx):
values = {
"STEER_TORQUE": apply_steer if lkas_active else 0,
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 3b423ae04..bd3e63740 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -167,22 +167,22 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHL_SPD11"]['WHL_SPD_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHL_SPD11"]['WHL_SPD_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHL_SPD11"]['WHL_SPD_RR'] * CV.KPH_TO_MS
- self.v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
- self.low_speed_lockout = self.v_wheel < 1.0
+ self.low_speed_lockout = v_wheel < 1.0
# Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
is_set_speed_in_mph = int(cp.vl["CLU11"]["CF_Clu_SPEED_UNIT"])
speed_conv = CV.MPH_TO_MS if is_set_speed_in_mph else CV.KPH_TO_MS
self.cruise_set_speed = cp.vl["SCC11"]['VSetDis'] * speed_conv
- self.standstill = not self.v_wheel > 0.1
+ self.standstill = not v_wheel > 0.1
self.angle_steers = cp.vl["SAS11"]['SAS_Angle']
self.angle_steers_rate = cp.vl["SAS11"]['SAS_Speed']
diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py
index a854eebbc..d7fc2bd39 100644
--- a/selfdrive/car/hyundai/interface.py
+++ b/selfdrive/car/hyundai/interface.py
@@ -87,7 +87,6 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.KIA_SORENTO:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 1985 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.1 # 10% higher at the center seems reasonable
@@ -96,7 +95,6 @@ class CarInterface(object):
ret.minSteerSpeed = 0.
elif candidate == CAR.ELANTRA:
ret.steerKf = 0.00006
- ret.steerRateCost = 0.5
ret.mass = 1275 + std_cargo
ret.wheelbase = 2.7
ret.steerRatio = 13.73 #Spec
@@ -106,7 +104,6 @@ class CarInterface(object):
ret.minSteerSpeed = 32 * CV.MPH_TO_MS
elif candidate == CAR.GENESIS:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 2060 + std_cargo
ret.wheelbase = 3.01
ret.steerRatio = 16.5
@@ -123,7 +120,6 @@ class CarInterface(object):
ret.steerKpV, ret.steerKiV = [[0.25], [0.05]]
elif candidate == CAR.KIA_STINGER:
ret.steerKf = 0.00005
- ret.steerRateCost = 0.5
ret.mass = 1825 + std_cargo
ret.wheelbase = 2.78
ret.steerRatio = 14.4 * 1.15 # 15% higher at the center seems reasonable
diff --git a/selfdrive/car/subaru/__init__.py b/selfdrive/car/subaru/__init__.py
new file mode 100644
index 000000000..8b1378917
--- /dev/null
+++ b/selfdrive/car/subaru/__init__.py
@@ -0,0 +1 @@
+
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
new file mode 100644
index 000000000..e12f5f957
--- /dev/null
+++ b/selfdrive/car/subaru/carstate.py
@@ -0,0 +1,104 @@
+import numpy as np
+from common.kalman.simple_kalman import KF1D
+from selfdrive.config import Conversions as CV
+from selfdrive.can.parser import CANParser
+from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD
+
+def get_powertrain_can_parser(CP):
+ # this function generates lists for signal, messages and initial values
+ signals = [
+ # sig_name, sig_address, default
+ ("Steer_Torque_Sensor", "Steering_Torque", 0),
+ ("Steering_Angle", "Steering_Torque", 0),
+ ("Cruise_On", "CruiseControl", 0),
+ ("Cruise_Activated", "CruiseControl", 0),
+ ("Brake_Pedal", "Brake_Pedal", 0),
+ ("Throttle_Pedal", "Throttle", 0),
+ ("LEFT_BLINKER", "Dashlights", 0),
+ ("RIGHT_BLINKER", "Dashlights", 0),
+ ("SEATBELT_FL", "Dashlights", 0),
+ ("FL", "Wheel_Speeds", 0),
+ ("FR", "Wheel_Speeds", 0),
+ ("RL", "Wheel_Speeds", 0),
+ ("RR", "Wheel_Speeds", 0),
+ ("DOOR_OPEN_FR", "BodyInfo", 1),
+ ("DOOR_OPEN_FL", "BodyInfo", 1),
+ ("DOOR_OPEN_RR", "BodyInfo", 1),
+ ("DOOR_OPEN_RL", "BodyInfo", 1),
+ ]
+
+ checks = [
+ # sig_address, frequency
+ ("Dashlights", 10),
+ ("CruiseControl", 20),
+ ("Wheel_Speeds", 50),
+ ("Steering_Torque", 100),
+ ("BodyInfo", 10),
+ ]
+
+ return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, 0)
+
+class CarState(object):
+ def __init__(self, CP):
+ # initialize can parser
+ self.CP = CP
+
+ self.car_fingerprint = CP.carFingerprint
+ self.left_blinker_on = False
+ self.prev_left_blinker_on = False
+ self.right_blinker_on = False
+ self.prev_right_blinker_on = False
+ self.steer_torque_driver = 0
+ self.steer_not_allowed = False
+ self.main_on = False
+
+ # vEgo kalman filter
+ dt = 0.01
+ self.v_ego_kf = KF1D(x0=np.matrix([[0.], [0.]]),
+ A=np.matrix([[1., dt], [0., 1.]]),
+ C=np.matrix([1., 0.]),
+ K=np.matrix([[0.12287673], [0.29666309]]))
+ self.v_ego = 0.
+
+ def update(self, cp):
+
+ self.can_valid = True
+ self.pedal_gas = cp.vl["Throttle"]['Throttle_Pedal']
+ self.brake_pressure = cp.vl["Brake_Pedal"]['Brake_Pedal']
+ self.user_gas_pressed = self.pedal_gas > 0
+ self.brake_pressed = self.brake_pressure > 0
+ self.brake_lights = bool(self.brake_pressed)
+
+ self.v_wheel_fl = cp.vl["Wheel_Speeds"]['FL'] * CV.KPH_TO_MS
+ self.v_wheel_fr = cp.vl["Wheel_Speeds"]['FR'] * CV.KPH_TO_MS
+ self.v_wheel_rl = cp.vl["Wheel_Speeds"]['RL'] * CV.KPH_TO_MS
+ self.v_wheel_rr = cp.vl["Wheel_Speeds"]['RR'] * CV.KPH_TO_MS
+
+ v_wheel = (self.v_wheel_fl + self.v_wheel_fr + self.v_wheel_rl + self.v_wheel_rr) / 4.
+ # Kalman filter, even though Hyundai raw wheel speed is heaviliy filtered by default
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
+
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
+
+ self.v_ego = float(v_ego_x[0])
+ self.a_ego = float(v_ego_x[1])
+ self.standstill = self.v_ego_raw < 0.01
+
+ self.prev_left_blinker_on = self.left_blinker_on
+ self.prev_right_blinker_on = self.right_blinker_on
+ self.left_blinker_on = cp.vl["Dashlights"]['LEFT_BLINKER'] == 1
+ self.right_blinker_on = cp.vl["Dashlights"]['RIGHT_BLINKER'] == 1
+ self.seatbelt_unlatched = cp.vl["Dashlights"]['SEATBELT_FL'] == 1
+ self.steer_torque_driver = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
+ self.acc_active = cp.vl["CruiseControl"]['Cruise_Activated']
+ self.main_on = cp.vl["CruiseControl"]['Cruise_On']
+ self.steer_override = abs(self.steer_torque_driver) > STEER_THRESHOLD[self.car_fingerprint]
+ self.angle_steers = cp.vl["Steering_Torque"]['Steering_Angle']
+ self.door_open = any([cp.vl["BodyInfo"]['DOOR_OPEN_RR'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_RL'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_FR'],
+ cp.vl["BodyInfo"]['DOOR_OPEN_FL']])
+
+
diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py
new file mode 100644
index 000000000..93a1ac62b
--- /dev/null
+++ b/selfdrive/car/subaru/interface.py
@@ -0,0 +1,205 @@
+#!/usr/bin/env python
+from cereal import car
+from common.realtime import sec_since_boot
+from selfdrive.config import Conversions as CV
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.car.subaru.values import CAR
+from selfdrive.car.subaru.carstate import CarState, get_powertrain_can_parser
+
+try:
+ from selfdrive.car.subaru.carcontroller import CarController
+except ImportError:
+ CarController = None
+
+
+class CarInterface(object):
+ def __init__(self, CP, sendcan=None):
+ self.CP = CP
+
+ self.frame = 0
+ self.can_invalid_count = 0
+ self.acc_active_prev = 0
+
+ # *** init the major players ***
+ self.CS = CarState(CP)
+ self.VM = VehicleModel(CP)
+ self.pt_cp = get_powertrain_can_parser(CP)
+
+ # sending if read only is False
+ if sendcan is not None:
+ self.sendcan = sendcan
+ self.CC = CarController(CP.carFingerprint)
+
+ @staticmethod
+ def compute_gb(accel, speed):
+ return float(accel) / 4.0
+
+ @staticmethod
+ def calc_accel_override(a_ego, a_target, v_ego, v_target):
+ return 1.0
+
+ @staticmethod
+ def get_params(candidate, fingerprint):
+ ret = car.CarParams.new_message()
+
+ ret.carName = "subaru"
+ ret.carFingerprint = candidate
+ ret.safetyModel = car.CarParams.SafetyModels.subaru
+
+ ret.enableCruise = False
+ ret.steerLimitAlert = True
+ ret.enableCamera = True
+
+ std_cargo = 136
+ ret.steerRateCost = 0.7
+
+ if candidate in [CAR.IMPREZA]:
+ ret.mass = 1568 + std_cargo
+ ret.wheelbase = 2.67
+ ret.centerToFront = ret.wheelbase * 0.5
+ ret.steerRatio = 15
+ tire_stiffness_factor = 1.0
+ ret.steerActuatorDelay = 0.4 # end-to-end angle controller
+ ret.steerKf = 0.00005
+ ret.steerKiBP, ret.steerKpBP = [[0., 20.], [0., 20.]]
+ ret.steerKpV, ret.steerKiV = [[0.2, 0.3], [0.02, 0.03]]
+ ret.steerMaxBP = [0.] # m/s
+ ret.steerMaxV = [1.]
+
+ ret.steerControlType = car.CarParams.SteerControlType.torque
+ ret.steerRatioRear = 0.
+ # testing tuning
+
+ # No long control in subaru
+ ret.gasMaxBP = [0.]
+ ret.gasMaxV = [0.]
+ ret.brakeMaxBP = [0.]
+ ret.brakeMaxV = [0.]
+ ret.longPidDeadzoneBP = [0.]
+ ret.longPidDeadzoneV = [0.]
+ ret.longitudinalKpBP = [0.]
+ ret.longitudinalKpV = [0.]
+ ret.longitudinalKiBP = [0.]
+ ret.longitudinalKiV = [0.]
+
+ # end from gm
+
+ # hardcoding honda civic 2016 touring params so they can be used to
+ # scale unknown params for other cars
+ mass_civic = 2923./2.205 + std_cargo
+ wheelbase_civic = 2.70
+ centerToFront_civic = wheelbase_civic * 0.4
+ centerToRear_civic = wheelbase_civic - centerToFront_civic
+ rotationalInertia_civic = 2500
+ tireStiffnessFront_civic = 192150
+ tireStiffnessRear_civic = 202500
+ centerToRear = ret.wheelbase - ret.centerToFront
+
+ # TODO: get actual value, for now starting with reasonable value for
+ # civic and scaling by mass and wheelbase
+ ret.rotationalInertia = rotationalInertia_civic * \
+ ret.mass * ret.wheelbase**2 / (mass_civic * wheelbase_civic**2)
+
+ # TODO: start from empirically derived lateral slip stiffness for the civic and scale by
+ # mass and CG position, so all cars will have approximately similar dyn behaviors
+ ret.tireStiffnessFront = (tireStiffnessFront_civic * tire_stiffness_factor) * \
+ ret.mass / mass_civic * \
+ (centerToRear / ret.wheelbase) / (centerToRear_civic / wheelbase_civic)
+ ret.tireStiffnessRear = (tireStiffnessRear_civic * tire_stiffness_factor) * \
+ ret.mass / mass_civic * \
+ (ret.centerToFront / ret.wheelbase) / (centerToFront_civic / wheelbase_civic)
+
+ return ret
+
+ # returns a car.CarState
+ def update(self, c):
+
+ self.pt_cp.update(int(sec_since_boot() * 1e9), False)
+ self.CS.update(self.pt_cp)
+
+ # create message
+ ret = car.CarState.new_message()
+
+ # speeds
+ ret.vEgo = self.CS.v_ego
+ ret.aEgo = self.CS.a_ego
+ ret.vEgoRaw = self.CS.v_ego_raw
+ ret.yawRate = self.VM.yaw_rate(self.CS.angle_steers * CV.DEG_TO_RAD, self.CS.v_ego)
+ ret.standstill = self.CS.standstill
+ ret.wheelSpeeds.fl = self.CS.v_wheel_fl
+ ret.wheelSpeeds.fr = self.CS.v_wheel_fr
+ ret.wheelSpeeds.rl = self.CS.v_wheel_rl
+ ret.wheelSpeeds.rr = self.CS.v_wheel_rr
+
+ # steering wheel
+ ret.steeringAngle = self.CS.angle_steers
+
+ # torque and user override. Driver awareness
+ # timer resets when the user uses the steering wheel.
+ ret.steeringPressed = self.CS.steer_override
+ ret.steeringTorque = self.CS.steer_torque_driver
+
+ # cruise state
+ ret.cruiseState.available = bool(self.CS.main_on)
+ ret.leftBlinker = self.CS.left_blinker_on
+ ret.rightBlinker = self.CS.right_blinker_on
+ ret.seatbeltUnlatched = self.CS.seatbelt_unlatched
+
+ buttonEvents = []
+
+ # blinkers
+ if self.CS.left_blinker_on != self.CS.prev_left_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'leftBlinker'
+ be.pressed = self.CS.left_blinker_on
+ buttonEvents.append(be)
+
+ if self.CS.right_blinker_on != self.CS.prev_right_blinker_on:
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'rightBlinker'
+ be.pressed = self.CS.right_blinker_on
+ buttonEvents.append(be)
+
+ be = car.CarState.ButtonEvent.new_message()
+ be.type = 'accelCruise'
+ buttonEvents.append(be)
+
+
+ events = []
+ if not self.CS.can_valid:
+ self.can_invalid_count += 1
+ if self.can_invalid_count >= 5:
+ events.append(create_event('commIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
+ else:
+ self.can_invalid_count = 0
+
+ if ret.seatbeltUnlatched:
+ events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
+ if self.CS.acc_active and not self.acc_active_prev:
+ events.append(create_event('pcmEnable', [ET.ENABLE]))
+ if not self.CS.acc_active:
+ events.append(create_event('pcmDisable', [ET.USER_DISABLE]))
+
+ ## handle button presses
+ #for b in ret.buttonEvents:
+ # # do enable on both accel and decel buttons
+ # if b.type in ["accelCruise", "decelCruise"] and not b.pressed:
+ # events.append(create_event('buttonEnable', [ET.ENABLE]))
+ # # do disable on button down
+ # if b.type == "cancel" and b.pressed:
+ # events.append(create_event('buttonCancel', [ET.USER_DISABLE]))
+
+ ret.events = events
+
+ # update previous brake/gas pressed
+ self.acc_active_prev = self.CS.acc_active
+
+
+ # cast to reader so it can't be modified
+ return ret.as_reader()
+
+ def apply(self, c):
+ self.CC.update(self.sendcan, c.enabled, self.CS, self.frame, c.actuators)
+ self.frame += 1
diff --git a/selfdrive/car/subaru/radar_interface.py b/selfdrive/car/subaru/radar_interface.py
new file mode 100644
index 000000000..96159fd87
--- /dev/null
+++ b/selfdrive/car/subaru/radar_interface.py
@@ -0,0 +1,24 @@
+#!/usr/bin/env python
+from cereal import car
+import time
+
+
+class RadarInterface(object):
+ def __init__(self, CP):
+ # radar
+ self.pts = {}
+ self.delay = 0.1
+
+ def update(self):
+
+ ret = car.RadarState.new_message()
+ time.sleep(0.05) # radard runs on RI updates
+
+ return ret
+
+if __name__ == "__main__":
+ RI = RadarInterface(None)
+ while 1:
+ ret = RI.update()
+ print(chr(27) + "[2J")
+ print ret
diff --git a/selfdrive/car/subaru/values.py b/selfdrive/car/subaru/values.py
new file mode 100644
index 000000000..18b892956
--- /dev/null
+++ b/selfdrive/car/subaru/values.py
@@ -0,0 +1,18 @@
+from selfdrive.car import dbc_dict
+
+class CAR:
+ IMPREZA = "SUBARU IMPREZA LIMITED 2019"
+
+FINGERPRINTS = {
+ CAR.IMPREZA: [{
+ 2: 8, 64: 8, 65: 8, 72: 8, 73: 8, 280: 8, 281: 8, 290: 8, 312: 8, 313: 8, 314: 8, 315: 8, 316: 8, 326: 8, 544: 8, 545: 8, 546: 8, 552: 8, 554: 8, 557: 8, 576: 8, 577: 8, 722: 8, 801: 8, 802: 8, 805: 8, 808: 8, 816: 8, 826: 8, 837: 8, 838: 8, 839: 8, 842: 8, 912: 8, 915: 8, 940: 8, 1614: 8, 1617: 8, 1632: 8, 1650: 8, 1657: 8, 1658: 8, 1677: 8, 1697: 8, 1722: 8, 1743: 8, 1759: 8, 1786: 5, 1787: 5, 1788: 8, 1809: 8, 1813: 8, 1817: 8, 1821: 8, 1840: 8, 1848: 8, 1924: 8, 1932: 8, 1952: 8, 1960: 8
+ }],
+}
+
+STEER_THRESHOLD = {
+ CAR.IMPREZA: 80,
+}
+
+DBC = {
+ CAR.IMPREZA: dbc_dict('subaru_global_2017', None),
+}
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 8c1ed1192..cdd1bde51 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -2,10 +2,11 @@ from cereal import car
from common.numpy_fast import clip, interp
from selfdrive.boardd.boardd import can_list_to_can_capnp
from selfdrive.car import apply_toyota_steer_torque_limits
+from selfdrive.car import create_gas_command
from selfdrive.car.toyota.toyotacan import make_can_msg, create_video_target,\
create_steer_command, create_ui_command, \
create_ipas_steer_command, create_accel_command, \
- create_fcw_command, create_gas_command
+ create_fcw_command
from selfdrive.car.toyota.values import ECU, STATIC_MSGS
from selfdrive.can.packer import CANPacker
@@ -222,10 +223,10 @@ class CarController(object):
else:
can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead))
- if CS.CP.enableGasInterceptor:
+ if (frame % 2 == 0) and (CS.CP.enableGasInterceptor):
# send exactly zero if apply_gas is zero. Interceptor will send the max between read value and apply_gas.
# This prevents unexpected pedal range rescaling
- can_sends.append(create_gas_command(self.packer, apply_gas))
+ can_sends.append(create_gas_command(self.packer, apply_gas, frame/2))
if frame % 10 == 0 and ECU.CAM in self.fake_ecus and not forwarding_camera:
for addr in TARGET_IDS:
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index bb3b3f052..02debc47b 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -62,7 +62,7 @@ def get_can_parser(CP):
if CP.carFingerprint == CAR.PRIUS:
signals += [("STATE", "AUTOPARK_STATUS", 0)]
-
+
# add gas interceptor reading if we are using it
if CP.enableGasInterceptor:
signals.append(("INTERCEPTOR_GAS", "GAS_SENSOR", 0))
@@ -129,17 +129,17 @@ class CarState(object):
self.v_wheel_fr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_FR'] * CV.KPH_TO_MS
self.v_wheel_rl = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RL'] * CV.KPH_TO_MS
self.v_wheel_rr = cp.vl["WHEEL_SPEEDS"]['WHEEL_SPEED_RR'] * CV.KPH_TO_MS
- self.v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
+ v_wheel = float(np.mean([self.v_wheel_fl, self.v_wheel_fr, self.v_wheel_rl, self.v_wheel_rr]))
# Kalman filter
- if abs(self.v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
- self.v_ego_x = np.matrix([[self.v_wheel], [0.0]])
+ if abs(v_wheel - self.v_ego) > 2.0: # Prevent large accelerations when car starts at non zero speed
+ self.v_ego_kf.x = np.matrix([[v_wheel], [0.0]])
- self.v_ego_raw = self.v_wheel
- v_ego_x = self.v_ego_kf.update(self.v_wheel)
+ self.v_ego_raw = v_wheel
+ v_ego_x = self.v_ego_kf.update(v_wheel)
self.v_ego = float(v_ego_x[0])
self.a_ego = float(v_ego_x[1])
- self.standstill = not self.v_wheel > 0.001
+ self.standstill = not v_wheel > 0.001
self.angle_steers = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
self.angle_steers_rate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py
index ae257940a..720c0ef60 100644
--- a/selfdrive/car/toyota/toyotacan.py
+++ b/selfdrive/car/toyota/toyotacan.py
@@ -78,18 +78,6 @@ def create_accel_command(packer, accel, pcm_cancel, standstill_req, lead):
}
return packer.make_can_msg("ACC_CONTROL", 0, values)
-def create_gas_command(packer, gas_amount):
- """Creates a CAN message for the Pedal DBC GAS_COMMAND."""
- enable = gas_amount > 0.001
-
- values = {"ENABLE": enable}
-
- if enable:
- values["GAS_COMMAND"] = gas_amount * 255.
- values["GAS_COMMAND2"] = gas_amount * 255.
-
- return packer.make_can_msg("GAS_COMMAND", 0, values)
-
def create_fcw_command(packer, fcw):
values = {
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index ec127c941..2f3f44448 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -89,23 +89,12 @@ FINGERPRINTS = {
36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 547: 8, 548: 8, 552: 4, 562: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 742: 8, 743: 8, 800: 8, 830: 7, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1207: 8, 1227: 8, 1235: 8, 1263: 8, 1279: 8, 1552: 8, 1553: 8, 1554: 8, 1555: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1745: 8, 1779: 8
}],
CAR.PRIUS: [{
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # Prius Prime
- {
- 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # Taiwanese Prius Prime
- {
+ # with ipas
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 512: 6, 513: 6, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 614: 8, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 814: 8, 824: 2, 829: 2, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2,898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 974: 8, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1082: 8, 1083: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1175: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1777: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
#Corolla w/ added Pedal Support (512L and 513L)
CAR.COROLLA: [{
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
- },
- # Corolla LE 2017 w/ added Pedal Support (512L and 513L)
- {
- 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
+ 36: 8, 37: 8, 170: 8, 180: 8, 186: 4, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 512: 6, 513: 6, 547: 8, 548: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 897: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 2, 921: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 4, 956: 8, 979: 2, 992: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1017: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1196: 8, 1227: 8, 1235: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1596: 8, 1597: 8, 1600: 8, 1664: 8, 1728: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8, 2016: 8, 2017: 8, 2018: 8, 2019: 8, 2020: 8, 2021: 8, 2022: 8, 2023: 8, 2024: 8
}],
CAR.LEXUS_RXH: [{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 5, 643: 7, 658: 8, 713: 8, 740: 5, 742: 8, 743: 8, 800: 8, 810: 2, 812: 3, 814: 8, 830: 7, 835: 8, 836: 8, 845: 5, 863: 8, 869: 7, 870: 7, 871: 2, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 6, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1005: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1059: 1, 1063: 8, 1071: 8, 1077: 8, 1082: 8, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1227: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1575: 8, 1595: 8, 1777: 8, 1779: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1840: 8, 1848: 8, 1904: 8, 1912: 8, 1940: 8, 1941: 8, 1948: 8, 1949: 8, 1952: 8, 1956: 8, 1960: 8, 1964: 8, 1986: 8, 1990: 8, 1994: 8, 1998: 8, 2004: 8, 2012: 8
@@ -133,6 +122,10 @@ FINGERPRINTS = {
#LE and LE with Blindspot Monitor
{
36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 983: 8, 984: 8, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1011: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1808: 8, 1810: 8, 1816: 8, 1818: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
+ },
+ #SL
+ {
+ 36: 8, 37: 8, 166: 8, 170: 8, 180: 8, 295: 8, 296: 8, 426: 6, 452: 8, 466: 8, 467: 8, 550: 8, 552: 4, 560: 7, 562: 6, 581: 5, 608: 8, 610: 8, 643: 7, 713: 8, 728: 8, 740: 5, 761: 8, 764: 8, 800: 8, 810: 2, 812: 8, 818: 8, 824: 8, 829: 2, 830: 7, 835: 8, 836: 8, 869: 7, 870: 7, 871: 2, 888: 8, 889: 8, 898: 8, 900: 6, 902: 6, 905: 8, 913: 8, 918: 8, 921: 8, 933: 8, 934: 8, 935: 8, 944: 8, 945: 8, 950: 8, 951: 8, 953: 8, 955: 8, 956: 8, 971: 7, 975: 5, 993: 8, 998: 5, 999: 7, 1000: 8, 1001: 8, 1002: 8, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1044: 8, 1056: 8, 1057: 8, 1059: 1, 1071: 8, 1076: 8, 1077: 8, 1084: 8, 1085: 8, 1086: 8, 1114: 8, 1132: 8, 1161: 8, 1162: 8, 1163: 8, 1164: 8, 1165: 8, 1166: 8, 1167: 8, 1228: 8, 1235: 8, 1237: 8, 1264: 8, 1279: 8, 1541: 8, 1552: 8, 1553: 8, 1556: 8, 1557: 8, 1568: 8, 1570: 8, 1571: 8, 1572: 8, 1595: 8, 1745: 8, 1779: 8, 1786: 8, 1787: 8, 1788: 8, 1789: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
}],
CAR.HIGHLANDER: [{
36: 8, 37: 8, 114: 5, 119: 6, 120: 4, 170: 8, 180: 8, 186: 4, 238: 4, 355: 5, 426: 6, 452: 8, 464: 8, 466: 8, 467: 8, 544: 4, 545: 5, 550: 8, 552: 4, 608: 8, 610: 5, 643: 7, 705: 8, 725: 2, 740: 5, 800: 8, 835: 8, 836: 8, 849: 4, 869: 7, 870: 7, 871: 2, 896: 8, 900: 6, 902: 6, 905: 8, 911: 8, 916: 3, 921: 8, 922: 8, 933: 8, 944: 8, 945: 8, 951: 8, 955: 8, 956: 8, 979: 2, 998: 5, 999: 7, 1000: 8, 1001: 8, 1008: 2, 1014: 8, 1017: 8, 1020: 8, 1041: 8, 1042: 8, 1043: 8, 1044: 8, 1056: 8, 1059: 1, 1114: 8, 1161: 8, 1162: 8, 1163: 8, 1176: 8, 1177: 8, 1178: 8, 1179: 8, 1180: 8, 1181: 8, 1182: 8, 1183: 8, 1189: 8, 1190: 8, 1191: 8, 1192: 8, 1196: 8, 1197: 8, 1198: 8, 1199: 8, 1206: 8, 1207: 8, 1212: 8, 1227: 8, 1235: 8, 1237: 8, 1279: 8, 1408: 8, 1409: 8, 1410: 8, 1552: 8, 1553: 8, 1554: 8, 1556: 8, 1557: 8, 1561: 8, 1562: 8, 1568: 8, 1569: 8, 1570: 8, 1571: 8, 1572: 8, 1584: 8, 1589: 8, 1592: 8, 1593: 8, 1595: 8, 1599: 8, 1656: 8, 1666: 8, 1667: 8, 1728: 8, 1745: 8, 1779: 8, 1904: 8, 1912: 8, 1990: 8, 1998: 8
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index b1deb2116..b1cb55d12 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.5.9-release"
+#define COMMA_VERSION "0.5.10-release"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index 7a17556ec..b5d699bda 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -11,7 +11,7 @@ import selfdrive.messaging as messaging
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
from selfdrive.car.car_helpers import get_car
-from selfdrive.controls.lib.drive_helpers import learn_angle_offset, \
+from selfdrive.controls.lib.drive_helpers import learn_angle_model_bias, \
get_events, \
create_event, \
EventTypes as ET, \
@@ -202,7 +202,7 @@ def state_transition(CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM
def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk,
- driver_status, LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc):
+ driver_status, LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc):
"""Given the state, this function returns an actuators packet"""
actuators = car.CarControl.Actuators.new_message()
@@ -241,7 +241,7 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
# Run angle offset learner at 20 Hz
if rk.frame % 5 == 2:
- angle_offset = learn_angle_offset(active, CS.vEgo, angle_offset,
+ angle_model_bias = learn_angle_model_bias(active, CS.vEgo, angle_model_bias,
path_plan.cPoly, path_plan.cProb, CS.steeringAngle,
CS.steeringPressed)
@@ -277,12 +277,12 @@ def state_control(plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise
AM.process_alerts(sec_since_boot())
- return actuators, v_cruise_kph, driver_status, angle_offset, v_acc_sol, a_acc_sol
+ return actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc_sol, a_acc_sol
def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate,
carcontrol, live100, AM, driver_status,
- LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc):
+ LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc):
"""Send actuators and hud commands to the car, send live100 and MPC logging"""
plan_ts = plan.logMonoTime
plan = plan.plan
@@ -353,7 +353,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
"vTargetLead": float(v_acc),
"aTarget": float(a_acc),
"jerkFactor": float(plan.jerkFactor),
- "angleOffset": float(angle_offset),
+ "angleModelBias": float(angle_model_bias),
"gpsPlannerActive": plan.gpsPlannerActive,
"vCurvature": plan.vCurvature,
"decelForTurn": plan.decelForTurn,
@@ -378,7 +378,7 @@ def data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruis
carcontrol.send(cc_send.to_bytes())
if (rk.frame % 36000) == 0: # update angle offset every 6 minutes
- params.put("ControlsParams", json.dumps({'angle_offset': angle_offset}))
+ params.put("ControlsParams", json.dumps({'angle_model_bias': angle_model_bias}))
return CC
@@ -461,12 +461,15 @@ def controlsd_thread(gctx=None, rate=100):
rk = Ratekeeper(rate, print_delay_threshold=2. / 1000)
controls_params = params.get("ControlsParams")
+
# Read angle offset from previous drive
+ angle_model_bias = 0.
if controls_params is not None:
- controls_params = json.loads(controls_params)
- angle_offset = controls_params['angle_offset']
- else:
- angle_offset = 0.
+ try:
+ controls_params = json.loads(controls_params)
+ angle_model_bias = controls_params['angle_model_bias']
+ except (ValueError, KeyError):
+ pass
prof = Profiler(False) # off by default
@@ -485,6 +488,8 @@ def controlsd_thread(gctx=None, rate=100):
plan_age = (start_time - plan.logMonoTime) / 1e9
if not path_plan.pathPlan.valid or plan_age > 0.5 or path_plan_age > 0.5:
events.append(create_event('plannerError', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ if not path_plan.pathPlan.paramsValid:
+ events.append(create_event('vehicleModelInvalid', [ET.WARNING]))
events += list(plan.plan.events)
# Only allow engagement with brake pressed when stopped behind another stopped car
@@ -498,16 +503,16 @@ def controlsd_thread(gctx=None, rate=100):
prof.checkpoint("State transition")
# Compute actuators (runs PID loops and lateral MPC)
- actuators, v_cruise_kph, driver_status, angle_offset, v_acc, a_acc = \
+ actuators, v_cruise_kph, driver_status, angle_model_bias, v_acc, a_acc = \
state_control(plan.plan, path_plan.pathPlan, CS, CP, state, events, v_cruise_kph,
v_cruise_kph_last, AM, rk, driver_status,
- LaC, LoC, VM, angle_offset, passive, is_metric, cal_perc)
+ LaC, LoC, VM, angle_model_bias, passive, is_metric, cal_perc)
prof.checkpoint("State Control")
# Publish data
CC = data_send(plan, path_plan, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, carstate, carcontrol,
- live100, AM, driver_status, LaC, LoC, angle_offset, passive, start_time, params, v_acc, a_acc)
+ live100, AM, driver_status, LaC, LoC, angle_model_bias, passive, start_time, params, v_acc, a_acc)
prof.checkpoint("Sent")
rk.keep_time() # Run at 100Hz
diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py
index cc9b19d5d..17caf7fbf 100644
--- a/selfdrive/controls/lib/alerts.py
+++ b/selfdrive/controls/lib/alerts.py
@@ -624,4 +624,11 @@ ALERTS = [
"Set 0111 for openpilot. 1011 for stock",
AlertStatus.normal, AlertSize.mid,
Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2),
+
+ Alert(
+ "vehicleModelInvalid",
+ "Vehicle Parameter Identification Failed",
+ "",
+ AlertStatus.normal, AlertSize.small,
+ Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1),
]
diff --git a/selfdrive/controls/lib/drive_helpers.py b/selfdrive/controls/lib/drive_helpers.py
index 9d78c9667..15d5a202f 100644
--- a/selfdrive/controls/lib/drive_helpers.py
+++ b/selfdrive/controls/lib/drive_helpers.py
@@ -55,7 +55,7 @@ def rate_limit(new_value, last_value, dw_step, up_step):
return clip(new_value, last_value + dw_step, last_value + up_step)
-def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, angle_steers, steer_override):
+def learn_angle_model_bias(lateral_control, v_ego, angle_model_bias, c_poly, c_prob, angle_steers, steer_override):
# simple integral controller that learns how much steering offset to put to have the car going straight
# while being in the middle of the lane
min_offset = -5. # deg
@@ -69,10 +69,10 @@ def learn_angle_offset(lateral_control, v_ego, angle_offset, c_poly, c_prob, ang
# only learn if lateral control is active and if driver is not overriding:
if lateral_control and not steer_override:
- angle_offset += c_poly[3] * alpha_v
- angle_offset = clip(angle_offset, min_offset, max_offset)
+ angle_model_bias += c_poly[3] * alpha_v
+ angle_model_bias = clip(angle_model_bias, min_offset, max_offset)
- return angle_offset
+ return angle_model_bias
def update_v_cruise(v_cruise_kph, buttonEvents, enabled):
diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py
new file mode 100644
index 000000000..f93a72cfc
--- /dev/null
+++ b/selfdrive/controls/lib/fcw.py
@@ -0,0 +1,71 @@
+import numpy as np
+from collections import defaultdict
+
+from common.numpy_fast import interp
+
+_FCW_A_ACT_V = [-3., -2.]
+_FCW_A_ACT_BP = [0., 30.]
+
+
+class FCWChecker(object):
+ def __init__(self):
+ self.reset_lead(0.0)
+
+ def reset_lead(self, cur_time):
+ self.last_fcw_a = 0.0
+ self.v_lead_max = 0.0
+ self.lead_seen_t = cur_time
+ self.last_fcw_time = 0.0
+ self.last_min_a = 0.0
+
+ self.counters = defaultdict(lambda: 0)
+
+ @staticmethod
+ def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
+ max_ttc = 5.0
+
+ v_rel = v_ego - v_lead
+ a_rel = a_ego - a_lead
+
+ # assuming that closing gap ARel comes from lead vehicle decel,
+ # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
+ # This helps underweighting ARel when v_lead is close to zero.
+ t_decel = 2.
+ a_rel = np.minimum(a_rel, v_lead / t_decel)
+
+ # delta of the quadratic equation to solve for ttc
+ delta = v_rel**2 + 2 * x_lead * a_rel
+
+ # assign an arbitrary high ttc value if there is no solution to ttc
+ if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
+ ttc = max_ttc
+ else:
+ ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
+ return ttc
+
+ def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
+ mpc_solution_a = list(mpc_solution[0].a_ego)
+ self.last_min_a = min(mpc_solution_a)
+ self.v_lead_max = max(self.v_lead_max, v_lead)
+
+ if (fcw_lead > 0.99):
+ ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
+ self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
+ self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
+ self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
+ self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
+ self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
+ self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
+ self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
+ self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
+
+ a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
+ a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
+
+ fcw_allowed = all(c >= 10 for c in self.counters.values())
+ if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
+ self.last_fcw_time = cur_time
+ self.last_fcw_a = self.last_min_a
+ return True
+
+ return False
diff --git a/selfdrive/controls/lib/latcontrol.py b/selfdrive/controls/lib/latcontrol.py
index cc06fd40d..694943870 100644
--- a/selfdrive/controls/lib/latcontrol.py
+++ b/selfdrive/controls/lib/latcontrol.py
@@ -37,6 +37,8 @@ class LatControl(object):
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
+ # TODO: feedforward something based on path_plan.rateSteers
+ steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override,
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
index f4cee7f2d..e5adf02cf 100644
--- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
+++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c
@@ -26,6 +26,7 @@ typedef struct {
double y[N+1];
double psi[N+1];
double delta[N+1];
+ double rate[N+1];
double cost;
} log_t;
@@ -103,7 +104,7 @@ int run_mpc(state_t * x0, log_t * solution,
acado_preparationStep();
acado_feedbackStep();
-
+
/* printf("lat its: %d\n", acado_getNWSR()); // n iterations
printf("Objective: %.6f\n", acado_getObjective()); // solution cost */
@@ -112,6 +113,7 @@ int run_mpc(state_t * x0, log_t * solution,
solution->y[i] = acadoVariables.x[i*NX+1];
solution->psi[i] = acadoVariables.x[i*NX+2];
solution->delta[i] = acadoVariables.x[i*NX+3];
+ solution->rate[i] = acadoVariables.u[i];
}
solution->cost = acado_getObjective();
diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o
index 029fda217..fdd82d242 100644
Binary files a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o and b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o differ
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc.so b/selfdrive/controls/lib/lateral_mpc/libmpc.so
index a0c0dc815..3fd132550 100755
Binary files a/selfdrive/controls/lib/lateral_mpc/libmpc.so and b/selfdrive/controls/lib/lateral_mpc/libmpc.so differ
diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
index 1537d2f6c..3af46c304 100644
--- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py
@@ -18,6 +18,7 @@ typedef struct {
double y[21];
double psi[21];
double delta[21];
+ double rate[21];
double cost;
} log_t;
diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py
new file mode 100644
index 000000000..19fb706de
--- /dev/null
+++ b/selfdrive/controls/lib/long_mpc.py
@@ -0,0 +1,120 @@
+import numpy as np
+
+import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from common.realtime import sec_since_boot
+from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import MPC_COST_LONG
+
+
+class LongitudinalMpc(object):
+ def __init__(self, mpc_id, live_longitudinal_mpc):
+ self.live_longitudinal_mpc = live_longitudinal_mpc
+ self.mpc_id = mpc_id
+
+ self.setup_mpc()
+ self.v_mpc = 0.0
+ self.v_mpc_future = 0.0
+ self.a_mpc = 0.0
+ self.v_cruise = 0.0
+ self.prev_lead_status = False
+ self.prev_lead_x = 0.0
+ self.new_lead = False
+
+ self.last_cloudlog_t = 0.0
+
+ def send_mpc_solution(self, qp_iterations, calculation_time):
+ qp_iterations = max(0, qp_iterations)
+ dat = messaging.new_message()
+ dat.init('liveLongitudinalMpc')
+ dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
+ dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
+ dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
+ dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
+ dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
+ dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
+ dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
+ dat.liveLongitudinalMpc.qpIterations = qp_iterations
+ dat.liveLongitudinalMpc.mpcId = self.mpc_id
+ dat.liveLongitudinalMpc.calculationTime = calculation_time
+ self.live_longitudinal_mpc.send(dat.to_bytes())
+
+ def setup_mpc(self):
+ ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
+ self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+ MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+
+ self.mpc_solution = ffi.new("log_t *")
+ self.cur_state = ffi.new("state_t *")
+ self.cur_state[0].v_ego = 0
+ self.cur_state[0].a_ego = 0
+ self.a_lead_tau = _LEAD_ACCEL_TAU
+
+ def set_cur_state(self, v, a):
+ self.cur_state[0].v_ego = v
+ self.cur_state[0].a_ego = a
+
+ def update(self, CS, lead, v_cruise_setpoint):
+ v_ego = CS.carState.vEgo
+
+ # Setup current mpc state
+ self.cur_state[0].x_ego = 0.0
+
+ if lead is not None and lead.status:
+ x_lead = lead.dRel
+ v_lead = max(0.0, lead.vLead)
+ a_lead = lead.aLeadK
+
+ if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
+ v_lead = 0.0
+ a_lead = 0.0
+
+ self.a_lead_tau = lead.aLeadTau
+ self.new_lead = False
+ if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
+ self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
+ self.new_lead = True
+
+ self.prev_lead_status = True
+ self.prev_lead_x = x_lead
+ self.cur_state[0].x_l = x_lead
+ self.cur_state[0].v_l = v_lead
+ else:
+ self.prev_lead_status = False
+ # Fake a fast lead car, so mpc keeps running
+ self.cur_state[0].x_l = 50.0
+ self.cur_state[0].v_l = v_ego + 10.0
+ a_lead = 0.0
+ self.a_lead_tau = _LEAD_ACCEL_TAU
+
+ # Calculate mpc
+ t = sec_since_boot()
+ n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
+ duration = int((sec_since_boot() - t) * 1e9)
+ self.send_mpc_solution(n_its, duration)
+
+ # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
+ self.v_mpc = self.mpc_solution[0].v_ego[1]
+ self.a_mpc = self.mpc_solution[0].a_ego[1]
+ self.v_mpc_future = self.mpc_solution[0].v_ego[10]
+
+ # Reset if NaN or goes through lead car
+ dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
+ crashing = min(dls) < -50.0
+ nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
+ backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
+
+ if ((backwards or crashing) and self.prev_lead_status) or nans:
+ if t > self.last_cloudlog_t + 5.0:
+ self.last_cloudlog_t = t
+ cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
+ self.mpc_id, backwards, crashing, nans))
+
+ self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
+ MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
+ self.cur_state[0].v_ego = v_ego
+ self.cur_state[0].a_ego = 0.0
+ self.v_mpc = v_ego
+ self.a_mpc = CS.carState.aEgo
+ self.prev_lead_status = False
diff --git a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
index f5c95394d..1f0e28d00 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/generator.cpp
@@ -18,30 +18,21 @@ int main( )
DifferentialEquation f;
DifferentialState x_ego, v_ego, a_ego;
- DifferentialState x_l, v_l, t;
-
- OnlineData lambda, a_l_0;
+ OnlineData x_l, v_l;
Control j_ego;
auto desired = 4.0 + RW(v_ego, v_l);
auto d_l = x_l - x_ego;
- // Directly calculate a_l to prevent instabilites due to discretization
- auto a_l = a_l_0 * exp(-lambda * t * t / 2);
-
// Equations of motion
f << dot(x_ego) == v_ego;
f << dot(v_ego) == a_ego;
f << dot(a_ego) == j_ego;
- f << dot(x_l) == v_l;
- f << dot(v_l) == a_l;
- f << dot(t) == 1;
-
// Running cost
Function h;
- h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+ h << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
h << (d_l - desired) / (0.05 * v_ego + 0.5);
h << a_ego * (0.1 * v_ego + 1.0);
h << j_ego * (0.1 * v_ego + 1.0);
@@ -51,7 +42,7 @@ int main( )
// Terminal cost
Function hN;
- hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l)) - exp(0.3 * NORM_RW_ERROR(v_ego, v_l, desired));
+ hN << exp(0.3 * NORM_RW_ERROR(v_ego, v_l, d_l));
hN << (d_l - desired) / (0.05 * v_ego + 0.5);
hN << a_ego * (0.1 * v_ego + 1.0);
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o
index 65182c5bb..5466ca143 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
index cecba6e74..bf64887f1 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_common.h
@@ -70,7 +70,7 @@ extern "C"
/** Number of control variables. */
#define ACADO_NU 1
/** Number of differential variables. */
-#define ACADO_NX 6
+#define ACADO_NX 3
/** Number of algebraic variables. */
#define ACADO_NXA 0
/** Number of differential derivative variables. */
@@ -80,7 +80,7 @@ extern "C"
/** Number of references/measurements on the last (N + 1)st node. */
#define ACADO_NYN 3
/** Total number of QP optimization variables. */
-#define ACADO_QP_NV 26
+#define ACADO_QP_NV 23
/** Number of Runge-Kutta stages per integration step. */
#define ACADO_RK_NSTAGES 4
/** Providing interface for arrival cost. */
@@ -102,11 +102,11 @@ extern "C"
typedef struct ACADOvariables_
{
int dummy;
-/** Matrix of size: 21 x 6 (row major format)
+/** Matrix of size: 21 x 3 (row major format)
*
* Matrix containing 21 differential variable vectors.
*/
-real_t x[ 126 ];
+real_t x[ 63 ];
/** Column vector of size: 20
*
@@ -138,11 +138,11 @@ real_t W[ 320 ];
/** Matrix of size: 3 x 3 (row major format) */
real_t WN[ 9 ];
-/** Column vector of size: 6
+/** Column vector of size: 3
*
* Current state feedback vector.
*/
-real_t x0[ 6 ];
+real_t x0[ 3 ];
} ACADOvariables;
@@ -155,22 +155,19 @@ real_t x0[ 6 ];
*/
typedef struct ACADOworkspace_
{
-/** Column vector of size: 10 */
-real_t rhs_aux[ 10 ];
-
real_t rk_ttt;
-/** Row vector of size: 51 */
-real_t rk_xxx[ 51 ];
+/** Row vector of size: 18 */
+real_t rk_xxx[ 18 ];
-/** Matrix of size: 4 x 48 (row major format) */
-real_t rk_kkk[ 192 ];
+/** Matrix of size: 4 x 15 (row major format) */
+real_t rk_kkk[ 60 ];
-/** Row vector of size: 51 */
-real_t state[ 51 ];
+/** Row vector of size: 18 */
+real_t state[ 18 ];
-/** Column vector of size: 120 */
-real_t d[ 120 ];
+/** Column vector of size: 60 */
+real_t d[ 60 ];
/** Column vector of size: 80 */
real_t Dy[ 80 ];
@@ -178,26 +175,26 @@ real_t Dy[ 80 ];
/** Column vector of size: 3 */
real_t DyN[ 3 ];
-/** Matrix of size: 120 x 6 (row major format) */
-real_t evGx[ 720 ];
+/** Matrix of size: 60 x 3 (row major format) */
+real_t evGx[ 180 ];
-/** Column vector of size: 120 */
-real_t evGu[ 120 ];
+/** Column vector of size: 60 */
+real_t evGu[ 60 ];
-/** Column vector of size: 30 */
-real_t objAuxVar[ 30 ];
+/** Column vector of size: 15 */
+real_t objAuxVar[ 15 ];
-/** Row vector of size: 9 */
-real_t objValueIn[ 9 ];
+/** Row vector of size: 6 */
+real_t objValueIn[ 6 ];
-/** Row vector of size: 32 */
-real_t objValueOut[ 32 ];
+/** Row vector of size: 20 */
+real_t objValueOut[ 20 ];
-/** Matrix of size: 120 x 6 (row major format) */
-real_t Q1[ 720 ];
+/** Matrix of size: 60 x 3 (row major format) */
+real_t Q1[ 180 ];
-/** Matrix of size: 120 x 4 (row major format) */
-real_t Q2[ 480 ];
+/** Matrix of size: 60 x 4 (row major format) */
+real_t Q2[ 240 ];
/** Column vector of size: 20 */
real_t R1[ 20 ];
@@ -205,53 +202,53 @@ real_t R1[ 20 ];
/** Matrix of size: 20 x 4 (row major format) */
real_t R2[ 80 ];
-/** Column vector of size: 120 */
-real_t S1[ 120 ];
+/** Column vector of size: 60 */
+real_t S1[ 60 ];
-/** Matrix of size: 6 x 6 (row major format) */
-real_t QN1[ 36 ];
+/** Matrix of size: 3 x 3 (row major format) */
+real_t QN1[ 9 ];
-/** Matrix of size: 6 x 3 (row major format) */
-real_t QN2[ 18 ];
+/** Matrix of size: 3 x 3 (row major format) */
+real_t QN2[ 9 ];
-/** Column vector of size: 6 */
-real_t Dx0[ 6 ];
+/** Column vector of size: 3 */
+real_t Dx0[ 3 ];
-/** Matrix of size: 6 x 6 (row major format) */
-real_t T[ 36 ];
+/** Matrix of size: 3 x 3 (row major format) */
+real_t T[ 9 ];
-/** Column vector of size: 1260 */
-real_t E[ 1260 ];
+/** Column vector of size: 630 */
+real_t E[ 630 ];
-/** Column vector of size: 1260 */
-real_t QE[ 1260 ];
+/** Column vector of size: 630 */
+real_t QE[ 630 ];
-/** Matrix of size: 120 x 6 (row major format) */
-real_t QGx[ 720 ];
+/** Matrix of size: 60 x 3 (row major format) */
+real_t QGx[ 180 ];
-/** Column vector of size: 120 */
-real_t Qd[ 120 ];
+/** Column vector of size: 60 */
+real_t Qd[ 60 ];
-/** Column vector of size: 126 */
-real_t QDy[ 126 ];
+/** Column vector of size: 63 */
+real_t QDy[ 63 ];
-/** Matrix of size: 20 x 6 (row major format) */
-real_t H10[ 120 ];
+/** Matrix of size: 20 x 3 (row major format) */
+real_t H10[ 60 ];
-/** Matrix of size: 26 x 26 (row major format) */
-real_t H[ 676 ];
+/** Matrix of size: 23 x 23 (row major format) */
+real_t H[ 529 ];
-/** Matrix of size: 20 x 26 (row major format) */
-real_t A[ 520 ];
+/** Matrix of size: 20 x 23 (row major format) */
+real_t A[ 460 ];
-/** Column vector of size: 26 */
-real_t g[ 26 ];
+/** Column vector of size: 23 */
+real_t g[ 23 ];
-/** Column vector of size: 26 */
-real_t lb[ 26 ];
+/** Column vector of size: 23 */
+real_t lb[ 23 ];
-/** Column vector of size: 26 */
-real_t ub[ 26 ];
+/** Column vector of size: 23 */
+real_t ub[ 23 ];
/** Column vector of size: 20 */
real_t lbA[ 20 ];
@@ -259,11 +256,11 @@ real_t lbA[ 20 ];
/** Column vector of size: 20 */
real_t ubA[ 20 ];
-/** Column vector of size: 26 */
-real_t x[ 26 ];
+/** Column vector of size: 23 */
+real_t x[ 23 ];
-/** Column vector of size: 46 */
-real_t y[ 46 ];
+/** Column vector of size: 43 */
+real_t y[ 43 ];
} ACADOworkspace;
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
index 2e77d5414..a0bac9681 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.c
@@ -23,72 +23,24 @@
void acado_rhs_forw(const real_t* in, real_t* out)
{
const real_t* xd = in;
-const real_t* u = in + 48;
-const real_t* od = in + 49;
-/* Vector of auxiliary variables; number of elements: 10. */
-real_t* a = acadoWorkspace.rhs_aux;
-
-/* Compute intermediate quantities: */
-a[0] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
-a[1] = ((real_t)(1.0000000000000000e+00)/(real_t)(2.0000000000000000e+00));
-a[2] = (exp((((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[5])/(real_t)(2.0000000000000000e+00))));
-a[3] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[36])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[36]))*a[1])*a[2]);
-a[4] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[37])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[37]))*a[1])*a[2]);
-a[5] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[38])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[38]))*a[1])*a[2]);
-a[6] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[39])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[39]))*a[1])*a[2]);
-a[7] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[40])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[40]))*a[1])*a[2]);
-a[8] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[41])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[41]))*a[1])*a[2]);
-a[9] = (((((((real_t)(0.0000000000000000e+00)-od[0])*xd[47])*xd[5])+((((real_t)(0.0000000000000000e+00)-od[0])*xd[5])*xd[47]))*a[1])*a[2]);
+const real_t* u = in + 15;
/* Compute outputs: */
out[0] = xd[1];
out[1] = xd[2];
out[2] = u[0];
-out[3] = xd[4];
-out[4] = (od[1]*a[0]);
-out[5] = (real_t)(1.0000000000000000e+00);
-out[6] = xd[12];
-out[7] = xd[13];
-out[8] = xd[14];
-out[9] = xd[15];
-out[10] = xd[16];
-out[11] = xd[17];
-out[12] = xd[18];
-out[13] = xd[19];
-out[14] = xd[20];
-out[15] = xd[21];
-out[16] = xd[22];
-out[17] = xd[23];
-out[18] = (real_t)(0.0000000000000000e+00);
-out[19] = (real_t)(0.0000000000000000e+00);
-out[20] = (real_t)(0.0000000000000000e+00);
-out[21] = (real_t)(0.0000000000000000e+00);
-out[22] = (real_t)(0.0000000000000000e+00);
-out[23] = (real_t)(0.0000000000000000e+00);
-out[24] = xd[30];
-out[25] = xd[31];
-out[26] = xd[32];
-out[27] = xd[33];
-out[28] = xd[34];
-out[29] = xd[35];
-out[30] = (od[1]*a[3]);
-out[31] = (od[1]*a[4]);
-out[32] = (od[1]*a[5]);
-out[33] = (od[1]*a[6]);
-out[34] = (od[1]*a[7]);
-out[35] = (od[1]*a[8]);
-out[36] = (real_t)(0.0000000000000000e+00);
-out[37] = (real_t)(0.0000000000000000e+00);
-out[38] = (real_t)(0.0000000000000000e+00);
-out[39] = (real_t)(0.0000000000000000e+00);
-out[40] = (real_t)(0.0000000000000000e+00);
-out[41] = (real_t)(0.0000000000000000e+00);
-out[42] = xd[43];
-out[43] = xd[44];
-out[44] = (real_t)(1.0000000000000000e+00);
-out[45] = xd[46];
-out[46] = (od[1]*a[9]);
-out[47] = (real_t)(0.0000000000000000e+00);
+out[3] = xd[6];
+out[4] = xd[7];
+out[5] = xd[8];
+out[6] = xd[9];
+out[7] = xd[10];
+out[8] = xd[11];
+out[9] = (real_t)(0.0000000000000000e+00);
+out[10] = (real_t)(0.0000000000000000e+00);
+out[11] = (real_t)(0.0000000000000000e+00);
+out[12] = xd[13];
+out[13] = xd[14];
+out[14] = (real_t)(1.0000000000000000e+00);
}
/* Fixed step size:0.2 */
@@ -100,51 +52,21 @@ int run1;
int numSteps[20] = {1, 1, 1, 1, 1, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3};
int numInts = numSteps[rk_index];
acadoWorkspace.rk_ttt = 0.0000000000000000e+00;
-rk_eta[6] = 1.0000000000000000e+00;
-rk_eta[7] = 0.0000000000000000e+00;
+rk_eta[3] = 1.0000000000000000e+00;
+rk_eta[4] = 0.0000000000000000e+00;
+rk_eta[5] = 0.0000000000000000e+00;
+rk_eta[6] = 0.0000000000000000e+00;
+rk_eta[7] = 1.0000000000000000e+00;
rk_eta[8] = 0.0000000000000000e+00;
rk_eta[9] = 0.0000000000000000e+00;
rk_eta[10] = 0.0000000000000000e+00;
-rk_eta[11] = 0.0000000000000000e+00;
+rk_eta[11] = 1.0000000000000000e+00;
rk_eta[12] = 0.0000000000000000e+00;
-rk_eta[13] = 1.0000000000000000e+00;
+rk_eta[13] = 0.0000000000000000e+00;
rk_eta[14] = 0.0000000000000000e+00;
-rk_eta[15] = 0.0000000000000000e+00;
-rk_eta[16] = 0.0000000000000000e+00;
-rk_eta[17] = 0.0000000000000000e+00;
-rk_eta[18] = 0.0000000000000000e+00;
-rk_eta[19] = 0.0000000000000000e+00;
-rk_eta[20] = 1.0000000000000000e+00;
-rk_eta[21] = 0.0000000000000000e+00;
-rk_eta[22] = 0.0000000000000000e+00;
-rk_eta[23] = 0.0000000000000000e+00;
-rk_eta[24] = 0.0000000000000000e+00;
-rk_eta[25] = 0.0000000000000000e+00;
-rk_eta[26] = 0.0000000000000000e+00;
-rk_eta[27] = 1.0000000000000000e+00;
-rk_eta[28] = 0.0000000000000000e+00;
-rk_eta[29] = 0.0000000000000000e+00;
-rk_eta[30] = 0.0000000000000000e+00;
-rk_eta[31] = 0.0000000000000000e+00;
-rk_eta[32] = 0.0000000000000000e+00;
-rk_eta[33] = 0.0000000000000000e+00;
-rk_eta[34] = 1.0000000000000000e+00;
-rk_eta[35] = 0.0000000000000000e+00;
-rk_eta[36] = 0.0000000000000000e+00;
-rk_eta[37] = 0.0000000000000000e+00;
-rk_eta[38] = 0.0000000000000000e+00;
-rk_eta[39] = 0.0000000000000000e+00;
-rk_eta[40] = 0.0000000000000000e+00;
-rk_eta[41] = 1.0000000000000000e+00;
-rk_eta[42] = 0.0000000000000000e+00;
-rk_eta[43] = 0.0000000000000000e+00;
-rk_eta[44] = 0.0000000000000000e+00;
-rk_eta[45] = 0.0000000000000000e+00;
-rk_eta[46] = 0.0000000000000000e+00;
-rk_eta[47] = 0.0000000000000000e+00;
-acadoWorkspace.rk_xxx[48] = rk_eta[48];
-acadoWorkspace.rk_xxx[49] = rk_eta[49];
-acadoWorkspace.rk_xxx[50] = rk_eta[50];
+acadoWorkspace.rk_xxx[15] = rk_eta[15];
+acadoWorkspace.rk_xxx[16] = rk_eta[16];
+acadoWorkspace.rk_xxx[17] = rk_eta[17];
for (run1 = 0; run1 < 1; ++run1)
{
@@ -164,39 +86,6 @@ acadoWorkspace.rk_xxx[11] = + rk_eta[11];
acadoWorkspace.rk_xxx[12] = + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + rk_eta[27];
-acadoWorkspace.rk_xxx[28] = + rk_eta[28];
-acadoWorkspace.rk_xxx[29] = + rk_eta[29];
-acadoWorkspace.rk_xxx[30] = + rk_eta[30];
-acadoWorkspace.rk_xxx[31] = + rk_eta[31];
-acadoWorkspace.rk_xxx[32] = + rk_eta[32];
-acadoWorkspace.rk_xxx[33] = + rk_eta[33];
-acadoWorkspace.rk_xxx[34] = + rk_eta[34];
-acadoWorkspace.rk_xxx[35] = + rk_eta[35];
-acadoWorkspace.rk_xxx[36] = + rk_eta[36];
-acadoWorkspace.rk_xxx[37] = + rk_eta[37];
-acadoWorkspace.rk_xxx[38] = + rk_eta[38];
-acadoWorkspace.rk_xxx[39] = + rk_eta[39];
-acadoWorkspace.rk_xxx[40] = + rk_eta[40];
-acadoWorkspace.rk_xxx[41] = + rk_eta[41];
-acadoWorkspace.rk_xxx[42] = + rk_eta[42];
-acadoWorkspace.rk_xxx[43] = + rk_eta[43];
-acadoWorkspace.rk_xxx[44] = + rk_eta[44];
-acadoWorkspace.rk_xxx[45] = + rk_eta[45];
-acadoWorkspace.rk_xxx[46] = + rk_eta[46];
-acadoWorkspace.rk_xxx[47] = + rk_eta[47];
acado_rhs_forw( acadoWorkspace.rk_xxx, acadoWorkspace.rk_kkk );
acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[0] + rk_eta[0];
acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[1] + rk_eta[1];
@@ -213,186 +102,54 @@ acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_k
acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[12] + rk_eta[12];
acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[13] + rk_eta[13];
acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[14] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[27];
-acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[28];
-acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[29];
-acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[30] + rk_eta[30];
-acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[31] + rk_eta[31];
-acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[32] + rk_eta[32];
-acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[33] + rk_eta[33];
-acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[34] + rk_eta[34];
-acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[35] + rk_eta[35];
-acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[36] + rk_eta[36];
-acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[37] + rk_eta[37];
-acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[38] + rk_eta[38];
-acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[39] + rk_eta[39];
-acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[40] + rk_eta[40];
-acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[41] + rk_eta[41];
-acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[42] + rk_eta[42];
-acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[43] + rk_eta[43];
-acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[44] + rk_eta[44];
-acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[45] + rk_eta[45];
-acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[46] + rk_eta[46];
-acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[47] + rk_eta[47];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 48 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[48] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[49] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[50] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[51] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[52] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[53] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[54] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[55] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[56] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[57] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[58] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[59] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[60] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[61] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[62] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[63] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[64] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[65] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[66] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[67] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[68] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[69] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[70] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[71] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[72] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[73] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[74] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[75] + rk_eta[27];
-acadoWorkspace.rk_xxx[28] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[76] + rk_eta[28];
-acadoWorkspace.rk_xxx[29] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[77] + rk_eta[29];
-acadoWorkspace.rk_xxx[30] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[78] + rk_eta[30];
-acadoWorkspace.rk_xxx[31] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[79] + rk_eta[31];
-acadoWorkspace.rk_xxx[32] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[80] + rk_eta[32];
-acadoWorkspace.rk_xxx[33] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[81] + rk_eta[33];
-acadoWorkspace.rk_xxx[34] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[82] + rk_eta[34];
-acadoWorkspace.rk_xxx[35] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[83] + rk_eta[35];
-acadoWorkspace.rk_xxx[36] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[84] + rk_eta[36];
-acadoWorkspace.rk_xxx[37] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[85] + rk_eta[37];
-acadoWorkspace.rk_xxx[38] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[86] + rk_eta[38];
-acadoWorkspace.rk_xxx[39] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[87] + rk_eta[39];
-acadoWorkspace.rk_xxx[40] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[88] + rk_eta[40];
-acadoWorkspace.rk_xxx[41] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[89] + rk_eta[41];
-acadoWorkspace.rk_xxx[42] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[90] + rk_eta[42];
-acadoWorkspace.rk_xxx[43] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[91] + rk_eta[43];
-acadoWorkspace.rk_xxx[44] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[92] + rk_eta[44];
-acadoWorkspace.rk_xxx[45] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[93] + rk_eta[45];
-acadoWorkspace.rk_xxx[46] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[94] + rk_eta[46];
-acadoWorkspace.rk_xxx[47] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[95] + rk_eta[47];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 96 ]) );
-acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[96] + rk_eta[0];
-acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[97] + rk_eta[1];
-acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[98] + rk_eta[2];
-acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[99] + rk_eta[3];
-acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[100] + rk_eta[4];
-acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[101] + rk_eta[5];
-acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[102] + rk_eta[6];
-acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[103] + rk_eta[7];
-acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[104] + rk_eta[8];
-acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[105] + rk_eta[9];
-acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[106] + rk_eta[10];
-acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[107] + rk_eta[11];
-acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[108] + rk_eta[12];
-acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[109] + rk_eta[13];
-acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[110] + rk_eta[14];
-acadoWorkspace.rk_xxx[15] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[111] + rk_eta[15];
-acadoWorkspace.rk_xxx[16] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[112] + rk_eta[16];
-acadoWorkspace.rk_xxx[17] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[113] + rk_eta[17];
-acadoWorkspace.rk_xxx[18] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[114] + rk_eta[18];
-acadoWorkspace.rk_xxx[19] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[115] + rk_eta[19];
-acadoWorkspace.rk_xxx[20] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[116] + rk_eta[20];
-acadoWorkspace.rk_xxx[21] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[117] + rk_eta[21];
-acadoWorkspace.rk_xxx[22] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[118] + rk_eta[22];
-acadoWorkspace.rk_xxx[23] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[119] + rk_eta[23];
-acadoWorkspace.rk_xxx[24] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[120] + rk_eta[24];
-acadoWorkspace.rk_xxx[25] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[121] + rk_eta[25];
-acadoWorkspace.rk_xxx[26] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[122] + rk_eta[26];
-acadoWorkspace.rk_xxx[27] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[123] + rk_eta[27];
-acadoWorkspace.rk_xxx[28] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[124] + rk_eta[28];
-acadoWorkspace.rk_xxx[29] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[125] + rk_eta[29];
-acadoWorkspace.rk_xxx[30] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[126] + rk_eta[30];
-acadoWorkspace.rk_xxx[31] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[127] + rk_eta[31];
-acadoWorkspace.rk_xxx[32] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[128] + rk_eta[32];
-acadoWorkspace.rk_xxx[33] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[129] + rk_eta[33];
-acadoWorkspace.rk_xxx[34] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[130] + rk_eta[34];
-acadoWorkspace.rk_xxx[35] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[131] + rk_eta[35];
-acadoWorkspace.rk_xxx[36] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[132] + rk_eta[36];
-acadoWorkspace.rk_xxx[37] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[133] + rk_eta[37];
-acadoWorkspace.rk_xxx[38] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[134] + rk_eta[38];
-acadoWorkspace.rk_xxx[39] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[135] + rk_eta[39];
-acadoWorkspace.rk_xxx[40] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[136] + rk_eta[40];
-acadoWorkspace.rk_xxx[41] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[137] + rk_eta[41];
-acadoWorkspace.rk_xxx[42] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[138] + rk_eta[42];
-acadoWorkspace.rk_xxx[43] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[139] + rk_eta[43];
-acadoWorkspace.rk_xxx[44] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[140] + rk_eta[44];
-acadoWorkspace.rk_xxx[45] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[141] + rk_eta[45];
-acadoWorkspace.rk_xxx[46] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[142] + rk_eta[46];
-acadoWorkspace.rk_xxx[47] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[143] + rk_eta[47];
-acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 144 ]) );
-rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[48] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[96] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[144];
-rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[49] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[97] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[145];
-rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[50] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[98] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[146];
-rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[51] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[99] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[147];
-rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[52] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[100] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[148];
-rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[53] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[101] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[149];
-rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[54] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[102] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[150];
-rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[55] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[103] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[151];
-rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[56] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[104] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[152];
-rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[57] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[105] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[153];
-rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[58] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[106] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[154];
-rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[59] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[107] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[155];
-rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[60] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[108] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[156];
-rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[61] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[109] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[157];
-rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[62] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[110] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[158];
-rk_eta[15] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[63] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[111] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[159];
-rk_eta[16] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[64] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[112] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[160];
-rk_eta[17] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[65] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[113] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[161];
-rk_eta[18] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[66] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[114] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[162];
-rk_eta[19] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[67] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[115] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[163];
-rk_eta[20] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[68] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[116] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[164];
-rk_eta[21] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[69] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[117] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[165];
-rk_eta[22] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[70] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[118] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[166];
-rk_eta[23] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[71] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[119] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[167];
-rk_eta[24] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[72] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[120] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[168];
-rk_eta[25] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[73] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[121] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[169];
-rk_eta[26] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[74] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[122] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[170];
-rk_eta[27] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[75] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[123] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[171];
-rk_eta[28] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[76] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[124] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[172];
-rk_eta[29] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[77] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[125] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[173];
-rk_eta[30] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[30] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[78] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[126] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[174];
-rk_eta[31] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[31] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[79] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[127] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[175];
-rk_eta[32] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[32] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[80] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[128] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[176];
-rk_eta[33] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[33] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[81] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[129] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[177];
-rk_eta[34] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[34] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[82] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[130] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[178];
-rk_eta[35] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[35] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[83] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[131] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[179];
-rk_eta[36] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[36] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[84] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[132] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[180];
-rk_eta[37] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[37] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[85] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[133] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[181];
-rk_eta[38] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[38] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[86] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[134] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[182];
-rk_eta[39] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[39] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[87] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[135] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[183];
-rk_eta[40] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[40] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[88] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[136] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[184];
-rk_eta[41] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[41] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[89] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[137] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[185];
-rk_eta[42] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[42] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[90] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[138] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[186];
-rk_eta[43] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[43] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[91] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[139] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[187];
-rk_eta[44] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[44] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[92] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[140] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[188];
-rk_eta[45] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[93] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[141] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[189];
-rk_eta[46] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[94] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[142] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[190];
-rk_eta[47] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[95] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[143] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[191];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 15 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[15] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[16] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[17] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[18] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[19] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[20] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[21] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[22] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[23] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[24] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[25] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[26] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[27] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[28] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)9.9999999999999964e-02*acadoWorkspace.rk_kkk[29] + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 30 ]) );
+acadoWorkspace.rk_xxx[0] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[30] + rk_eta[0];
+acadoWorkspace.rk_xxx[1] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[31] + rk_eta[1];
+acadoWorkspace.rk_xxx[2] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[32] + rk_eta[2];
+acadoWorkspace.rk_xxx[3] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[33] + rk_eta[3];
+acadoWorkspace.rk_xxx[4] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[34] + rk_eta[4];
+acadoWorkspace.rk_xxx[5] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[35] + rk_eta[5];
+acadoWorkspace.rk_xxx[6] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[36] + rk_eta[6];
+acadoWorkspace.rk_xxx[7] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[37] + rk_eta[7];
+acadoWorkspace.rk_xxx[8] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[38] + rk_eta[8];
+acadoWorkspace.rk_xxx[9] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[39] + rk_eta[9];
+acadoWorkspace.rk_xxx[10] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[40] + rk_eta[10];
+acadoWorkspace.rk_xxx[11] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[41] + rk_eta[11];
+acadoWorkspace.rk_xxx[12] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[42] + rk_eta[12];
+acadoWorkspace.rk_xxx[13] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[43] + rk_eta[13];
+acadoWorkspace.rk_xxx[14] = + (real_t)1.9999999999999993e-01*acadoWorkspace.rk_kkk[44] + rk_eta[14];
+acado_rhs_forw( acadoWorkspace.rk_xxx, &(acadoWorkspace.rk_kkk[ 45 ]) );
+rk_eta[0] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[0] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[15] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[30] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[45];
+rk_eta[1] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[1] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[16] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[31] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[46];
+rk_eta[2] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[2] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[17] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[32] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[47];
+rk_eta[3] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[3] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[18] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[33] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[48];
+rk_eta[4] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[4] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[19] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[34] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[49];
+rk_eta[5] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[5] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[20] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[35] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[50];
+rk_eta[6] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[6] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[21] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[36] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[51];
+rk_eta[7] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[7] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[22] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[37] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[52];
+rk_eta[8] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[8] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[23] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[38] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[53];
+rk_eta[9] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[9] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[24] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[39] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[54];
+rk_eta[10] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[10] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[25] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[40] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[55];
+rk_eta[11] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[11] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[26] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[41] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[56];
+rk_eta[12] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[12] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[27] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[42] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[57];
+rk_eta[13] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[13] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[28] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[43] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[58];
+rk_eta[14] += + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[14] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[29] + (real_t)6.6666666666666638e-02*acadoWorkspace.rk_kkk[44] + (real_t)3.3333333333333319e-02*acadoWorkspace.rk_kkk[59];
acadoWorkspace.rk_ttt += 1.0000000000000000e+00;
}
}
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o
index 0008b1397..2ee2d1300 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
index a4d9f8cde..36e675e03 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.cpp
@@ -40,7 +40,7 @@ int acado_solve( void )
{
acado_nWSR = QPOASES_NWSRMAX;
- QProblem qp(26, 20);
+ QProblem qp(23, 20);
returnValue retVal = qp.init(acadoWorkspace.H, acadoWorkspace.g, acadoWorkspace.A, acadoWorkspace.lb, acadoWorkspace.ub, acadoWorkspace.lbA, acadoWorkspace.ubA, acado_nWSR, acadoWorkspace.y);
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
index 3b3c75aa1..8a58805f8 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.hpp
@@ -37,7 +37,7 @@
*/
/** Maximum number of optimization variables. */
-#define QPOASES_NVMAX 26
+#define QPOASES_NVMAX 23
/** Maximum number of constraints. */
#define QPOASES_NCMAX 20
/** Maximum number of working set recalculations. */
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o
index ce919eb77..212935769 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
index 676787bbf..e4bf392f9 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.c
@@ -37,69 +37,33 @@ int lRun1;
ret = 0;
for (lRun1 = 0; lRun1 < 20; ++lRun1)
{
-acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 6];
-acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 6 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 6 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[lRun1 * 6 + 3];
-acadoWorkspace.state[4] = acadoVariables.x[lRun1 * 6 + 4];
-acadoWorkspace.state[5] = acadoVariables.x[lRun1 * 6 + 5];
+acadoWorkspace.state[0] = acadoVariables.x[lRun1 * 3];
+acadoWorkspace.state[1] = acadoVariables.x[lRun1 * 3 + 1];
+acadoWorkspace.state[2] = acadoVariables.x[lRun1 * 3 + 2];
-acadoWorkspace.state[48] = acadoVariables.u[lRun1];
-acadoWorkspace.state[49] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.state[50] = acadoVariables.od[lRun1 * 2 + 1];
+acadoWorkspace.state[15] = acadoVariables.u[lRun1];
+acadoWorkspace.state[16] = acadoVariables.od[lRun1 * 2];
+acadoWorkspace.state[17] = acadoVariables.od[lRun1 * 2 + 1];
ret = acado_integrate(acadoWorkspace.state, 1, lRun1);
-acadoWorkspace.d[lRun1 * 6] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 6 + 6];
-acadoWorkspace.d[lRun1 * 6 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 6 + 7];
-acadoWorkspace.d[lRun1 * 6 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 6 + 8];
-acadoWorkspace.d[lRun1 * 6 + 3] = acadoWorkspace.state[3] - acadoVariables.x[lRun1 * 6 + 9];
-acadoWorkspace.d[lRun1 * 6 + 4] = acadoWorkspace.state[4] - acadoVariables.x[lRun1 * 6 + 10];
-acadoWorkspace.d[lRun1 * 6 + 5] = acadoWorkspace.state[5] - acadoVariables.x[lRun1 * 6 + 11];
-
-acadoWorkspace.evGx[lRun1 * 36] = acadoWorkspace.state[6];
-acadoWorkspace.evGx[lRun1 * 36 + 1] = acadoWorkspace.state[7];
-acadoWorkspace.evGx[lRun1 * 36 + 2] = acadoWorkspace.state[8];
-acadoWorkspace.evGx[lRun1 * 36 + 3] = acadoWorkspace.state[9];
-acadoWorkspace.evGx[lRun1 * 36 + 4] = acadoWorkspace.state[10];
-acadoWorkspace.evGx[lRun1 * 36 + 5] = acadoWorkspace.state[11];
-acadoWorkspace.evGx[lRun1 * 36 + 6] = acadoWorkspace.state[12];
-acadoWorkspace.evGx[lRun1 * 36 + 7] = acadoWorkspace.state[13];
-acadoWorkspace.evGx[lRun1 * 36 + 8] = acadoWorkspace.state[14];
-acadoWorkspace.evGx[lRun1 * 36 + 9] = acadoWorkspace.state[15];
-acadoWorkspace.evGx[lRun1 * 36 + 10] = acadoWorkspace.state[16];
-acadoWorkspace.evGx[lRun1 * 36 + 11] = acadoWorkspace.state[17];
-acadoWorkspace.evGx[lRun1 * 36 + 12] = acadoWorkspace.state[18];
-acadoWorkspace.evGx[lRun1 * 36 + 13] = acadoWorkspace.state[19];
-acadoWorkspace.evGx[lRun1 * 36 + 14] = acadoWorkspace.state[20];
-acadoWorkspace.evGx[lRun1 * 36 + 15] = acadoWorkspace.state[21];
-acadoWorkspace.evGx[lRun1 * 36 + 16] = acadoWorkspace.state[22];
-acadoWorkspace.evGx[lRun1 * 36 + 17] = acadoWorkspace.state[23];
-acadoWorkspace.evGx[lRun1 * 36 + 18] = acadoWorkspace.state[24];
-acadoWorkspace.evGx[lRun1 * 36 + 19] = acadoWorkspace.state[25];
-acadoWorkspace.evGx[lRun1 * 36 + 20] = acadoWorkspace.state[26];
-acadoWorkspace.evGx[lRun1 * 36 + 21] = acadoWorkspace.state[27];
-acadoWorkspace.evGx[lRun1 * 36 + 22] = acadoWorkspace.state[28];
-acadoWorkspace.evGx[lRun1 * 36 + 23] = acadoWorkspace.state[29];
-acadoWorkspace.evGx[lRun1 * 36 + 24] = acadoWorkspace.state[30];
-acadoWorkspace.evGx[lRun1 * 36 + 25] = acadoWorkspace.state[31];
-acadoWorkspace.evGx[lRun1 * 36 + 26] = acadoWorkspace.state[32];
-acadoWorkspace.evGx[lRun1 * 36 + 27] = acadoWorkspace.state[33];
-acadoWorkspace.evGx[lRun1 * 36 + 28] = acadoWorkspace.state[34];
-acadoWorkspace.evGx[lRun1 * 36 + 29] = acadoWorkspace.state[35];
-acadoWorkspace.evGx[lRun1 * 36 + 30] = acadoWorkspace.state[36];
-acadoWorkspace.evGx[lRun1 * 36 + 31] = acadoWorkspace.state[37];
-acadoWorkspace.evGx[lRun1 * 36 + 32] = acadoWorkspace.state[38];
-acadoWorkspace.evGx[lRun1 * 36 + 33] = acadoWorkspace.state[39];
-acadoWorkspace.evGx[lRun1 * 36 + 34] = acadoWorkspace.state[40];
-acadoWorkspace.evGx[lRun1 * 36 + 35] = acadoWorkspace.state[41];
-
-acadoWorkspace.evGu[lRun1 * 6] = acadoWorkspace.state[42];
-acadoWorkspace.evGu[lRun1 * 6 + 1] = acadoWorkspace.state[43];
-acadoWorkspace.evGu[lRun1 * 6 + 2] = acadoWorkspace.state[44];
-acadoWorkspace.evGu[lRun1 * 6 + 3] = acadoWorkspace.state[45];
-acadoWorkspace.evGu[lRun1 * 6 + 4] = acadoWorkspace.state[46];
-acadoWorkspace.evGu[lRun1 * 6 + 5] = acadoWorkspace.state[47];
+acadoWorkspace.d[lRun1 * 3] = acadoWorkspace.state[0] - acadoVariables.x[lRun1 * 3 + 3];
+acadoWorkspace.d[lRun1 * 3 + 1] = acadoWorkspace.state[1] - acadoVariables.x[lRun1 * 3 + 4];
+acadoWorkspace.d[lRun1 * 3 + 2] = acadoWorkspace.state[2] - acadoVariables.x[lRun1 * 3 + 5];
+
+acadoWorkspace.evGx[lRun1 * 9] = acadoWorkspace.state[3];
+acadoWorkspace.evGx[lRun1 * 9 + 1] = acadoWorkspace.state[4];
+acadoWorkspace.evGx[lRun1 * 9 + 2] = acadoWorkspace.state[5];
+acadoWorkspace.evGx[lRun1 * 9 + 3] = acadoWorkspace.state[6];
+acadoWorkspace.evGx[lRun1 * 9 + 4] = acadoWorkspace.state[7];
+acadoWorkspace.evGx[lRun1 * 9 + 5] = acadoWorkspace.state[8];
+acadoWorkspace.evGx[lRun1 * 9 + 6] = acadoWorkspace.state[9];
+acadoWorkspace.evGx[lRun1 * 9 + 7] = acadoWorkspace.state[10];
+acadoWorkspace.evGx[lRun1 * 9 + 8] = acadoWorkspace.state[11];
+
+acadoWorkspace.evGu[lRun1 * 3] = acadoWorkspace.state[12];
+acadoWorkspace.evGu[lRun1 * 3 + 1] = acadoWorkspace.state[13];
+acadoWorkspace.evGu[lRun1 * 3 + 2] = acadoWorkspace.state[14];
}
return ret;
}
@@ -107,201 +71,112 @@ return ret;
void acado_evaluateLSQ(const real_t* in, real_t* out)
{
const real_t* xd = in;
-const real_t* u = in + 6;
-/* Vector of auxiliary variables; number of elements: 30. */
+const real_t* u = in + 3;
+const real_t* od = in + 4;
+/* Vector of auxiliary variables; number of elements: 15. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]);
-a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[9] = (a[8]*(real_t)(5.0000000000000000e-01));
-a[10] = (a[4]*a[4]);
-a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
+a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
+a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
+a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
+a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
+a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
+a[8] = (a[2]*a[2]);
+a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
+a[10] = (real_t)(0.0000000000000000e+00);
+a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01)));
-a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[16] = (a[15]*(real_t)(5.0000000000000000e-01));
-a[17] = (a[14]*a[14]);
-a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
-a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]);
-a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
-a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
-a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[28] = (a[26]*a[26]);
-a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
+a[13] = (a[11]*a[11]);
+a[14] = (real_t)(0.0000000000000000e+00);
/* Compute outputs: */
-out[0] = (a[1]-a[3]);
-out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
+out[0] = a[1];
+out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
out[3] = (u[0]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
-out[4] = a[6];
-out[5] = (a[11]-a[19]);
-out[6] = (real_t)(0.0000000000000000e+00);
-out[7] = a[20];
-out[8] = (a[22]-a[25]);
+out[4] = a[4];
+out[5] = a[9];
+out[6] = a[10];
+out[7] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
+out[8] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
out[9] = (real_t)(0.0000000000000000e+00);
-out[10] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]);
-out[11] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
-out[12] = (real_t)(0.0000000000000000e+00);
-out[13] = a[26];
-out[14] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]);
+out[10] = (real_t)(0.0000000000000000e+00);
+out[11] = (xd[2]*(real_t)(1.0000000000000001e-01));
+out[12] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
+out[13] = (real_t)(0.0000000000000000e+00);
+out[14] = (u[0]*(real_t)(1.0000000000000001e-01));
out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = (real_t)(0.0000000000000000e+00);
-out[17] = (xd[2]*(real_t)(1.0000000000000001e-01));
-out[18] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
-out[19] = (real_t)(0.0000000000000000e+00);
-out[20] = (real_t)(0.0000000000000000e+00);
-out[21] = (real_t)(0.0000000000000000e+00);
-out[22] = (real_t)(0.0000000000000000e+00);
-out[23] = (u[0]*(real_t)(1.0000000000000001e-01));
-out[24] = (real_t)(0.0000000000000000e+00);
-out[25] = (real_t)(0.0000000000000000e+00);
-out[26] = (real_t)(0.0000000000000000e+00);
-out[27] = (real_t)(0.0000000000000000e+00);
-out[28] = (real_t)(0.0000000000000000e+00);
-out[29] = (real_t)(0.0000000000000000e+00);
-out[30] = (real_t)(0.0000000000000000e+00);
-out[31] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
+out[16] = a[14];
+out[17] = (real_t)(0.0000000000000000e+00);
+out[18] = (real_t)(0.0000000000000000e+00);
+out[19] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
}
void acado_evaluateLSQEndTerm(const real_t* in, real_t* out)
{
const real_t* xd = in;
-/* Vector of auxiliary variables; number of elements: 30. */
+const real_t* od = in + 3;
+/* Vector of auxiliary variables; number of elements: 14. */
real_t* a = acadoWorkspace.objAuxVar;
/* Compute intermediate quantities: */
a[0] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[2] = (sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[4] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
-a[5] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
-a[6] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[4]))*a[5]);
-a[7] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[8] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[9] = (a[8]*(real_t)(5.0000000000000000e-01));
-a[10] = (a[4]*a[4]);
-a[11] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[7]))*a[4])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(xd[3]-xd[0]))*a[9])*a[10])))*a[5]);
+a[1] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[2] = ((real_t)(1.0000000000000000e+00)/(a[0]+(real_t)(1.0000000000000001e-01)));
+a[3] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))/(a[0]+(real_t)(1.0000000000000001e-01))))));
+a[4] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00)))*a[2]))*a[3]);
+a[5] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
+a[6] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
+a[7] = (a[6]*(real_t)(5.0000000000000000e-01));
+a[8] = (a[2]*a[2]);
+a[9] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[5]))*a[2])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-(od[0]-xd[0]))*a[7])*a[8])))*a[3]);
+a[10] = (real_t)(0.0000000000000000e+00);
+a[11] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
a[12] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[13] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[14] = ((real_t)(1.0000000000000000e+00)/(a[2]+(real_t)(1.0000000000000001e-01)));
-a[15] = (1.0/sqrt((xd[1]+(real_t)(5.0000000000000000e-01))));
-a[16] = (a[15]*(real_t)(5.0000000000000000e-01));
-a[17] = (a[14]*a[14]);
-a[18] = (exp(((real_t)(2.9999999999999999e-01)*(((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(a[2]+(real_t)(1.0000000000000001e-01))))));
-a[19] = (((real_t)(2.9999999999999999e-01)*((((((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12]))-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[13])))*a[14])-((((((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))+(real_t)(4.0000000000000000e+00))-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*a[16])*a[17])))*a[18]);
-a[20] = (((real_t)(2.9999999999999999e-01)*(((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[4]))*a[5]);
-a[21] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[22] = (((real_t)(2.9999999999999999e-01)*((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[21]))*a[4]))*a[5]);
-a[23] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[24] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[25] = (((real_t)(2.9999999999999999e-01)*(((((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[23]))-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[24])))*a[14]))*a[18]);
-a[26] = ((real_t)(1.0000000000000000e+00)/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
-a[27] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
-a[28] = (a[26]*a[26]);
-a[29] = ((real_t)(1.0000000000000000e+00)/(real_t)(1.9620000000000001e+01));
+a[13] = (a[11]*a[11]);
/* Compute outputs: */
-out[0] = (a[1]-a[3]);
-out[1] = (((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
+out[0] = a[1];
+out[1] = (((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))/(((real_t)(5.0000000000000003e-02)*xd[1])+(real_t)(5.0000000000000000e-01)));
out[2] = (xd[2]*(((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00)));
-out[3] = a[6];
-out[4] = (a[11]-a[19]);
-out[5] = (real_t)(0.0000000000000000e+00);
-out[6] = a[20];
-out[7] = (a[22]-a[25]);
+out[3] = a[4];
+out[4] = a[9];
+out[5] = a[10];
+out[6] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[11]);
+out[7] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[12])))*a[11])-((((od[0]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((od[1]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((od[1]*od[1])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[13]));
out[8] = (real_t)(0.0000000000000000e+00);
-out[9] = (((real_t)(0.0000000000000000e+00)-(real_t)(1.0000000000000000e+00))*a[26]);
-out[10] = ((((real_t)(0.0000000000000000e+00)-(((real_t)(1.8000000000000000e+00)-((real_t)(-1.8000000000000000e+00)))+((xd[1]+xd[1])*a[27])))*a[26])-((((xd[3]-xd[0])-((real_t)(4.0000000000000000e+00)+((((xd[1]*(real_t)(1.8000000000000000e+00))-((xd[4]-xd[1])*(real_t)(1.8000000000000000e+00)))+((xd[1]*xd[1])/(real_t)(1.9620000000000001e+01)))-((xd[4]*xd[4])/(real_t)(1.9620000000000001e+01)))))*(real_t)(5.0000000000000003e-02))*a[28]));
-out[11] = (real_t)(0.0000000000000000e+00);
-out[12] = a[26];
-out[13] = (((real_t)(0.0000000000000000e+00)-(((real_t)(0.0000000000000000e+00)-(real_t)(1.8000000000000000e+00))-((xd[4]+xd[4])*a[29])))*a[26]);
-out[14] = (real_t)(0.0000000000000000e+00);
-out[15] = (real_t)(0.0000000000000000e+00);
-out[16] = (xd[2]*(real_t)(1.0000000000000001e-01));
-out[17] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
-out[18] = (real_t)(0.0000000000000000e+00);
-out[19] = (real_t)(0.0000000000000000e+00);
-out[20] = (real_t)(0.0000000000000000e+00);
+out[9] = (real_t)(0.0000000000000000e+00);
+out[10] = (xd[2]*(real_t)(1.0000000000000001e-01));
+out[11] = (((real_t)(1.0000000000000001e-01)*xd[1])+(real_t)(1.0000000000000000e+00));
}
void acado_setObjQ1Q2( real_t* const tmpFx, real_t* const tmpObjS, real_t* const tmpQ1, real_t* const tmpQ2 )
{
-tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[6]*tmpObjS[4] + tmpFx[12]*tmpObjS[8] + tmpFx[18]*tmpObjS[12];
-tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[6]*tmpObjS[5] + tmpFx[12]*tmpObjS[9] + tmpFx[18]*tmpObjS[13];
-tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[6]*tmpObjS[6] + tmpFx[12]*tmpObjS[10] + tmpFx[18]*tmpObjS[14];
-tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[6]*tmpObjS[7] + tmpFx[12]*tmpObjS[11] + tmpFx[18]*tmpObjS[15];
-tmpQ2[4] = + tmpFx[1]*tmpObjS[0] + tmpFx[7]*tmpObjS[4] + tmpFx[13]*tmpObjS[8] + tmpFx[19]*tmpObjS[12];
-tmpQ2[5] = + tmpFx[1]*tmpObjS[1] + tmpFx[7]*tmpObjS[5] + tmpFx[13]*tmpObjS[9] + tmpFx[19]*tmpObjS[13];
-tmpQ2[6] = + tmpFx[1]*tmpObjS[2] + tmpFx[7]*tmpObjS[6] + tmpFx[13]*tmpObjS[10] + tmpFx[19]*tmpObjS[14];
-tmpQ2[7] = + tmpFx[1]*tmpObjS[3] + tmpFx[7]*tmpObjS[7] + tmpFx[13]*tmpObjS[11] + tmpFx[19]*tmpObjS[15];
-tmpQ2[8] = + tmpFx[2]*tmpObjS[0] + tmpFx[8]*tmpObjS[4] + tmpFx[14]*tmpObjS[8] + tmpFx[20]*tmpObjS[12];
-tmpQ2[9] = + tmpFx[2]*tmpObjS[1] + tmpFx[8]*tmpObjS[5] + tmpFx[14]*tmpObjS[9] + tmpFx[20]*tmpObjS[13];
-tmpQ2[10] = + tmpFx[2]*tmpObjS[2] + tmpFx[8]*tmpObjS[6] + tmpFx[14]*tmpObjS[10] + tmpFx[20]*tmpObjS[14];
-tmpQ2[11] = + tmpFx[2]*tmpObjS[3] + tmpFx[8]*tmpObjS[7] + tmpFx[14]*tmpObjS[11] + tmpFx[20]*tmpObjS[15];
-tmpQ2[12] = + tmpFx[3]*tmpObjS[0] + tmpFx[9]*tmpObjS[4] + tmpFx[15]*tmpObjS[8] + tmpFx[21]*tmpObjS[12];
-tmpQ2[13] = + tmpFx[3]*tmpObjS[1] + tmpFx[9]*tmpObjS[5] + tmpFx[15]*tmpObjS[9] + tmpFx[21]*tmpObjS[13];
-tmpQ2[14] = + tmpFx[3]*tmpObjS[2] + tmpFx[9]*tmpObjS[6] + tmpFx[15]*tmpObjS[10] + tmpFx[21]*tmpObjS[14];
-tmpQ2[15] = + tmpFx[3]*tmpObjS[3] + tmpFx[9]*tmpObjS[7] + tmpFx[15]*tmpObjS[11] + tmpFx[21]*tmpObjS[15];
-tmpQ2[16] = + tmpFx[4]*tmpObjS[0] + tmpFx[10]*tmpObjS[4] + tmpFx[16]*tmpObjS[8] + tmpFx[22]*tmpObjS[12];
-tmpQ2[17] = + tmpFx[4]*tmpObjS[1] + tmpFx[10]*tmpObjS[5] + tmpFx[16]*tmpObjS[9] + tmpFx[22]*tmpObjS[13];
-tmpQ2[18] = + tmpFx[4]*tmpObjS[2] + tmpFx[10]*tmpObjS[6] + tmpFx[16]*tmpObjS[10] + tmpFx[22]*tmpObjS[14];
-tmpQ2[19] = + tmpFx[4]*tmpObjS[3] + tmpFx[10]*tmpObjS[7] + tmpFx[16]*tmpObjS[11] + tmpFx[22]*tmpObjS[15];
-tmpQ2[20] = + tmpFx[5]*tmpObjS[0] + tmpFx[11]*tmpObjS[4] + tmpFx[17]*tmpObjS[8] + tmpFx[23]*tmpObjS[12];
-tmpQ2[21] = + tmpFx[5]*tmpObjS[1] + tmpFx[11]*tmpObjS[5] + tmpFx[17]*tmpObjS[9] + tmpFx[23]*tmpObjS[13];
-tmpQ2[22] = + tmpFx[5]*tmpObjS[2] + tmpFx[11]*tmpObjS[6] + tmpFx[17]*tmpObjS[10] + tmpFx[23]*tmpObjS[14];
-tmpQ2[23] = + tmpFx[5]*tmpObjS[3] + tmpFx[11]*tmpObjS[7] + tmpFx[17]*tmpObjS[11] + tmpFx[23]*tmpObjS[15];
-tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[6] + tmpQ2[2]*tmpFx[12] + tmpQ2[3]*tmpFx[18];
-tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[7] + tmpQ2[2]*tmpFx[13] + tmpQ2[3]*tmpFx[19];
-tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[8] + tmpQ2[2]*tmpFx[14] + tmpQ2[3]*tmpFx[20];
-tmpQ1[3] = + tmpQ2[0]*tmpFx[3] + tmpQ2[1]*tmpFx[9] + tmpQ2[2]*tmpFx[15] + tmpQ2[3]*tmpFx[21];
-tmpQ1[4] = + tmpQ2[0]*tmpFx[4] + tmpQ2[1]*tmpFx[10] + tmpQ2[2]*tmpFx[16] + tmpQ2[3]*tmpFx[22];
-tmpQ1[5] = + tmpQ2[0]*tmpFx[5] + tmpQ2[1]*tmpFx[11] + tmpQ2[2]*tmpFx[17] + tmpQ2[3]*tmpFx[23];
-tmpQ1[6] = + tmpQ2[4]*tmpFx[0] + tmpQ2[5]*tmpFx[6] + tmpQ2[6]*tmpFx[12] + tmpQ2[7]*tmpFx[18];
-tmpQ1[7] = + tmpQ2[4]*tmpFx[1] + tmpQ2[5]*tmpFx[7] + tmpQ2[6]*tmpFx[13] + tmpQ2[7]*tmpFx[19];
-tmpQ1[8] = + tmpQ2[4]*tmpFx[2] + tmpQ2[5]*tmpFx[8] + tmpQ2[6]*tmpFx[14] + tmpQ2[7]*tmpFx[20];
-tmpQ1[9] = + tmpQ2[4]*tmpFx[3] + tmpQ2[5]*tmpFx[9] + tmpQ2[6]*tmpFx[15] + tmpQ2[7]*tmpFx[21];
-tmpQ1[10] = + tmpQ2[4]*tmpFx[4] + tmpQ2[5]*tmpFx[10] + tmpQ2[6]*tmpFx[16] + tmpQ2[7]*tmpFx[22];
-tmpQ1[11] = + tmpQ2[4]*tmpFx[5] + tmpQ2[5]*tmpFx[11] + tmpQ2[6]*tmpFx[17] + tmpQ2[7]*tmpFx[23];
-tmpQ1[12] = + tmpQ2[8]*tmpFx[0] + tmpQ2[9]*tmpFx[6] + tmpQ2[10]*tmpFx[12] + tmpQ2[11]*tmpFx[18];
-tmpQ1[13] = + tmpQ2[8]*tmpFx[1] + tmpQ2[9]*tmpFx[7] + tmpQ2[10]*tmpFx[13] + tmpQ2[11]*tmpFx[19];
-tmpQ1[14] = + tmpQ2[8]*tmpFx[2] + tmpQ2[9]*tmpFx[8] + tmpQ2[10]*tmpFx[14] + tmpQ2[11]*tmpFx[20];
-tmpQ1[15] = + tmpQ2[8]*tmpFx[3] + tmpQ2[9]*tmpFx[9] + tmpQ2[10]*tmpFx[15] + tmpQ2[11]*tmpFx[21];
-tmpQ1[16] = + tmpQ2[8]*tmpFx[4] + tmpQ2[9]*tmpFx[10] + tmpQ2[10]*tmpFx[16] + tmpQ2[11]*tmpFx[22];
-tmpQ1[17] = + tmpQ2[8]*tmpFx[5] + tmpQ2[9]*tmpFx[11] + tmpQ2[10]*tmpFx[17] + tmpQ2[11]*tmpFx[23];
-tmpQ1[18] = + tmpQ2[12]*tmpFx[0] + tmpQ2[13]*tmpFx[6] + tmpQ2[14]*tmpFx[12] + tmpQ2[15]*tmpFx[18];
-tmpQ1[19] = + tmpQ2[12]*tmpFx[1] + tmpQ2[13]*tmpFx[7] + tmpQ2[14]*tmpFx[13] + tmpQ2[15]*tmpFx[19];
-tmpQ1[20] = + tmpQ2[12]*tmpFx[2] + tmpQ2[13]*tmpFx[8] + tmpQ2[14]*tmpFx[14] + tmpQ2[15]*tmpFx[20];
-tmpQ1[21] = + tmpQ2[12]*tmpFx[3] + tmpQ2[13]*tmpFx[9] + tmpQ2[14]*tmpFx[15] + tmpQ2[15]*tmpFx[21];
-tmpQ1[22] = + tmpQ2[12]*tmpFx[4] + tmpQ2[13]*tmpFx[10] + tmpQ2[14]*tmpFx[16] + tmpQ2[15]*tmpFx[22];
-tmpQ1[23] = + tmpQ2[12]*tmpFx[5] + tmpQ2[13]*tmpFx[11] + tmpQ2[14]*tmpFx[17] + tmpQ2[15]*tmpFx[23];
-tmpQ1[24] = + tmpQ2[16]*tmpFx[0] + tmpQ2[17]*tmpFx[6] + tmpQ2[18]*tmpFx[12] + tmpQ2[19]*tmpFx[18];
-tmpQ1[25] = + tmpQ2[16]*tmpFx[1] + tmpQ2[17]*tmpFx[7] + tmpQ2[18]*tmpFx[13] + tmpQ2[19]*tmpFx[19];
-tmpQ1[26] = + tmpQ2[16]*tmpFx[2] + tmpQ2[17]*tmpFx[8] + tmpQ2[18]*tmpFx[14] + tmpQ2[19]*tmpFx[20];
-tmpQ1[27] = + tmpQ2[16]*tmpFx[3] + tmpQ2[17]*tmpFx[9] + tmpQ2[18]*tmpFx[15] + tmpQ2[19]*tmpFx[21];
-tmpQ1[28] = + tmpQ2[16]*tmpFx[4] + tmpQ2[17]*tmpFx[10] + tmpQ2[18]*tmpFx[16] + tmpQ2[19]*tmpFx[22];
-tmpQ1[29] = + tmpQ2[16]*tmpFx[5] + tmpQ2[17]*tmpFx[11] + tmpQ2[18]*tmpFx[17] + tmpQ2[19]*tmpFx[23];
-tmpQ1[30] = + tmpQ2[20]*tmpFx[0] + tmpQ2[21]*tmpFx[6] + tmpQ2[22]*tmpFx[12] + tmpQ2[23]*tmpFx[18];
-tmpQ1[31] = + tmpQ2[20]*tmpFx[1] + tmpQ2[21]*tmpFx[7] + tmpQ2[22]*tmpFx[13] + tmpQ2[23]*tmpFx[19];
-tmpQ1[32] = + tmpQ2[20]*tmpFx[2] + tmpQ2[21]*tmpFx[8] + tmpQ2[22]*tmpFx[14] + tmpQ2[23]*tmpFx[20];
-tmpQ1[33] = + tmpQ2[20]*tmpFx[3] + tmpQ2[21]*tmpFx[9] + tmpQ2[22]*tmpFx[15] + tmpQ2[23]*tmpFx[21];
-tmpQ1[34] = + tmpQ2[20]*tmpFx[4] + tmpQ2[21]*tmpFx[10] + tmpQ2[22]*tmpFx[16] + tmpQ2[23]*tmpFx[22];
-tmpQ1[35] = + tmpQ2[20]*tmpFx[5] + tmpQ2[21]*tmpFx[11] + tmpQ2[22]*tmpFx[17] + tmpQ2[23]*tmpFx[23];
+tmpQ2[0] = + tmpFx[0]*tmpObjS[0] + tmpFx[3]*tmpObjS[4] + tmpFx[6]*tmpObjS[8] + tmpFx[9]*tmpObjS[12];
+tmpQ2[1] = + tmpFx[0]*tmpObjS[1] + tmpFx[3]*tmpObjS[5] + tmpFx[6]*tmpObjS[9] + tmpFx[9]*tmpObjS[13];
+tmpQ2[2] = + tmpFx[0]*tmpObjS[2] + tmpFx[3]*tmpObjS[6] + tmpFx[6]*tmpObjS[10] + tmpFx[9]*tmpObjS[14];
+tmpQ2[3] = + tmpFx[0]*tmpObjS[3] + tmpFx[3]*tmpObjS[7] + tmpFx[6]*tmpObjS[11] + tmpFx[9]*tmpObjS[15];
+tmpQ2[4] = + tmpFx[1]*tmpObjS[0] + tmpFx[4]*tmpObjS[4] + tmpFx[7]*tmpObjS[8] + tmpFx[10]*tmpObjS[12];
+tmpQ2[5] = + tmpFx[1]*tmpObjS[1] + tmpFx[4]*tmpObjS[5] + tmpFx[7]*tmpObjS[9] + tmpFx[10]*tmpObjS[13];
+tmpQ2[6] = + tmpFx[1]*tmpObjS[2] + tmpFx[4]*tmpObjS[6] + tmpFx[7]*tmpObjS[10] + tmpFx[10]*tmpObjS[14];
+tmpQ2[7] = + tmpFx[1]*tmpObjS[3] + tmpFx[4]*tmpObjS[7] + tmpFx[7]*tmpObjS[11] + tmpFx[10]*tmpObjS[15];
+tmpQ2[8] = + tmpFx[2]*tmpObjS[0] + tmpFx[5]*tmpObjS[4] + tmpFx[8]*tmpObjS[8] + tmpFx[11]*tmpObjS[12];
+tmpQ2[9] = + tmpFx[2]*tmpObjS[1] + tmpFx[5]*tmpObjS[5] + tmpFx[8]*tmpObjS[9] + tmpFx[11]*tmpObjS[13];
+tmpQ2[10] = + tmpFx[2]*tmpObjS[2] + tmpFx[5]*tmpObjS[6] + tmpFx[8]*tmpObjS[10] + tmpFx[11]*tmpObjS[14];
+tmpQ2[11] = + tmpFx[2]*tmpObjS[3] + tmpFx[5]*tmpObjS[7] + tmpFx[8]*tmpObjS[11] + tmpFx[11]*tmpObjS[15];
+tmpQ1[0] = + tmpQ2[0]*tmpFx[0] + tmpQ2[1]*tmpFx[3] + tmpQ2[2]*tmpFx[6] + tmpQ2[3]*tmpFx[9];
+tmpQ1[1] = + tmpQ2[0]*tmpFx[1] + tmpQ2[1]*tmpFx[4] + tmpQ2[2]*tmpFx[7] + tmpQ2[3]*tmpFx[10];
+tmpQ1[2] = + tmpQ2[0]*tmpFx[2] + tmpQ2[1]*tmpFx[5] + tmpQ2[2]*tmpFx[8] + tmpQ2[3]*tmpFx[11];
+tmpQ1[3] = + tmpQ2[4]*tmpFx[0] + tmpQ2[5]*tmpFx[3] + tmpQ2[6]*tmpFx[6] + tmpQ2[7]*tmpFx[9];
+tmpQ1[4] = + tmpQ2[4]*tmpFx[1] + tmpQ2[5]*tmpFx[4] + tmpQ2[6]*tmpFx[7] + tmpQ2[7]*tmpFx[10];
+tmpQ1[5] = + tmpQ2[4]*tmpFx[2] + tmpQ2[5]*tmpFx[5] + tmpQ2[6]*tmpFx[8] + tmpQ2[7]*tmpFx[11];
+tmpQ1[6] = + tmpQ2[8]*tmpFx[0] + tmpQ2[9]*tmpFx[3] + tmpQ2[10]*tmpFx[6] + tmpQ2[11]*tmpFx[9];
+tmpQ1[7] = + tmpQ2[8]*tmpFx[1] + tmpQ2[9]*tmpFx[4] + tmpQ2[10]*tmpFx[7] + tmpQ2[11]*tmpFx[10];
+tmpQ1[8] = + tmpQ2[8]*tmpFx[2] + tmpQ2[9]*tmpFx[5] + tmpQ2[10]*tmpFx[8] + tmpQ2[11]*tmpFx[11];
}
void acado_setObjR1R2( real_t* const tmpFu, real_t* const tmpObjS, real_t* const tmpR1, real_t* const tmpR2 )
@@ -315,60 +190,24 @@ tmpR1[0] = + tmpR2[0]*tmpFu[0] + tmpR2[1]*tmpFu[1] + tmpR2[2]*tmpFu[2] + tmpR2[3
void acado_setObjQN1QN2( real_t* const tmpFx, real_t* const tmpObjSEndTerm, real_t* const tmpQN1, real_t* const tmpQN2 )
{
-tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[6]*tmpObjSEndTerm[3] + tmpFx[12]*tmpObjSEndTerm[6];
-tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[6]*tmpObjSEndTerm[4] + tmpFx[12]*tmpObjSEndTerm[7];
-tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[6]*tmpObjSEndTerm[5] + tmpFx[12]*tmpObjSEndTerm[8];
-tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[7]*tmpObjSEndTerm[3] + tmpFx[13]*tmpObjSEndTerm[6];
-tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[7]*tmpObjSEndTerm[4] + tmpFx[13]*tmpObjSEndTerm[7];
-tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[7]*tmpObjSEndTerm[5] + tmpFx[13]*tmpObjSEndTerm[8];
-tmpQN2[6] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[8]*tmpObjSEndTerm[3] + tmpFx[14]*tmpObjSEndTerm[6];
-tmpQN2[7] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[8]*tmpObjSEndTerm[4] + tmpFx[14]*tmpObjSEndTerm[7];
-tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[8]*tmpObjSEndTerm[5] + tmpFx[14]*tmpObjSEndTerm[8];
-tmpQN2[9] = + tmpFx[3]*tmpObjSEndTerm[0] + tmpFx[9]*tmpObjSEndTerm[3] + tmpFx[15]*tmpObjSEndTerm[6];
-tmpQN2[10] = + tmpFx[3]*tmpObjSEndTerm[1] + tmpFx[9]*tmpObjSEndTerm[4] + tmpFx[15]*tmpObjSEndTerm[7];
-tmpQN2[11] = + tmpFx[3]*tmpObjSEndTerm[2] + tmpFx[9]*tmpObjSEndTerm[5] + tmpFx[15]*tmpObjSEndTerm[8];
-tmpQN2[12] = + tmpFx[4]*tmpObjSEndTerm[0] + tmpFx[10]*tmpObjSEndTerm[3] + tmpFx[16]*tmpObjSEndTerm[6];
-tmpQN2[13] = + tmpFx[4]*tmpObjSEndTerm[1] + tmpFx[10]*tmpObjSEndTerm[4] + tmpFx[16]*tmpObjSEndTerm[7];
-tmpQN2[14] = + tmpFx[4]*tmpObjSEndTerm[2] + tmpFx[10]*tmpObjSEndTerm[5] + tmpFx[16]*tmpObjSEndTerm[8];
-tmpQN2[15] = + tmpFx[5]*tmpObjSEndTerm[0] + tmpFx[11]*tmpObjSEndTerm[3] + tmpFx[17]*tmpObjSEndTerm[6];
-tmpQN2[16] = + tmpFx[5]*tmpObjSEndTerm[1] + tmpFx[11]*tmpObjSEndTerm[4] + tmpFx[17]*tmpObjSEndTerm[7];
-tmpQN2[17] = + tmpFx[5]*tmpObjSEndTerm[2] + tmpFx[11]*tmpObjSEndTerm[5] + tmpFx[17]*tmpObjSEndTerm[8];
-tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[6] + tmpQN2[2]*tmpFx[12];
-tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[7] + tmpQN2[2]*tmpFx[13];
-tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[8] + tmpQN2[2]*tmpFx[14];
-tmpQN1[3] = + tmpQN2[0]*tmpFx[3] + tmpQN2[1]*tmpFx[9] + tmpQN2[2]*tmpFx[15];
-tmpQN1[4] = + tmpQN2[0]*tmpFx[4] + tmpQN2[1]*tmpFx[10] + tmpQN2[2]*tmpFx[16];
-tmpQN1[5] = + tmpQN2[0]*tmpFx[5] + tmpQN2[1]*tmpFx[11] + tmpQN2[2]*tmpFx[17];
-tmpQN1[6] = + tmpQN2[3]*tmpFx[0] + tmpQN2[4]*tmpFx[6] + tmpQN2[5]*tmpFx[12];
-tmpQN1[7] = + tmpQN2[3]*tmpFx[1] + tmpQN2[4]*tmpFx[7] + tmpQN2[5]*tmpFx[13];
-tmpQN1[8] = + tmpQN2[3]*tmpFx[2] + tmpQN2[4]*tmpFx[8] + tmpQN2[5]*tmpFx[14];
-tmpQN1[9] = + tmpQN2[3]*tmpFx[3] + tmpQN2[4]*tmpFx[9] + tmpQN2[5]*tmpFx[15];
-tmpQN1[10] = + tmpQN2[3]*tmpFx[4] + tmpQN2[4]*tmpFx[10] + tmpQN2[5]*tmpFx[16];
-tmpQN1[11] = + tmpQN2[3]*tmpFx[5] + tmpQN2[4]*tmpFx[11] + tmpQN2[5]*tmpFx[17];
-tmpQN1[12] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[6] + tmpQN2[8]*tmpFx[12];
-tmpQN1[13] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[7] + tmpQN2[8]*tmpFx[13];
-tmpQN1[14] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[8] + tmpQN2[8]*tmpFx[14];
-tmpQN1[15] = + tmpQN2[6]*tmpFx[3] + tmpQN2[7]*tmpFx[9] + tmpQN2[8]*tmpFx[15];
-tmpQN1[16] = + tmpQN2[6]*tmpFx[4] + tmpQN2[7]*tmpFx[10] + tmpQN2[8]*tmpFx[16];
-tmpQN1[17] = + tmpQN2[6]*tmpFx[5] + tmpQN2[7]*tmpFx[11] + tmpQN2[8]*tmpFx[17];
-tmpQN1[18] = + tmpQN2[9]*tmpFx[0] + tmpQN2[10]*tmpFx[6] + tmpQN2[11]*tmpFx[12];
-tmpQN1[19] = + tmpQN2[9]*tmpFx[1] + tmpQN2[10]*tmpFx[7] + tmpQN2[11]*tmpFx[13];
-tmpQN1[20] = + tmpQN2[9]*tmpFx[2] + tmpQN2[10]*tmpFx[8] + tmpQN2[11]*tmpFx[14];
-tmpQN1[21] = + tmpQN2[9]*tmpFx[3] + tmpQN2[10]*tmpFx[9] + tmpQN2[11]*tmpFx[15];
-tmpQN1[22] = + tmpQN2[9]*tmpFx[4] + tmpQN2[10]*tmpFx[10] + tmpQN2[11]*tmpFx[16];
-tmpQN1[23] = + tmpQN2[9]*tmpFx[5] + tmpQN2[10]*tmpFx[11] + tmpQN2[11]*tmpFx[17];
-tmpQN1[24] = + tmpQN2[12]*tmpFx[0] + tmpQN2[13]*tmpFx[6] + tmpQN2[14]*tmpFx[12];
-tmpQN1[25] = + tmpQN2[12]*tmpFx[1] + tmpQN2[13]*tmpFx[7] + tmpQN2[14]*tmpFx[13];
-tmpQN1[26] = + tmpQN2[12]*tmpFx[2] + tmpQN2[13]*tmpFx[8] + tmpQN2[14]*tmpFx[14];
-tmpQN1[27] = + tmpQN2[12]*tmpFx[3] + tmpQN2[13]*tmpFx[9] + tmpQN2[14]*tmpFx[15];
-tmpQN1[28] = + tmpQN2[12]*tmpFx[4] + tmpQN2[13]*tmpFx[10] + tmpQN2[14]*tmpFx[16];
-tmpQN1[29] = + tmpQN2[12]*tmpFx[5] + tmpQN2[13]*tmpFx[11] + tmpQN2[14]*tmpFx[17];
-tmpQN1[30] = + tmpQN2[15]*tmpFx[0] + tmpQN2[16]*tmpFx[6] + tmpQN2[17]*tmpFx[12];
-tmpQN1[31] = + tmpQN2[15]*tmpFx[1] + tmpQN2[16]*tmpFx[7] + tmpQN2[17]*tmpFx[13];
-tmpQN1[32] = + tmpQN2[15]*tmpFx[2] + tmpQN2[16]*tmpFx[8] + tmpQN2[17]*tmpFx[14];
-tmpQN1[33] = + tmpQN2[15]*tmpFx[3] + tmpQN2[16]*tmpFx[9] + tmpQN2[17]*tmpFx[15];
-tmpQN1[34] = + tmpQN2[15]*tmpFx[4] + tmpQN2[16]*tmpFx[10] + tmpQN2[17]*tmpFx[16];
-tmpQN1[35] = + tmpQN2[15]*tmpFx[5] + tmpQN2[16]*tmpFx[11] + tmpQN2[17]*tmpFx[17];
+tmpQN2[0] = + tmpFx[0]*tmpObjSEndTerm[0] + tmpFx[3]*tmpObjSEndTerm[3] + tmpFx[6]*tmpObjSEndTerm[6];
+tmpQN2[1] = + tmpFx[0]*tmpObjSEndTerm[1] + tmpFx[3]*tmpObjSEndTerm[4] + tmpFx[6]*tmpObjSEndTerm[7];
+tmpQN2[2] = + tmpFx[0]*tmpObjSEndTerm[2] + tmpFx[3]*tmpObjSEndTerm[5] + tmpFx[6]*tmpObjSEndTerm[8];
+tmpQN2[3] = + tmpFx[1]*tmpObjSEndTerm[0] + tmpFx[4]*tmpObjSEndTerm[3] + tmpFx[7]*tmpObjSEndTerm[6];
+tmpQN2[4] = + tmpFx[1]*tmpObjSEndTerm[1] + tmpFx[4]*tmpObjSEndTerm[4] + tmpFx[7]*tmpObjSEndTerm[7];
+tmpQN2[5] = + tmpFx[1]*tmpObjSEndTerm[2] + tmpFx[4]*tmpObjSEndTerm[5] + tmpFx[7]*tmpObjSEndTerm[8];
+tmpQN2[6] = + tmpFx[2]*tmpObjSEndTerm[0] + tmpFx[5]*tmpObjSEndTerm[3] + tmpFx[8]*tmpObjSEndTerm[6];
+tmpQN2[7] = + tmpFx[2]*tmpObjSEndTerm[1] + tmpFx[5]*tmpObjSEndTerm[4] + tmpFx[8]*tmpObjSEndTerm[7];
+tmpQN2[8] = + tmpFx[2]*tmpObjSEndTerm[2] + tmpFx[5]*tmpObjSEndTerm[5] + tmpFx[8]*tmpObjSEndTerm[8];
+tmpQN1[0] = + tmpQN2[0]*tmpFx[0] + tmpQN2[1]*tmpFx[3] + tmpQN2[2]*tmpFx[6];
+tmpQN1[1] = + tmpQN2[0]*tmpFx[1] + tmpQN2[1]*tmpFx[4] + tmpQN2[2]*tmpFx[7];
+tmpQN1[2] = + tmpQN2[0]*tmpFx[2] + tmpQN2[1]*tmpFx[5] + tmpQN2[2]*tmpFx[8];
+tmpQN1[3] = + tmpQN2[3]*tmpFx[0] + tmpQN2[4]*tmpFx[3] + tmpQN2[5]*tmpFx[6];
+tmpQN1[4] = + tmpQN2[3]*tmpFx[1] + tmpQN2[4]*tmpFx[4] + tmpQN2[5]*tmpFx[7];
+tmpQN1[5] = + tmpQN2[3]*tmpFx[2] + tmpQN2[4]*tmpFx[5] + tmpQN2[5]*tmpFx[8];
+tmpQN1[6] = + tmpQN2[6]*tmpFx[0] + tmpQN2[7]*tmpFx[3] + tmpQN2[8]*tmpFx[6];
+tmpQN1[7] = + tmpQN2[6]*tmpFx[1] + tmpQN2[7]*tmpFx[4] + tmpQN2[8]*tmpFx[7];
+tmpQN1[8] = + tmpQN2[6]*tmpFx[2] + tmpQN2[7]*tmpFx[5] + tmpQN2[8]*tmpFx[8];
}
void acado_evaluateObjective( )
@@ -376,15 +215,12 @@ void acado_evaluateObjective( )
int runObj;
for (runObj = 0; runObj < 20; ++runObj)
{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 6];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 6 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 6 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[runObj * 6 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.x[runObj * 6 + 4];
-acadoWorkspace.objValueIn[5] = acadoVariables.x[runObj * 6 + 5];
-acadoWorkspace.objValueIn[6] = acadoVariables.u[runObj];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[runObj * 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[runObj * 2 + 1];
+acadoWorkspace.objValueIn[0] = acadoVariables.x[runObj * 3];
+acadoWorkspace.objValueIn[1] = acadoVariables.x[runObj * 3 + 1];
+acadoWorkspace.objValueIn[2] = acadoVariables.x[runObj * 3 + 2];
+acadoWorkspace.objValueIn[3] = acadoVariables.u[runObj];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[runObj * 2];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[runObj * 2 + 1];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[runObj * 4] = acadoWorkspace.objValueOut[0];
@@ -392,19 +228,16 @@ acadoWorkspace.Dy[runObj * 4 + 1] = acadoWorkspace.objValueOut[1];
acadoWorkspace.Dy[runObj * 4 + 2] = acadoWorkspace.objValueOut[2];
acadoWorkspace.Dy[runObj * 4 + 3] = acadoWorkspace.objValueOut[3];
-acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.Q1[ runObj * 36 ]), &(acadoWorkspace.Q2[ runObj * 24 ]) );
+acado_setObjQ1Q2( &(acadoWorkspace.objValueOut[ 4 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.Q1[ runObj * 9 ]), &(acadoWorkspace.Q2[ runObj * 12 ]) );
-acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 28 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) );
+acado_setObjR1R2( &(acadoWorkspace.objValueOut[ 16 ]), &(acadoVariables.W[ runObj * 16 ]), &(acadoWorkspace.R1[ runObj ]), &(acadoWorkspace.R2[ runObj * 4 ]) );
}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[120];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[121];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
-acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
-acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
+acadoWorkspace.objValueIn[0] = acadoVariables.x[60];
+acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
+acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
+acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0];
@@ -417,12 +250,9 @@ acado_setObjQN1QN2( &(acadoWorkspace.objValueOut[ 3 ]), acadoVariables.WN, acado
void acado_multGxd( real_t* const dOld, real_t* const Gx1, real_t* const dNew )
{
-dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3] + Gx1[4]*dOld[4] + Gx1[5]*dOld[5];
-dNew[1] += + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2] + Gx1[9]*dOld[3] + Gx1[10]*dOld[4] + Gx1[11]*dOld[5];
-dNew[2] += + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3] + Gx1[16]*dOld[4] + Gx1[17]*dOld[5];
-dNew[3] += + Gx1[18]*dOld[0] + Gx1[19]*dOld[1] + Gx1[20]*dOld[2] + Gx1[21]*dOld[3] + Gx1[22]*dOld[4] + Gx1[23]*dOld[5];
-dNew[4] += + Gx1[24]*dOld[0] + Gx1[25]*dOld[1] + Gx1[26]*dOld[2] + Gx1[27]*dOld[3] + Gx1[28]*dOld[4] + Gx1[29]*dOld[5];
-dNew[5] += + Gx1[30]*dOld[0] + Gx1[31]*dOld[1] + Gx1[32]*dOld[2] + Gx1[33]*dOld[3] + Gx1[34]*dOld[4] + Gx1[35]*dOld[5];
+dNew[0] += + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2];
+dNew[1] += + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2];
+dNew[2] += + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2];
}
void acado_moveGxT( real_t* const Gx1, real_t* const Gx2 )
@@ -436,83 +266,26 @@ Gx2[5] = Gx1[5];
Gx2[6] = Gx1[6];
Gx2[7] = Gx1[7];
Gx2[8] = Gx1[8];
-Gx2[9] = Gx1[9];
-Gx2[10] = Gx1[10];
-Gx2[11] = Gx1[11];
-Gx2[12] = Gx1[12];
-Gx2[13] = Gx1[13];
-Gx2[14] = Gx1[14];
-Gx2[15] = Gx1[15];
-Gx2[16] = Gx1[16];
-Gx2[17] = Gx1[17];
-Gx2[18] = Gx1[18];
-Gx2[19] = Gx1[19];
-Gx2[20] = Gx1[20];
-Gx2[21] = Gx1[21];
-Gx2[22] = Gx1[22];
-Gx2[23] = Gx1[23];
-Gx2[24] = Gx1[24];
-Gx2[25] = Gx1[25];
-Gx2[26] = Gx1[26];
-Gx2[27] = Gx1[27];
-Gx2[28] = Gx1[28];
-Gx2[29] = Gx1[29];
-Gx2[30] = Gx1[30];
-Gx2[31] = Gx1[31];
-Gx2[32] = Gx1[32];
-Gx2[33] = Gx1[33];
-Gx2[34] = Gx1[34];
-Gx2[35] = Gx1[35];
}
void acado_multGxGx( real_t* const Gx1, real_t* const Gx2, real_t* const Gx3 )
{
-Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[6] + Gx1[2]*Gx2[12] + Gx1[3]*Gx2[18] + Gx1[4]*Gx2[24] + Gx1[5]*Gx2[30];
-Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[7] + Gx1[2]*Gx2[13] + Gx1[3]*Gx2[19] + Gx1[4]*Gx2[25] + Gx1[5]*Gx2[31];
-Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[8] + Gx1[2]*Gx2[14] + Gx1[3]*Gx2[20] + Gx1[4]*Gx2[26] + Gx1[5]*Gx2[32];
-Gx3[3] = + Gx1[0]*Gx2[3] + Gx1[1]*Gx2[9] + Gx1[2]*Gx2[15] + Gx1[3]*Gx2[21] + Gx1[4]*Gx2[27] + Gx1[5]*Gx2[33];
-Gx3[4] = + Gx1[0]*Gx2[4] + Gx1[1]*Gx2[10] + Gx1[2]*Gx2[16] + Gx1[3]*Gx2[22] + Gx1[4]*Gx2[28] + Gx1[5]*Gx2[34];
-Gx3[5] = + Gx1[0]*Gx2[5] + Gx1[1]*Gx2[11] + Gx1[2]*Gx2[17] + Gx1[3]*Gx2[23] + Gx1[4]*Gx2[29] + Gx1[5]*Gx2[35];
-Gx3[6] = + Gx1[6]*Gx2[0] + Gx1[7]*Gx2[6] + Gx1[8]*Gx2[12] + Gx1[9]*Gx2[18] + Gx1[10]*Gx2[24] + Gx1[11]*Gx2[30];
-Gx3[7] = + Gx1[6]*Gx2[1] + Gx1[7]*Gx2[7] + Gx1[8]*Gx2[13] + Gx1[9]*Gx2[19] + Gx1[10]*Gx2[25] + Gx1[11]*Gx2[31];
-Gx3[8] = + Gx1[6]*Gx2[2] + Gx1[7]*Gx2[8] + Gx1[8]*Gx2[14] + Gx1[9]*Gx2[20] + Gx1[10]*Gx2[26] + Gx1[11]*Gx2[32];
-Gx3[9] = + Gx1[6]*Gx2[3] + Gx1[7]*Gx2[9] + Gx1[8]*Gx2[15] + Gx1[9]*Gx2[21] + Gx1[10]*Gx2[27] + Gx1[11]*Gx2[33];
-Gx3[10] = + Gx1[6]*Gx2[4] + Gx1[7]*Gx2[10] + Gx1[8]*Gx2[16] + Gx1[9]*Gx2[22] + Gx1[10]*Gx2[28] + Gx1[11]*Gx2[34];
-Gx3[11] = + Gx1[6]*Gx2[5] + Gx1[7]*Gx2[11] + Gx1[8]*Gx2[17] + Gx1[9]*Gx2[23] + Gx1[10]*Gx2[29] + Gx1[11]*Gx2[35];
-Gx3[12] = + Gx1[12]*Gx2[0] + Gx1[13]*Gx2[6] + Gx1[14]*Gx2[12] + Gx1[15]*Gx2[18] + Gx1[16]*Gx2[24] + Gx1[17]*Gx2[30];
-Gx3[13] = + Gx1[12]*Gx2[1] + Gx1[13]*Gx2[7] + Gx1[14]*Gx2[13] + Gx1[15]*Gx2[19] + Gx1[16]*Gx2[25] + Gx1[17]*Gx2[31];
-Gx3[14] = + Gx1[12]*Gx2[2] + Gx1[13]*Gx2[8] + Gx1[14]*Gx2[14] + Gx1[15]*Gx2[20] + Gx1[16]*Gx2[26] + Gx1[17]*Gx2[32];
-Gx3[15] = + Gx1[12]*Gx2[3] + Gx1[13]*Gx2[9] + Gx1[14]*Gx2[15] + Gx1[15]*Gx2[21] + Gx1[16]*Gx2[27] + Gx1[17]*Gx2[33];
-Gx3[16] = + Gx1[12]*Gx2[4] + Gx1[13]*Gx2[10] + Gx1[14]*Gx2[16] + Gx1[15]*Gx2[22] + Gx1[16]*Gx2[28] + Gx1[17]*Gx2[34];
-Gx3[17] = + Gx1[12]*Gx2[5] + Gx1[13]*Gx2[11] + Gx1[14]*Gx2[17] + Gx1[15]*Gx2[23] + Gx1[16]*Gx2[29] + Gx1[17]*Gx2[35];
-Gx3[18] = + Gx1[18]*Gx2[0] + Gx1[19]*Gx2[6] + Gx1[20]*Gx2[12] + Gx1[21]*Gx2[18] + Gx1[22]*Gx2[24] + Gx1[23]*Gx2[30];
-Gx3[19] = + Gx1[18]*Gx2[1] + Gx1[19]*Gx2[7] + Gx1[20]*Gx2[13] + Gx1[21]*Gx2[19] + Gx1[22]*Gx2[25] + Gx1[23]*Gx2[31];
-Gx3[20] = + Gx1[18]*Gx2[2] + Gx1[19]*Gx2[8] + Gx1[20]*Gx2[14] + Gx1[21]*Gx2[20] + Gx1[22]*Gx2[26] + Gx1[23]*Gx2[32];
-Gx3[21] = + Gx1[18]*Gx2[3] + Gx1[19]*Gx2[9] + Gx1[20]*Gx2[15] + Gx1[21]*Gx2[21] + Gx1[22]*Gx2[27] + Gx1[23]*Gx2[33];
-Gx3[22] = + Gx1[18]*Gx2[4] + Gx1[19]*Gx2[10] + Gx1[20]*Gx2[16] + Gx1[21]*Gx2[22] + Gx1[22]*Gx2[28] + Gx1[23]*Gx2[34];
-Gx3[23] = + Gx1[18]*Gx2[5] + Gx1[19]*Gx2[11] + Gx1[20]*Gx2[17] + Gx1[21]*Gx2[23] + Gx1[22]*Gx2[29] + Gx1[23]*Gx2[35];
-Gx3[24] = + Gx1[24]*Gx2[0] + Gx1[25]*Gx2[6] + Gx1[26]*Gx2[12] + Gx1[27]*Gx2[18] + Gx1[28]*Gx2[24] + Gx1[29]*Gx2[30];
-Gx3[25] = + Gx1[24]*Gx2[1] + Gx1[25]*Gx2[7] + Gx1[26]*Gx2[13] + Gx1[27]*Gx2[19] + Gx1[28]*Gx2[25] + Gx1[29]*Gx2[31];
-Gx3[26] = + Gx1[24]*Gx2[2] + Gx1[25]*Gx2[8] + Gx1[26]*Gx2[14] + Gx1[27]*Gx2[20] + Gx1[28]*Gx2[26] + Gx1[29]*Gx2[32];
-Gx3[27] = + Gx1[24]*Gx2[3] + Gx1[25]*Gx2[9] + Gx1[26]*Gx2[15] + Gx1[27]*Gx2[21] + Gx1[28]*Gx2[27] + Gx1[29]*Gx2[33];
-Gx3[28] = + Gx1[24]*Gx2[4] + Gx1[25]*Gx2[10] + Gx1[26]*Gx2[16] + Gx1[27]*Gx2[22] + Gx1[28]*Gx2[28] + Gx1[29]*Gx2[34];
-Gx3[29] = + Gx1[24]*Gx2[5] + Gx1[25]*Gx2[11] + Gx1[26]*Gx2[17] + Gx1[27]*Gx2[23] + Gx1[28]*Gx2[29] + Gx1[29]*Gx2[35];
-Gx3[30] = + Gx1[30]*Gx2[0] + Gx1[31]*Gx2[6] + Gx1[32]*Gx2[12] + Gx1[33]*Gx2[18] + Gx1[34]*Gx2[24] + Gx1[35]*Gx2[30];
-Gx3[31] = + Gx1[30]*Gx2[1] + Gx1[31]*Gx2[7] + Gx1[32]*Gx2[13] + Gx1[33]*Gx2[19] + Gx1[34]*Gx2[25] + Gx1[35]*Gx2[31];
-Gx3[32] = + Gx1[30]*Gx2[2] + Gx1[31]*Gx2[8] + Gx1[32]*Gx2[14] + Gx1[33]*Gx2[20] + Gx1[34]*Gx2[26] + Gx1[35]*Gx2[32];
-Gx3[33] = + Gx1[30]*Gx2[3] + Gx1[31]*Gx2[9] + Gx1[32]*Gx2[15] + Gx1[33]*Gx2[21] + Gx1[34]*Gx2[27] + Gx1[35]*Gx2[33];
-Gx3[34] = + Gx1[30]*Gx2[4] + Gx1[31]*Gx2[10] + Gx1[32]*Gx2[16] + Gx1[33]*Gx2[22] + Gx1[34]*Gx2[28] + Gx1[35]*Gx2[34];
-Gx3[35] = + Gx1[30]*Gx2[5] + Gx1[31]*Gx2[11] + Gx1[32]*Gx2[17] + Gx1[33]*Gx2[23] + Gx1[34]*Gx2[29] + Gx1[35]*Gx2[35];
+Gx3[0] = + Gx1[0]*Gx2[0] + Gx1[1]*Gx2[3] + Gx1[2]*Gx2[6];
+Gx3[1] = + Gx1[0]*Gx2[1] + Gx1[1]*Gx2[4] + Gx1[2]*Gx2[7];
+Gx3[2] = + Gx1[0]*Gx2[2] + Gx1[1]*Gx2[5] + Gx1[2]*Gx2[8];
+Gx3[3] = + Gx1[3]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[5]*Gx2[6];
+Gx3[4] = + Gx1[3]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[5]*Gx2[7];
+Gx3[5] = + Gx1[3]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[5]*Gx2[8];
+Gx3[6] = + Gx1[6]*Gx2[0] + Gx1[7]*Gx2[3] + Gx1[8]*Gx2[6];
+Gx3[7] = + Gx1[6]*Gx2[1] + Gx1[7]*Gx2[4] + Gx1[8]*Gx2[7];
+Gx3[8] = + Gx1[6]*Gx2[2] + Gx1[7]*Gx2[5] + Gx1[8]*Gx2[8];
}
void acado_multGxGu( real_t* const Gx1, real_t* const Gu1, real_t* const Gu2 )
{
-Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2] + Gx1[3]*Gu1[3] + Gx1[4]*Gu1[4] + Gx1[5]*Gu1[5];
-Gu2[1] = + Gx1[6]*Gu1[0] + Gx1[7]*Gu1[1] + Gx1[8]*Gu1[2] + Gx1[9]*Gu1[3] + Gx1[10]*Gu1[4] + Gx1[11]*Gu1[5];
-Gu2[2] = + Gx1[12]*Gu1[0] + Gx1[13]*Gu1[1] + Gx1[14]*Gu1[2] + Gx1[15]*Gu1[3] + Gx1[16]*Gu1[4] + Gx1[17]*Gu1[5];
-Gu2[3] = + Gx1[18]*Gu1[0] + Gx1[19]*Gu1[1] + Gx1[20]*Gu1[2] + Gx1[21]*Gu1[3] + Gx1[22]*Gu1[4] + Gx1[23]*Gu1[5];
-Gu2[4] = + Gx1[24]*Gu1[0] + Gx1[25]*Gu1[1] + Gx1[26]*Gu1[2] + Gx1[27]*Gu1[3] + Gx1[28]*Gu1[4] + Gx1[29]*Gu1[5];
-Gu2[5] = + Gx1[30]*Gu1[0] + Gx1[31]*Gu1[1] + Gx1[32]*Gu1[2] + Gx1[33]*Gu1[3] + Gx1[34]*Gu1[4] + Gx1[35]*Gu1[5];
+Gu2[0] = + Gx1[0]*Gu1[0] + Gx1[1]*Gu1[1] + Gx1[2]*Gu1[2];
+Gu2[1] = + Gx1[3]*Gu1[0] + Gx1[4]*Gu1[1] + Gx1[5]*Gu1[2];
+Gu2[2] = + Gx1[6]*Gu1[0] + Gx1[7]*Gu1[1] + Gx1[8]*Gu1[2];
}
void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
@@ -520,49 +293,40 @@ void acado_moveGuE( real_t* const Gu1, real_t* const Gu2 )
Gu2[0] = Gu1[0];
Gu2[1] = Gu1[1];
Gu2[2] = Gu1[2];
-Gu2[3] = Gu1[3];
-Gu2[4] = Gu1[4];
-Gu2[5] = Gu1[5];
}
void acado_setBlockH11( int iRow, int iCol, real_t* const Gu1, real_t* const Gu2 )
{
-acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2] + Gu1[3]*Gu2[3] + Gu1[4]*Gu2[4] + Gu1[5]*Gu2[5];
+acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] += + Gu1[0]*Gu2[0] + Gu1[1]*Gu2[1] + Gu1[2]*Gu2[2];
}
void acado_setBlockH11_R1( int iRow, int iCol, real_t* const R11 )
{
-acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = R11[0];
+acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = R11[0];
}
void acado_zeroBlockH11( int iRow, int iCol )
{
-acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = 0.0000000000000000e+00;
+acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = 0.0000000000000000e+00;
}
void acado_copyHTH( int iRow, int iCol )
{
-acadoWorkspace.H[(iRow * 26 + 156) + (iCol + 6)] = acadoWorkspace.H[(iCol * 26 + 156) + (iRow + 6)];
+acadoWorkspace.H[(iRow * 23 + 69) + (iCol + 3)] = acadoWorkspace.H[(iCol * 23 + 69) + (iRow + 3)];
}
void acado_multQ1d( real_t* const Gx1, real_t* const dOld, real_t* const dNew )
{
-dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2] + Gx1[3]*dOld[3] + Gx1[4]*dOld[4] + Gx1[5]*dOld[5];
-dNew[1] = + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2] + Gx1[9]*dOld[3] + Gx1[10]*dOld[4] + Gx1[11]*dOld[5];
-dNew[2] = + Gx1[12]*dOld[0] + Gx1[13]*dOld[1] + Gx1[14]*dOld[2] + Gx1[15]*dOld[3] + Gx1[16]*dOld[4] + Gx1[17]*dOld[5];
-dNew[3] = + Gx1[18]*dOld[0] + Gx1[19]*dOld[1] + Gx1[20]*dOld[2] + Gx1[21]*dOld[3] + Gx1[22]*dOld[4] + Gx1[23]*dOld[5];
-dNew[4] = + Gx1[24]*dOld[0] + Gx1[25]*dOld[1] + Gx1[26]*dOld[2] + Gx1[27]*dOld[3] + Gx1[28]*dOld[4] + Gx1[29]*dOld[5];
-dNew[5] = + Gx1[30]*dOld[0] + Gx1[31]*dOld[1] + Gx1[32]*dOld[2] + Gx1[33]*dOld[3] + Gx1[34]*dOld[4] + Gx1[35]*dOld[5];
+dNew[0] = + Gx1[0]*dOld[0] + Gx1[1]*dOld[1] + Gx1[2]*dOld[2];
+dNew[1] = + Gx1[3]*dOld[0] + Gx1[4]*dOld[1] + Gx1[5]*dOld[2];
+dNew[2] = + Gx1[6]*dOld[0] + Gx1[7]*dOld[1] + Gx1[8]*dOld[2];
}
void acado_multQN1d( real_t* const QN1, real_t* const dOld, real_t* const dNew )
{
-dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2] + acadoWorkspace.QN1[3]*dOld[3] + acadoWorkspace.QN1[4]*dOld[4] + acadoWorkspace.QN1[5]*dOld[5];
-dNew[1] = + acadoWorkspace.QN1[6]*dOld[0] + acadoWorkspace.QN1[7]*dOld[1] + acadoWorkspace.QN1[8]*dOld[2] + acadoWorkspace.QN1[9]*dOld[3] + acadoWorkspace.QN1[10]*dOld[4] + acadoWorkspace.QN1[11]*dOld[5];
-dNew[2] = + acadoWorkspace.QN1[12]*dOld[0] + acadoWorkspace.QN1[13]*dOld[1] + acadoWorkspace.QN1[14]*dOld[2] + acadoWorkspace.QN1[15]*dOld[3] + acadoWorkspace.QN1[16]*dOld[4] + acadoWorkspace.QN1[17]*dOld[5];
-dNew[3] = + acadoWorkspace.QN1[18]*dOld[0] + acadoWorkspace.QN1[19]*dOld[1] + acadoWorkspace.QN1[20]*dOld[2] + acadoWorkspace.QN1[21]*dOld[3] + acadoWorkspace.QN1[22]*dOld[4] + acadoWorkspace.QN1[23]*dOld[5];
-dNew[4] = + acadoWorkspace.QN1[24]*dOld[0] + acadoWorkspace.QN1[25]*dOld[1] + acadoWorkspace.QN1[26]*dOld[2] + acadoWorkspace.QN1[27]*dOld[3] + acadoWorkspace.QN1[28]*dOld[4] + acadoWorkspace.QN1[29]*dOld[5];
-dNew[5] = + acadoWorkspace.QN1[30]*dOld[0] + acadoWorkspace.QN1[31]*dOld[1] + acadoWorkspace.QN1[32]*dOld[2] + acadoWorkspace.QN1[33]*dOld[3] + acadoWorkspace.QN1[34]*dOld[4] + acadoWorkspace.QN1[35]*dOld[5];
+dNew[0] = + acadoWorkspace.QN1[0]*dOld[0] + acadoWorkspace.QN1[1]*dOld[1] + acadoWorkspace.QN1[2]*dOld[2];
+dNew[1] = + acadoWorkspace.QN1[3]*dOld[0] + acadoWorkspace.QN1[4]*dOld[1] + acadoWorkspace.QN1[5]*dOld[2];
+dNew[2] = + acadoWorkspace.QN1[6]*dOld[0] + acadoWorkspace.QN1[7]*dOld[1] + acadoWorkspace.QN1[8]*dOld[2];
}
void acado_multRDy( real_t* const R2, real_t* const Dy1, real_t* const RDy1 )
@@ -575,29 +339,23 @@ void acado_multQDy( real_t* const Q2, real_t* const Dy1, real_t* const QDy1 )
QDy1[0] = + Q2[0]*Dy1[0] + Q2[1]*Dy1[1] + Q2[2]*Dy1[2] + Q2[3]*Dy1[3];
QDy1[1] = + Q2[4]*Dy1[0] + Q2[5]*Dy1[1] + Q2[6]*Dy1[2] + Q2[7]*Dy1[3];
QDy1[2] = + Q2[8]*Dy1[0] + Q2[9]*Dy1[1] + Q2[10]*Dy1[2] + Q2[11]*Dy1[3];
-QDy1[3] = + Q2[12]*Dy1[0] + Q2[13]*Dy1[1] + Q2[14]*Dy1[2] + Q2[15]*Dy1[3];
-QDy1[4] = + Q2[16]*Dy1[0] + Q2[17]*Dy1[1] + Q2[18]*Dy1[2] + Q2[19]*Dy1[3];
-QDy1[5] = + Q2[20]*Dy1[0] + Q2[21]*Dy1[1] + Q2[22]*Dy1[2] + Q2[23]*Dy1[3];
}
void acado_multEQDy( real_t* const E1, real_t* const QDy1, real_t* const U1 )
{
-U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2] + E1[3]*QDy1[3] + E1[4]*QDy1[4] + E1[5]*QDy1[5];
+U1[0] += + E1[0]*QDy1[0] + E1[1]*QDy1[1] + E1[2]*QDy1[2];
}
void acado_multQETGx( real_t* const E1, real_t* const Gx1, real_t* const H101 )
{
-H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[6] + E1[2]*Gx1[12] + E1[3]*Gx1[18] + E1[4]*Gx1[24] + E1[5]*Gx1[30];
-H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[7] + E1[2]*Gx1[13] + E1[3]*Gx1[19] + E1[4]*Gx1[25] + E1[5]*Gx1[31];
-H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[8] + E1[2]*Gx1[14] + E1[3]*Gx1[20] + E1[4]*Gx1[26] + E1[5]*Gx1[32];
-H101[3] += + E1[0]*Gx1[3] + E1[1]*Gx1[9] + E1[2]*Gx1[15] + E1[3]*Gx1[21] + E1[4]*Gx1[27] + E1[5]*Gx1[33];
-H101[4] += + E1[0]*Gx1[4] + E1[1]*Gx1[10] + E1[2]*Gx1[16] + E1[3]*Gx1[22] + E1[4]*Gx1[28] + E1[5]*Gx1[34];
-H101[5] += + E1[0]*Gx1[5] + E1[1]*Gx1[11] + E1[2]*Gx1[17] + E1[3]*Gx1[23] + E1[4]*Gx1[29] + E1[5]*Gx1[35];
+H101[0] += + E1[0]*Gx1[0] + E1[1]*Gx1[3] + E1[2]*Gx1[6];
+H101[1] += + E1[0]*Gx1[1] + E1[1]*Gx1[4] + E1[2]*Gx1[7];
+H101[2] += + E1[0]*Gx1[2] + E1[1]*Gx1[5] + E1[2]*Gx1[8];
}
void acado_zeroBlockH10( real_t* const H101 )
{
-{ int lCopy; for (lCopy = 0; lCopy < 6; lCopy++) H101[ lCopy ] = 0; }
+{ int lCopy; for (lCopy = 0; lCopy < 3; lCopy++) H101[ lCopy ] = 0; }
}
void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew )
@@ -605,9 +363,6 @@ void acado_multEDu( real_t* const E1, real_t* const U1, real_t* const dNew )
dNew[0] += + E1[0]*U1[0];
dNew[1] += + E1[1]*U1[0];
dNew[2] += + E1[2]*U1[0];
-dNew[3] += + E1[3]*U1[0];
-dNew[4] += + E1[4]*U1[0];
-dNew[5] += + E1[5]*U1[0];
}
void acado_zeroBlockH00( )
@@ -615,79 +370,25 @@ void acado_zeroBlockH00( )
acadoWorkspace.H[0] = 0.0000000000000000e+00;
acadoWorkspace.H[1] = 0.0000000000000000e+00;
acadoWorkspace.H[2] = 0.0000000000000000e+00;
-acadoWorkspace.H[3] = 0.0000000000000000e+00;
-acadoWorkspace.H[4] = 0.0000000000000000e+00;
-acadoWorkspace.H[5] = 0.0000000000000000e+00;
-acadoWorkspace.H[26] = 0.0000000000000000e+00;
-acadoWorkspace.H[27] = 0.0000000000000000e+00;
-acadoWorkspace.H[28] = 0.0000000000000000e+00;
-acadoWorkspace.H[29] = 0.0000000000000000e+00;
-acadoWorkspace.H[30] = 0.0000000000000000e+00;
-acadoWorkspace.H[31] = 0.0000000000000000e+00;
-acadoWorkspace.H[52] = 0.0000000000000000e+00;
-acadoWorkspace.H[53] = 0.0000000000000000e+00;
-acadoWorkspace.H[54] = 0.0000000000000000e+00;
-acadoWorkspace.H[55] = 0.0000000000000000e+00;
-acadoWorkspace.H[56] = 0.0000000000000000e+00;
-acadoWorkspace.H[57] = 0.0000000000000000e+00;
-acadoWorkspace.H[78] = 0.0000000000000000e+00;
-acadoWorkspace.H[79] = 0.0000000000000000e+00;
-acadoWorkspace.H[80] = 0.0000000000000000e+00;
-acadoWorkspace.H[81] = 0.0000000000000000e+00;
-acadoWorkspace.H[82] = 0.0000000000000000e+00;
-acadoWorkspace.H[83] = 0.0000000000000000e+00;
-acadoWorkspace.H[104] = 0.0000000000000000e+00;
-acadoWorkspace.H[105] = 0.0000000000000000e+00;
-acadoWorkspace.H[106] = 0.0000000000000000e+00;
-acadoWorkspace.H[107] = 0.0000000000000000e+00;
-acadoWorkspace.H[108] = 0.0000000000000000e+00;
-acadoWorkspace.H[109] = 0.0000000000000000e+00;
-acadoWorkspace.H[130] = 0.0000000000000000e+00;
-acadoWorkspace.H[131] = 0.0000000000000000e+00;
-acadoWorkspace.H[132] = 0.0000000000000000e+00;
-acadoWorkspace.H[133] = 0.0000000000000000e+00;
-acadoWorkspace.H[134] = 0.0000000000000000e+00;
-acadoWorkspace.H[135] = 0.0000000000000000e+00;
+acadoWorkspace.H[23] = 0.0000000000000000e+00;
+acadoWorkspace.H[24] = 0.0000000000000000e+00;
+acadoWorkspace.H[25] = 0.0000000000000000e+00;
+acadoWorkspace.H[46] = 0.0000000000000000e+00;
+acadoWorkspace.H[47] = 0.0000000000000000e+00;
+acadoWorkspace.H[48] = 0.0000000000000000e+00;
}
void acado_multCTQC( real_t* const Gx1, real_t* const Gx2 )
{
-acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[6]*Gx2[6] + Gx1[12]*Gx2[12] + Gx1[18]*Gx2[18] + Gx1[24]*Gx2[24] + Gx1[30]*Gx2[30];
-acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[6]*Gx2[7] + Gx1[12]*Gx2[13] + Gx1[18]*Gx2[19] + Gx1[24]*Gx2[25] + Gx1[30]*Gx2[31];
-acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[6]*Gx2[8] + Gx1[12]*Gx2[14] + Gx1[18]*Gx2[20] + Gx1[24]*Gx2[26] + Gx1[30]*Gx2[32];
-acadoWorkspace.H[3] += + Gx1[0]*Gx2[3] + Gx1[6]*Gx2[9] + Gx1[12]*Gx2[15] + Gx1[18]*Gx2[21] + Gx1[24]*Gx2[27] + Gx1[30]*Gx2[33];
-acadoWorkspace.H[4] += + Gx1[0]*Gx2[4] + Gx1[6]*Gx2[10] + Gx1[12]*Gx2[16] + Gx1[18]*Gx2[22] + Gx1[24]*Gx2[28] + Gx1[30]*Gx2[34];
-acadoWorkspace.H[5] += + Gx1[0]*Gx2[5] + Gx1[6]*Gx2[11] + Gx1[12]*Gx2[17] + Gx1[18]*Gx2[23] + Gx1[24]*Gx2[29] + Gx1[30]*Gx2[35];
-acadoWorkspace.H[26] += + Gx1[1]*Gx2[0] + Gx1[7]*Gx2[6] + Gx1[13]*Gx2[12] + Gx1[19]*Gx2[18] + Gx1[25]*Gx2[24] + Gx1[31]*Gx2[30];
-acadoWorkspace.H[27] += + Gx1[1]*Gx2[1] + Gx1[7]*Gx2[7] + Gx1[13]*Gx2[13] + Gx1[19]*Gx2[19] + Gx1[25]*Gx2[25] + Gx1[31]*Gx2[31];
-acadoWorkspace.H[28] += + Gx1[1]*Gx2[2] + Gx1[7]*Gx2[8] + Gx1[13]*Gx2[14] + Gx1[19]*Gx2[20] + Gx1[25]*Gx2[26] + Gx1[31]*Gx2[32];
-acadoWorkspace.H[29] += + Gx1[1]*Gx2[3] + Gx1[7]*Gx2[9] + Gx1[13]*Gx2[15] + Gx1[19]*Gx2[21] + Gx1[25]*Gx2[27] + Gx1[31]*Gx2[33];
-acadoWorkspace.H[30] += + Gx1[1]*Gx2[4] + Gx1[7]*Gx2[10] + Gx1[13]*Gx2[16] + Gx1[19]*Gx2[22] + Gx1[25]*Gx2[28] + Gx1[31]*Gx2[34];
-acadoWorkspace.H[31] += + Gx1[1]*Gx2[5] + Gx1[7]*Gx2[11] + Gx1[13]*Gx2[17] + Gx1[19]*Gx2[23] + Gx1[25]*Gx2[29] + Gx1[31]*Gx2[35];
-acadoWorkspace.H[52] += + Gx1[2]*Gx2[0] + Gx1[8]*Gx2[6] + Gx1[14]*Gx2[12] + Gx1[20]*Gx2[18] + Gx1[26]*Gx2[24] + Gx1[32]*Gx2[30];
-acadoWorkspace.H[53] += + Gx1[2]*Gx2[1] + Gx1[8]*Gx2[7] + Gx1[14]*Gx2[13] + Gx1[20]*Gx2[19] + Gx1[26]*Gx2[25] + Gx1[32]*Gx2[31];
-acadoWorkspace.H[54] += + Gx1[2]*Gx2[2] + Gx1[8]*Gx2[8] + Gx1[14]*Gx2[14] + Gx1[20]*Gx2[20] + Gx1[26]*Gx2[26] + Gx1[32]*Gx2[32];
-acadoWorkspace.H[55] += + Gx1[2]*Gx2[3] + Gx1[8]*Gx2[9] + Gx1[14]*Gx2[15] + Gx1[20]*Gx2[21] + Gx1[26]*Gx2[27] + Gx1[32]*Gx2[33];
-acadoWorkspace.H[56] += + Gx1[2]*Gx2[4] + Gx1[8]*Gx2[10] + Gx1[14]*Gx2[16] + Gx1[20]*Gx2[22] + Gx1[26]*Gx2[28] + Gx1[32]*Gx2[34];
-acadoWorkspace.H[57] += + Gx1[2]*Gx2[5] + Gx1[8]*Gx2[11] + Gx1[14]*Gx2[17] + Gx1[20]*Gx2[23] + Gx1[26]*Gx2[29] + Gx1[32]*Gx2[35];
-acadoWorkspace.H[78] += + Gx1[3]*Gx2[0] + Gx1[9]*Gx2[6] + Gx1[15]*Gx2[12] + Gx1[21]*Gx2[18] + Gx1[27]*Gx2[24] + Gx1[33]*Gx2[30];
-acadoWorkspace.H[79] += + Gx1[3]*Gx2[1] + Gx1[9]*Gx2[7] + Gx1[15]*Gx2[13] + Gx1[21]*Gx2[19] + Gx1[27]*Gx2[25] + Gx1[33]*Gx2[31];
-acadoWorkspace.H[80] += + Gx1[3]*Gx2[2] + Gx1[9]*Gx2[8] + Gx1[15]*Gx2[14] + Gx1[21]*Gx2[20] + Gx1[27]*Gx2[26] + Gx1[33]*Gx2[32];
-acadoWorkspace.H[81] += + Gx1[3]*Gx2[3] + Gx1[9]*Gx2[9] + Gx1[15]*Gx2[15] + Gx1[21]*Gx2[21] + Gx1[27]*Gx2[27] + Gx1[33]*Gx2[33];
-acadoWorkspace.H[82] += + Gx1[3]*Gx2[4] + Gx1[9]*Gx2[10] + Gx1[15]*Gx2[16] + Gx1[21]*Gx2[22] + Gx1[27]*Gx2[28] + Gx1[33]*Gx2[34];
-acadoWorkspace.H[83] += + Gx1[3]*Gx2[5] + Gx1[9]*Gx2[11] + Gx1[15]*Gx2[17] + Gx1[21]*Gx2[23] + Gx1[27]*Gx2[29] + Gx1[33]*Gx2[35];
-acadoWorkspace.H[104] += + Gx1[4]*Gx2[0] + Gx1[10]*Gx2[6] + Gx1[16]*Gx2[12] + Gx1[22]*Gx2[18] + Gx1[28]*Gx2[24] + Gx1[34]*Gx2[30];
-acadoWorkspace.H[105] += + Gx1[4]*Gx2[1] + Gx1[10]*Gx2[7] + Gx1[16]*Gx2[13] + Gx1[22]*Gx2[19] + Gx1[28]*Gx2[25] + Gx1[34]*Gx2[31];
-acadoWorkspace.H[106] += + Gx1[4]*Gx2[2] + Gx1[10]*Gx2[8] + Gx1[16]*Gx2[14] + Gx1[22]*Gx2[20] + Gx1[28]*Gx2[26] + Gx1[34]*Gx2[32];
-acadoWorkspace.H[107] += + Gx1[4]*Gx2[3] + Gx1[10]*Gx2[9] + Gx1[16]*Gx2[15] + Gx1[22]*Gx2[21] + Gx1[28]*Gx2[27] + Gx1[34]*Gx2[33];
-acadoWorkspace.H[108] += + Gx1[4]*Gx2[4] + Gx1[10]*Gx2[10] + Gx1[16]*Gx2[16] + Gx1[22]*Gx2[22] + Gx1[28]*Gx2[28] + Gx1[34]*Gx2[34];
-acadoWorkspace.H[109] += + Gx1[4]*Gx2[5] + Gx1[10]*Gx2[11] + Gx1[16]*Gx2[17] + Gx1[22]*Gx2[23] + Gx1[28]*Gx2[29] + Gx1[34]*Gx2[35];
-acadoWorkspace.H[130] += + Gx1[5]*Gx2[0] + Gx1[11]*Gx2[6] + Gx1[17]*Gx2[12] + Gx1[23]*Gx2[18] + Gx1[29]*Gx2[24] + Gx1[35]*Gx2[30];
-acadoWorkspace.H[131] += + Gx1[5]*Gx2[1] + Gx1[11]*Gx2[7] + Gx1[17]*Gx2[13] + Gx1[23]*Gx2[19] + Gx1[29]*Gx2[25] + Gx1[35]*Gx2[31];
-acadoWorkspace.H[132] += + Gx1[5]*Gx2[2] + Gx1[11]*Gx2[8] + Gx1[17]*Gx2[14] + Gx1[23]*Gx2[20] + Gx1[29]*Gx2[26] + Gx1[35]*Gx2[32];
-acadoWorkspace.H[133] += + Gx1[5]*Gx2[3] + Gx1[11]*Gx2[9] + Gx1[17]*Gx2[15] + Gx1[23]*Gx2[21] + Gx1[29]*Gx2[27] + Gx1[35]*Gx2[33];
-acadoWorkspace.H[134] += + Gx1[5]*Gx2[4] + Gx1[11]*Gx2[10] + Gx1[17]*Gx2[16] + Gx1[23]*Gx2[22] + Gx1[29]*Gx2[28] + Gx1[35]*Gx2[34];
-acadoWorkspace.H[135] += + Gx1[5]*Gx2[5] + Gx1[11]*Gx2[11] + Gx1[17]*Gx2[17] + Gx1[23]*Gx2[23] + Gx1[29]*Gx2[29] + Gx1[35]*Gx2[35];
+acadoWorkspace.H[0] += + Gx1[0]*Gx2[0] + Gx1[3]*Gx2[3] + Gx1[6]*Gx2[6];
+acadoWorkspace.H[1] += + Gx1[0]*Gx2[1] + Gx1[3]*Gx2[4] + Gx1[6]*Gx2[7];
+acadoWorkspace.H[2] += + Gx1[0]*Gx2[2] + Gx1[3]*Gx2[5] + Gx1[6]*Gx2[8];
+acadoWorkspace.H[23] += + Gx1[1]*Gx2[0] + Gx1[4]*Gx2[3] + Gx1[7]*Gx2[6];
+acadoWorkspace.H[24] += + Gx1[1]*Gx2[1] + Gx1[4]*Gx2[4] + Gx1[7]*Gx2[7];
+acadoWorkspace.H[25] += + Gx1[1]*Gx2[2] + Gx1[4]*Gx2[5] + Gx1[7]*Gx2[8];
+acadoWorkspace.H[46] += + Gx1[2]*Gx2[0] + Gx1[5]*Gx2[3] + Gx1[8]*Gx2[6];
+acadoWorkspace.H[47] += + Gx1[2]*Gx2[1] + Gx1[5]*Gx2[4] + Gx1[8]*Gx2[7];
+acadoWorkspace.H[48] += + Gx1[2]*Gx2[2] + Gx1[5]*Gx2[5] + Gx1[8]*Gx2[8];
}
void acado_macCTSlx( real_t* const C0, real_t* const g0 )
@@ -698,12 +399,6 @@ g0[1] += 0.0;
;
g0[2] += 0.0;
;
-g0[3] += 0.0;
-;
-g0[4] += 0.0;
-;
-g0[5] += 0.0;
-;
}
void acado_macETSlu( real_t* const E0, real_t* const g1 )
@@ -721,2896 +416,2836 @@ int lRun4;
int lRun5;
/** Row vector of size: 20 */
static const int xBoundIndices[ 20 ] =
-{ 7, 13, 19, 25, 31, 37, 43, 49, 55, 61, 67, 73, 79, 85, 91, 97, 103, 109, 115, 121 };
+{ 4, 7, 10, 13, 16, 19, 22, 25, 28, 31, 34, 37, 40, 43, 46, 49, 52, 55, 58, 61 };
acado_moveGuE( acadoWorkspace.evGu, acadoWorkspace.E );
+acado_moveGxT( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.T );
+acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.d[ 3 ]) );
+acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 9 ]) );
+
+acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 3 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 3 ]), &(acadoWorkspace.E[ 6 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.d[ 6 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.evGx[ 18 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.E[ 9 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.E[ 12 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 6 ]), &(acadoWorkspace.E[ 15 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.d[ 9 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.evGx[ 27 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.E[ 18 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 21 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.E[ 24 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 9 ]), &(acadoWorkspace.E[ 27 ]) );
+
acado_moveGxT( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.T );
-acado_multGxd( acadoWorkspace.d, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.d[ 6 ]) );
-acado_multGxGx( acadoWorkspace.T, acadoWorkspace.evGx, &(acadoWorkspace.evGx[ 36 ]) );
+acado_multGxd( &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.d[ 12 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.evGx[ 36 ]) );
-acado_multGxGu( acadoWorkspace.T, acadoWorkspace.E, &(acadoWorkspace.E[ 6 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.E[ 30 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.E[ 33 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 36 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.E[ 39 ]) );
-acado_moveGuE( &(acadoWorkspace.evGu[ 6 ]), &(acadoWorkspace.E[ 12 ]) );
+acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 42 ]) );
-acado_moveGxT( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.d[ 12 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.evGx[ 72 ]) );
+acado_moveGxT( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.d[ 15 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.evGx[ 45 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.E[ 18 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.E[ 24 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.E[ 45 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.E[ 48 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 51 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.E[ 54 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.E[ 57 ]) );
-acado_moveGuE( &(acadoWorkspace.evGu[ 12 ]), &(acadoWorkspace.E[ 30 ]) );
+acado_moveGuE( &(acadoWorkspace.evGu[ 15 ]), &(acadoWorkspace.E[ 60 ]) );
-acado_moveGxT( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.d[ 18 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.evGx[ 108 ]) );
+acado_moveGxT( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.d[ 18 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.evGx[ 54 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.E[ 63 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 66 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.E[ 69 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.E[ 72 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.E[ 75 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 78 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 18 ]), &(acadoWorkspace.E[ 81 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.E[ 36 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.E[ 42 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.E[ 48 ]) );
+acado_moveGxT( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.d[ 21 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.evGx[ 63 ]) );
-acado_moveGuE( &(acadoWorkspace.evGu[ 18 ]), &(acadoWorkspace.E[ 54 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.E[ 84 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.E[ 87 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.E[ 90 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 93 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.E[ 96 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.E[ 99 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.E[ 102 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 21 ]), &(acadoWorkspace.E[ 105 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.d[ 24 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.evGx[ 72 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 108 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.E[ 111 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.E[ 114 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.E[ 117 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 120 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.E[ 123 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.E[ 126 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.E[ 129 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 132 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.d[ 27 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.evGx[ 81 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 135 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.E[ 138 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.E[ 141 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.E[ 144 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 147 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.E[ 150 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.E[ 153 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.E[ 156 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 159 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 27 ]), &(acadoWorkspace.E[ 162 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.d[ 30 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.evGx[ 90 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.E[ 165 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.E[ 168 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.E[ 171 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 174 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.E[ 177 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.E[ 180 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.E[ 183 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 186 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.E[ 189 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.E[ 192 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 30 ]), &(acadoWorkspace.E[ 195 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.d[ 33 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.evGx[ 99 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.E[ 198 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 201 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.E[ 204 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.E[ 207 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.E[ 210 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 213 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.E[ 216 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.E[ 219 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.E[ 222 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 225 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.E[ 228 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 33 ]), &(acadoWorkspace.E[ 231 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.d[ 36 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.evGx[ 108 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.E[ 234 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.E[ 237 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 240 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.E[ 243 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.E[ 246 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.E[ 249 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 252 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.E[ 255 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.E[ 258 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.E[ 261 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 264 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.E[ 267 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 270 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.d[ 39 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.evGx[ 117 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.E[ 273 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.E[ 276 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 279 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.E[ 282 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.E[ 285 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.E[ 288 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 291 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.E[ 294 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.E[ 297 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.E[ 300 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 303 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.E[ 306 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.E[ 309 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 39 ]), &(acadoWorkspace.E[ 312 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.d[ 42 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.evGx[ 126 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.E[ 315 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 318 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.E[ 321 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.E[ 324 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.E[ 327 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 330 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.E[ 333 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.E[ 336 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.E[ 339 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 342 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.E[ 345 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.E[ 348 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.E[ 351 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 354 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 42 ]), &(acadoWorkspace.E[ 357 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.d[ 45 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.evGx[ 135 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.E[ 360 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.E[ 363 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.E[ 366 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 369 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.E[ 372 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.E[ 375 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.E[ 378 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 381 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.E[ 384 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.E[ 387 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.E[ 390 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 393 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.E[ 396 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.E[ 399 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.E[ 402 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 45 ]), &(acadoWorkspace.E[ 405 ]) );
acado_moveGxT( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 24 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.evGx[ 144 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.E[ 60 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.E[ 66 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.E[ 72 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.E[ 78 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 24 ]), &(acadoWorkspace.E[ 84 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 180 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.d[ 30 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 180 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.E[ 90 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.E[ 96 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.E[ 102 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.E[ 108 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.E[ 114 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 30 ]), &(acadoWorkspace.E[ 120 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 216 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.d[ 36 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.evGx[ 216 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.E[ 126 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.E[ 132 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.E[ 138 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.E[ 144 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.E[ 150 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.E[ 156 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 36 ]), &(acadoWorkspace.E[ 162 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 252 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.d[ 42 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.evGx[ 252 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.E[ 168 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.E[ 174 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.E[ 180 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.E[ 186 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.E[ 192 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.E[ 198 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.E[ 204 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 42 ]), &(acadoWorkspace.E[ 210 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.d[ 48 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.evGx[ 288 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.E[ 216 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.E[ 222 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.E[ 228 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.E[ 234 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.E[ 240 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.E[ 246 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.E[ 252 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.E[ 258 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 264 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 324 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.d[ 54 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.evGx[ 324 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.E[ 270 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.E[ 276 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.E[ 282 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.E[ 288 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.E[ 294 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.E[ 300 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.E[ 306 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.E[ 312 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.E[ 318 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 54 ]), &(acadoWorkspace.E[ 324 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 360 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.d[ 60 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.evGx[ 360 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.E[ 330 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.E[ 336 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.E[ 342 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.E[ 348 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.E[ 354 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.E[ 360 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.E[ 366 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.E[ 372 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.E[ 378 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.E[ 384 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 60 ]), &(acadoWorkspace.E[ 390 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 396 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.d[ 66 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.evGx[ 396 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.E[ 396 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.E[ 402 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.E[ 408 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.E[ 414 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.E[ 420 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 426 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.E[ 432 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 438 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.E[ 444 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 450 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.E[ 456 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 66 ]), &(acadoWorkspace.E[ 462 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 66 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.d[ 72 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.evGx[ 432 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 468 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.E[ 474 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 480 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.E[ 486 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 492 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.E[ 498 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 504 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.E[ 510 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 516 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.E[ 522 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 528 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.E[ 534 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 72 ]), &(acadoWorkspace.E[ 540 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 468 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.d[ 78 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.evGx[ 468 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 546 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.E[ 552 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 558 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.E[ 564 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 570 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.E[ 576 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 582 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.E[ 588 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 594 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.E[ 600 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 606 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.E[ 612 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 618 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 78 ]), &(acadoWorkspace.E[ 624 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 504 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 78 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.d[ 84 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.evGx[ 504 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.E[ 630 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 636 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.E[ 642 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 648 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.E[ 654 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.E[ 660 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.E[ 666 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.E[ 672 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.E[ 678 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.E[ 684 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.E[ 690 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.E[ 696 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.E[ 702 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.E[ 708 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 84 ]), &(acadoWorkspace.E[ 714 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 540 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.d[ 90 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.evGx[ 540 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.E[ 720 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.E[ 726 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.E[ 732 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.E[ 738 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.E[ 744 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.E[ 750 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.E[ 756 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.E[ 762 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.E[ 768 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.E[ 774 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.E[ 780 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.E[ 786 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.E[ 792 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.E[ 798 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.E[ 804 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 90 ]), &(acadoWorkspace.E[ 810 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 90 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.d[ 96 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.evGx[ 576 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.E[ 816 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.E[ 822 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.E[ 828 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.E[ 834 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.E[ 840 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.E[ 846 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.E[ 852 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.E[ 858 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.E[ 864 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.E[ 870 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.E[ 876 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.E[ 882 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.E[ 888 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.E[ 894 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.E[ 900 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.E[ 906 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 96 ]), &(acadoWorkspace.E[ 912 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.d[ 102 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.evGx[ 612 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.E[ 918 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.E[ 924 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.E[ 930 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.E[ 936 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.E[ 942 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.E[ 948 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.E[ 954 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.E[ 960 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.E[ 966 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.E[ 972 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.E[ 978 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.E[ 984 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.E[ 990 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.E[ 996 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.E[ 1002 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.E[ 1008 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.E[ 1014 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 102 ]), &(acadoWorkspace.E[ 1020 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 102 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.d[ 108 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.evGx[ 648 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.E[ 1026 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.E[ 1032 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.E[ 1038 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.E[ 1044 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.E[ 1050 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.E[ 1056 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.E[ 1062 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.E[ 1068 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.E[ 1074 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.E[ 1080 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.E[ 1086 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.E[ 1092 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.E[ 1098 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.E[ 1104 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.E[ 1110 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.E[ 1116 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.E[ 1122 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.E[ 1128 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 108 ]), &(acadoWorkspace.E[ 1134 ]) );
-
-acado_moveGxT( &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.T );
-acado_multGxd( &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.d[ 114 ]) );
-acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.evGx[ 684 ]) );
-
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.E[ 1140 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.E[ 1146 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.E[ 1152 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.E[ 1158 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.E[ 1164 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.E[ 1170 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.E[ 1176 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.E[ 1182 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.E[ 1188 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.E[ 1194 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.E[ 1200 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.E[ 1206 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.E[ 1212 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.E[ 1218 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.E[ 1224 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.E[ 1230 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.E[ 1236 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.E[ 1242 ]) );
-acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.E[ 1248 ]) );
-
-acado_moveGuE( &(acadoWorkspace.evGu[ 114 ]), &(acadoWorkspace.E[ 1254 ]) );
-
-acado_multGxGx( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.evGx, acadoWorkspace.QGx );
-acado_multGxGx( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.QGx[ 180 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.QGx[ 216 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.QGx[ 252 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.QGx[ 324 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.QGx[ 360 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.QGx[ 396 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.QGx[ 468 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.QGx[ 504 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.QGx[ 540 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.QGx[ 612 ]) );
-acado_multGxGx( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.QGx[ 648 ]) );
-acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.QGx[ 684 ]) );
-
-acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.E, acadoWorkspace.QE );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 630 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 642 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 726 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 738 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 822 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 834 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 918 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 924 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 930 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 936 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1026 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1032 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1038 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1044 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_multGxGu( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1140 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1146 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1152 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1158 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1164 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1170 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1176 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1182 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1188 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1194 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1200 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1206 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1212 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1218 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1224 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1230 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1236 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1242 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1248 ]) );
-acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_multGxd( &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.d[ 48 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.evGx[ 144 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.E[ 408 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.E[ 411 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.E[ 414 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.E[ 417 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.E[ 420 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.E[ 423 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.E[ 426 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.E[ 429 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.E[ 432 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.E[ 435 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.E[ 438 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.E[ 441 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.E[ 444 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.E[ 447 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.E[ 450 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.E[ 453 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 48 ]), &(acadoWorkspace.E[ 456 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.d[ 51 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.evGx[ 153 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.E[ 459 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.E[ 462 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.E[ 465 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.E[ 468 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.E[ 471 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.E[ 474 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.E[ 477 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.E[ 480 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.E[ 483 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.E[ 486 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.E[ 489 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.E[ 492 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.E[ 495 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.E[ 498 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.E[ 501 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.E[ 504 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.E[ 507 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 51 ]), &(acadoWorkspace.E[ 510 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.d[ 54 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.evGx[ 162 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.E[ 513 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.E[ 516 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.E[ 519 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.E[ 522 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.E[ 525 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.E[ 528 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.E[ 531 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.E[ 534 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.E[ 537 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.E[ 540 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.E[ 543 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.E[ 546 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.E[ 549 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.E[ 552 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.E[ 555 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.E[ 558 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.E[ 561 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.E[ 564 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 54 ]), &(acadoWorkspace.E[ 567 ]) );
+
+acado_moveGxT( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.T );
+acado_multGxd( &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.d[ 57 ]) );
+acado_multGxGx( acadoWorkspace.T, &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.evGx[ 171 ]) );
+
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.E[ 570 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.E[ 573 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.E[ 576 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.E[ 579 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.E[ 582 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.E[ 585 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.E[ 588 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.E[ 591 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.E[ 594 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.E[ 597 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.E[ 600 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.E[ 603 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.E[ 606 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.E[ 609 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.E[ 612 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.E[ 615 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.E[ 618 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.E[ 621 ]) );
+acado_multGxGu( acadoWorkspace.T, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.E[ 624 ]) );
+
+acado_moveGuE( &(acadoWorkspace.evGu[ 57 ]), &(acadoWorkspace.E[ 627 ]) );
+
+acado_multGxGx( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.evGx, acadoWorkspace.QGx );
+acado_multGxGx( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) );
+acado_multGxGx( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) );
+acado_multGxGx( acadoWorkspace.QN1, &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) );
+
+acado_multGxGu( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.E, acadoWorkspace.QE );
+acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_multGxGu( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
+acado_multGxGu( acadoWorkspace.QN1, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_zeroBlockH00( );
acado_multCTQC( acadoWorkspace.evGx, acadoWorkspace.QGx );
+acado_multCTQC( &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.QGx[ 9 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.QGx[ 18 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.QGx[ 27 ]) );
acado_multCTQC( &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.QGx[ 36 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.QGx[ 45 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.QGx[ 54 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.QGx[ 63 ]) );
acado_multCTQC( &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.QGx[ 72 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.QGx[ 81 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.QGx[ 90 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.QGx[ 99 ]) );
acado_multCTQC( &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.QGx[ 108 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.QGx[ 117 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.QGx[ 126 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.QGx[ 135 ]) );
acado_multCTQC( &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.QGx[ 144 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.QGx[ 180 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.QGx[ 216 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.QGx[ 252 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.QGx[ 288 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.QGx[ 324 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.QGx[ 360 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.QGx[ 396 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.QGx[ 432 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.QGx[ 468 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.QGx[ 504 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.QGx[ 540 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.QGx[ 576 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.QGx[ 612 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.QGx[ 648 ]) );
-acado_multCTQC( &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.QGx[ 684 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.QGx[ 153 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.QGx[ 162 ]) );
+acado_multCTQC( &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.QGx[ 171 ]) );
acado_zeroBlockH10( acadoWorkspace.H10 );
acado_multQETGx( acadoWorkspace.QE, acadoWorkspace.evGx, acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.evGx[ 180 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.evGx[ 216 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 252 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.evGx[ 324 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.evGx[ 360 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 396 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.evGx[ 468 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 630 ]), &(acadoWorkspace.evGx[ 504 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.evGx[ 540 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 918 ]), &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 1026 ]), &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.H10 );
-acado_multQETGx( &(acadoWorkspace.QE[ 1140 ]), &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.H10 );
+acado_multQETGx( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.H10 );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.evGx[ 9 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 3 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 3 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 726 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 822 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 924 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1032 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 6 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1146 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.evGx[ 18 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 6 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.evGx[ 27 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 9 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 9 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 642 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 930 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1038 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 12 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1152 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.evGx[ 36 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 12 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.evGx[ 45 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 15 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 15 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 738 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 834 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 936 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1044 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 18 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1158 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.evGx[ 54 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 18 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.evGx[ 63 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 21 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 21 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 654 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 840 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 942 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1050 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 24 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1164 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.evGx[ 72 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 24 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.evGx[ 81 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 27 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 27 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.evGx[ 180 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 750 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 846 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 948 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1056 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 30 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1170 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.evGx[ 90 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 30 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.evGx[ 99 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 33 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 33 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.evGx[ 216 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 666 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 852 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 954 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1062 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 36 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1176 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.evGx[ 108 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 36 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 117 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 39 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 39 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.evGx[ 252 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 762 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 858 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 960 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1068 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 42 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1182 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.evGx[ 126 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 42 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 45 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.evGx[ 135 ]), &(acadoWorkspace.H10[ 45 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 45 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 45 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 45 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 45 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.evGx[ 288 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 678 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 864 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 966 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1074 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 48 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1188 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 48 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 144 ]), &(acadoWorkspace.H10[ 48 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 48 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 48 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 48 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 51 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.evGx[ 153 ]), &(acadoWorkspace.H10[ 51 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 51 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 51 ]) );
acado_zeroBlockH10( &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.evGx[ 324 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 774 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 870 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 972 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1080 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1194 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 54 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.evGx[ 360 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 690 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 876 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 978 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1086 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1200 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 60 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.evGx[ 396 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 786 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 882 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 984 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1092 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1206 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 66 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.evGx[ 432 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 702 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 888 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 990 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1098 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1212 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 72 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 468 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 798 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 894 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 996 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1104 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1218 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 78 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 714 ]), &(acadoWorkspace.evGx[ 504 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 900 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1002 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1110 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1224 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 84 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 90 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 810 ]), &(acadoWorkspace.evGx[ 540 ]), &(acadoWorkspace.H10[ 90 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 906 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 90 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1008 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 90 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1116 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 90 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1230 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 90 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 96 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 912 ]), &(acadoWorkspace.evGx[ 576 ]), &(acadoWorkspace.H10[ 96 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1014 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 96 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1122 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 96 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1236 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 96 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 102 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1020 ]), &(acadoWorkspace.evGx[ 612 ]), &(acadoWorkspace.H10[ 102 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1128 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 102 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1242 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 102 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 108 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1134 ]), &(acadoWorkspace.evGx[ 648 ]), &(acadoWorkspace.H10[ 108 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1248 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 108 ]) );
-acado_zeroBlockH10( &(acadoWorkspace.H10[ 114 ]) );
-acado_multQETGx( &(acadoWorkspace.QE[ 1254 ]), &(acadoWorkspace.evGx[ 684 ]), &(acadoWorkspace.H10[ 114 ]) );
-
-acadoWorkspace.H[6] = acadoWorkspace.H10[0];
-acadoWorkspace.H[7] = acadoWorkspace.H10[6];
-acadoWorkspace.H[8] = acadoWorkspace.H10[12];
+acado_multQETGx( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.evGx[ 162 ]), &(acadoWorkspace.H10[ 54 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 54 ]) );
+acado_zeroBlockH10( &(acadoWorkspace.H10[ 57 ]) );
+acado_multQETGx( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.evGx[ 171 ]), &(acadoWorkspace.H10[ 57 ]) );
+
+acadoWorkspace.H[3] = acadoWorkspace.H10[0];
+acadoWorkspace.H[4] = acadoWorkspace.H10[3];
+acadoWorkspace.H[5] = acadoWorkspace.H10[6];
+acadoWorkspace.H[6] = acadoWorkspace.H10[9];
+acadoWorkspace.H[7] = acadoWorkspace.H10[12];
+acadoWorkspace.H[8] = acadoWorkspace.H10[15];
acadoWorkspace.H[9] = acadoWorkspace.H10[18];
-acadoWorkspace.H[10] = acadoWorkspace.H10[24];
-acadoWorkspace.H[11] = acadoWorkspace.H10[30];
-acadoWorkspace.H[12] = acadoWorkspace.H10[36];
-acadoWorkspace.H[13] = acadoWorkspace.H10[42];
-acadoWorkspace.H[14] = acadoWorkspace.H10[48];
-acadoWorkspace.H[15] = acadoWorkspace.H10[54];
-acadoWorkspace.H[16] = acadoWorkspace.H10[60];
-acadoWorkspace.H[17] = acadoWorkspace.H10[66];
-acadoWorkspace.H[18] = acadoWorkspace.H10[72];
-acadoWorkspace.H[19] = acadoWorkspace.H10[78];
-acadoWorkspace.H[20] = acadoWorkspace.H10[84];
-acadoWorkspace.H[21] = acadoWorkspace.H10[90];
-acadoWorkspace.H[22] = acadoWorkspace.H10[96];
-acadoWorkspace.H[23] = acadoWorkspace.H10[102];
-acadoWorkspace.H[24] = acadoWorkspace.H10[108];
-acadoWorkspace.H[25] = acadoWorkspace.H10[114];
-acadoWorkspace.H[32] = acadoWorkspace.H10[1];
-acadoWorkspace.H[33] = acadoWorkspace.H10[7];
-acadoWorkspace.H[34] = acadoWorkspace.H10[13];
-acadoWorkspace.H[35] = acadoWorkspace.H10[19];
-acadoWorkspace.H[36] = acadoWorkspace.H10[25];
-acadoWorkspace.H[37] = acadoWorkspace.H10[31];
+acadoWorkspace.H[10] = acadoWorkspace.H10[21];
+acadoWorkspace.H[11] = acadoWorkspace.H10[24];
+acadoWorkspace.H[12] = acadoWorkspace.H10[27];
+acadoWorkspace.H[13] = acadoWorkspace.H10[30];
+acadoWorkspace.H[14] = acadoWorkspace.H10[33];
+acadoWorkspace.H[15] = acadoWorkspace.H10[36];
+acadoWorkspace.H[16] = acadoWorkspace.H10[39];
+acadoWorkspace.H[17] = acadoWorkspace.H10[42];
+acadoWorkspace.H[18] = acadoWorkspace.H10[45];
+acadoWorkspace.H[19] = acadoWorkspace.H10[48];
+acadoWorkspace.H[20] = acadoWorkspace.H10[51];
+acadoWorkspace.H[21] = acadoWorkspace.H10[54];
+acadoWorkspace.H[22] = acadoWorkspace.H10[57];
+acadoWorkspace.H[26] = acadoWorkspace.H10[1];
+acadoWorkspace.H[27] = acadoWorkspace.H10[4];
+acadoWorkspace.H[28] = acadoWorkspace.H10[7];
+acadoWorkspace.H[29] = acadoWorkspace.H10[10];
+acadoWorkspace.H[30] = acadoWorkspace.H10[13];
+acadoWorkspace.H[31] = acadoWorkspace.H10[16];
+acadoWorkspace.H[32] = acadoWorkspace.H10[19];
+acadoWorkspace.H[33] = acadoWorkspace.H10[22];
+acadoWorkspace.H[34] = acadoWorkspace.H10[25];
+acadoWorkspace.H[35] = acadoWorkspace.H10[28];
+acadoWorkspace.H[36] = acadoWorkspace.H10[31];
+acadoWorkspace.H[37] = acadoWorkspace.H10[34];
acadoWorkspace.H[38] = acadoWorkspace.H10[37];
-acadoWorkspace.H[39] = acadoWorkspace.H10[43];
-acadoWorkspace.H[40] = acadoWorkspace.H10[49];
-acadoWorkspace.H[41] = acadoWorkspace.H10[55];
-acadoWorkspace.H[42] = acadoWorkspace.H10[61];
-acadoWorkspace.H[43] = acadoWorkspace.H10[67];
-acadoWorkspace.H[44] = acadoWorkspace.H10[73];
-acadoWorkspace.H[45] = acadoWorkspace.H10[79];
-acadoWorkspace.H[46] = acadoWorkspace.H10[85];
-acadoWorkspace.H[47] = acadoWorkspace.H10[91];
-acadoWorkspace.H[48] = acadoWorkspace.H10[97];
-acadoWorkspace.H[49] = acadoWorkspace.H10[103];
-acadoWorkspace.H[50] = acadoWorkspace.H10[109];
-acadoWorkspace.H[51] = acadoWorkspace.H10[115];
-acadoWorkspace.H[58] = acadoWorkspace.H10[2];
-acadoWorkspace.H[59] = acadoWorkspace.H10[8];
-acadoWorkspace.H[60] = acadoWorkspace.H10[14];
-acadoWorkspace.H[61] = acadoWorkspace.H10[20];
-acadoWorkspace.H[62] = acadoWorkspace.H10[26];
-acadoWorkspace.H[63] = acadoWorkspace.H10[32];
-acadoWorkspace.H[64] = acadoWorkspace.H10[38];
-acadoWorkspace.H[65] = acadoWorkspace.H10[44];
-acadoWorkspace.H[66] = acadoWorkspace.H10[50];
+acadoWorkspace.H[39] = acadoWorkspace.H10[40];
+acadoWorkspace.H[40] = acadoWorkspace.H10[43];
+acadoWorkspace.H[41] = acadoWorkspace.H10[46];
+acadoWorkspace.H[42] = acadoWorkspace.H10[49];
+acadoWorkspace.H[43] = acadoWorkspace.H10[52];
+acadoWorkspace.H[44] = acadoWorkspace.H10[55];
+acadoWorkspace.H[45] = acadoWorkspace.H10[58];
+acadoWorkspace.H[49] = acadoWorkspace.H10[2];
+acadoWorkspace.H[50] = acadoWorkspace.H10[5];
+acadoWorkspace.H[51] = acadoWorkspace.H10[8];
+acadoWorkspace.H[52] = acadoWorkspace.H10[11];
+acadoWorkspace.H[53] = acadoWorkspace.H10[14];
+acadoWorkspace.H[54] = acadoWorkspace.H10[17];
+acadoWorkspace.H[55] = acadoWorkspace.H10[20];
+acadoWorkspace.H[56] = acadoWorkspace.H10[23];
+acadoWorkspace.H[57] = acadoWorkspace.H10[26];
+acadoWorkspace.H[58] = acadoWorkspace.H10[29];
+acadoWorkspace.H[59] = acadoWorkspace.H10[32];
+acadoWorkspace.H[60] = acadoWorkspace.H10[35];
+acadoWorkspace.H[61] = acadoWorkspace.H10[38];
+acadoWorkspace.H[62] = acadoWorkspace.H10[41];
+acadoWorkspace.H[63] = acadoWorkspace.H10[44];
+acadoWorkspace.H[64] = acadoWorkspace.H10[47];
+acadoWorkspace.H[65] = acadoWorkspace.H10[50];
+acadoWorkspace.H[66] = acadoWorkspace.H10[53];
acadoWorkspace.H[67] = acadoWorkspace.H10[56];
-acadoWorkspace.H[68] = acadoWorkspace.H10[62];
-acadoWorkspace.H[69] = acadoWorkspace.H10[68];
-acadoWorkspace.H[70] = acadoWorkspace.H10[74];
-acadoWorkspace.H[71] = acadoWorkspace.H10[80];
-acadoWorkspace.H[72] = acadoWorkspace.H10[86];
-acadoWorkspace.H[73] = acadoWorkspace.H10[92];
-acadoWorkspace.H[74] = acadoWorkspace.H10[98];
-acadoWorkspace.H[75] = acadoWorkspace.H10[104];
-acadoWorkspace.H[76] = acadoWorkspace.H10[110];
-acadoWorkspace.H[77] = acadoWorkspace.H10[116];
-acadoWorkspace.H[84] = acadoWorkspace.H10[3];
-acadoWorkspace.H[85] = acadoWorkspace.H10[9];
-acadoWorkspace.H[86] = acadoWorkspace.H10[15];
-acadoWorkspace.H[87] = acadoWorkspace.H10[21];
-acadoWorkspace.H[88] = acadoWorkspace.H10[27];
-acadoWorkspace.H[89] = acadoWorkspace.H10[33];
-acadoWorkspace.H[90] = acadoWorkspace.H10[39];
-acadoWorkspace.H[91] = acadoWorkspace.H10[45];
-acadoWorkspace.H[92] = acadoWorkspace.H10[51];
-acadoWorkspace.H[93] = acadoWorkspace.H10[57];
-acadoWorkspace.H[94] = acadoWorkspace.H10[63];
-acadoWorkspace.H[95] = acadoWorkspace.H10[69];
-acadoWorkspace.H[96] = acadoWorkspace.H10[75];
-acadoWorkspace.H[97] = acadoWorkspace.H10[81];
-acadoWorkspace.H[98] = acadoWorkspace.H10[87];
-acadoWorkspace.H[99] = acadoWorkspace.H10[93];
-acadoWorkspace.H[100] = acadoWorkspace.H10[99];
-acadoWorkspace.H[101] = acadoWorkspace.H10[105];
-acadoWorkspace.H[102] = acadoWorkspace.H10[111];
-acadoWorkspace.H[103] = acadoWorkspace.H10[117];
-acadoWorkspace.H[110] = acadoWorkspace.H10[4];
-acadoWorkspace.H[111] = acadoWorkspace.H10[10];
-acadoWorkspace.H[112] = acadoWorkspace.H10[16];
-acadoWorkspace.H[113] = acadoWorkspace.H10[22];
-acadoWorkspace.H[114] = acadoWorkspace.H10[28];
-acadoWorkspace.H[115] = acadoWorkspace.H10[34];
-acadoWorkspace.H[116] = acadoWorkspace.H10[40];
-acadoWorkspace.H[117] = acadoWorkspace.H10[46];
-acadoWorkspace.H[118] = acadoWorkspace.H10[52];
-acadoWorkspace.H[119] = acadoWorkspace.H10[58];
-acadoWorkspace.H[120] = acadoWorkspace.H10[64];
-acadoWorkspace.H[121] = acadoWorkspace.H10[70];
-acadoWorkspace.H[122] = acadoWorkspace.H10[76];
-acadoWorkspace.H[123] = acadoWorkspace.H10[82];
-acadoWorkspace.H[124] = acadoWorkspace.H10[88];
-acadoWorkspace.H[125] = acadoWorkspace.H10[94];
-acadoWorkspace.H[126] = acadoWorkspace.H10[100];
-acadoWorkspace.H[127] = acadoWorkspace.H10[106];
-acadoWorkspace.H[128] = acadoWorkspace.H10[112];
-acadoWorkspace.H[129] = acadoWorkspace.H10[118];
-acadoWorkspace.H[136] = acadoWorkspace.H10[5];
-acadoWorkspace.H[137] = acadoWorkspace.H10[11];
-acadoWorkspace.H[138] = acadoWorkspace.H10[17];
-acadoWorkspace.H[139] = acadoWorkspace.H10[23];
-acadoWorkspace.H[140] = acadoWorkspace.H10[29];
-acadoWorkspace.H[141] = acadoWorkspace.H10[35];
-acadoWorkspace.H[142] = acadoWorkspace.H10[41];
-acadoWorkspace.H[143] = acadoWorkspace.H10[47];
-acadoWorkspace.H[144] = acadoWorkspace.H10[53];
-acadoWorkspace.H[145] = acadoWorkspace.H10[59];
-acadoWorkspace.H[146] = acadoWorkspace.H10[65];
-acadoWorkspace.H[147] = acadoWorkspace.H10[71];
-acadoWorkspace.H[148] = acadoWorkspace.H10[77];
-acadoWorkspace.H[149] = acadoWorkspace.H10[83];
-acadoWorkspace.H[150] = acadoWorkspace.H10[89];
-acadoWorkspace.H[151] = acadoWorkspace.H10[95];
-acadoWorkspace.H[152] = acadoWorkspace.H10[101];
-acadoWorkspace.H[153] = acadoWorkspace.H10[107];
-acadoWorkspace.H[154] = acadoWorkspace.H10[113];
-acadoWorkspace.H[155] = acadoWorkspace.H10[119];
+acadoWorkspace.H[68] = acadoWorkspace.H10[59];
acado_setBlockH11_R1( 0, 0, acadoWorkspace.R1 );
acado_setBlockH11( 0, 0, acadoWorkspace.E, acadoWorkspace.QE );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 3 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 9 ]) );
acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 18 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 630 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 720 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 816 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 918 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1026 ]) );
-acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1140 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 45 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 63 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 135 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 165 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 273 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 315 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 459 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 513 ]) );
+acado_setBlockH11( 0, 0, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
acado_zeroBlockH11( 0, 1 );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 42 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 222 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 636 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 726 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 822 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 924 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1032 ]) );
-acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1146 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QE[ 6 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 12 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 21 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 33 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 48 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 66 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 87 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 111 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 138 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 168 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 201 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 237 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 276 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 318 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 363 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 411 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 462 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 516 ]) );
+acado_setBlockH11( 0, 1, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 573 ]) );
acado_zeroBlockH11( 0, 2 );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 642 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 732 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 828 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 930 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1038 ]) );
-acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1152 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QE[ 15 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 24 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 36 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 51 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 69 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 90 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 114 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 141 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 171 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 204 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 240 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 279 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 321 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 366 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 414 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 465 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 519 ]) );
+acado_setBlockH11( 0, 2, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 576 ]) );
acado_zeroBlockH11( 0, 3 );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 648 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 738 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 834 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 936 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1044 ]) );
-acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1158 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QE[ 27 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 39 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 54 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 72 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 93 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 117 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 144 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 174 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 207 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 243 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 282 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 324 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 369 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 417 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 468 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 522 ]) );
+acado_setBlockH11( 0, 3, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 579 ]) );
acado_zeroBlockH11( 0, 4 );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1164 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 372 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 420 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_setBlockH11( 0, 4, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 582 ]) );
acado_zeroBlockH11( 0, 5 );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 0, 5, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 0, 6 );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 0, 6, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 0, 7 );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 0, 7, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 0, 8 );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 0, 8, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 0, 9 );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 0, 9, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 0, 10 );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 0, 10, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 0, 11 );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 0, 11, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 0, 12 );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 0, 12, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 0, 13 );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 0, 13, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 0, 14 );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 0, 14, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 0, 15 );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 0, 15, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 0, 16 );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 0, 16, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 0, 17 );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 0, 17, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 0, 18 );
-acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 0, 18, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 0, 19 );
-acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 0, 19, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 1, 1, &(acadoWorkspace.R1[ 1 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QE[ 6 ]) );
acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 12 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 21 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 33 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 66 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 87 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 111 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 168 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 201 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 237 ]) );
acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 276 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 636 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 726 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 822 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 924 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1032 ]) );
-acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1146 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 363 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 411 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
+acado_setBlockH11( 1, 1, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 573 ]) );
acado_zeroBlockH11( 1, 2 );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 642 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 732 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 828 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 930 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1038 ]) );
-acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1152 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QE[ 15 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 24 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 36 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 51 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 69 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 90 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 114 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 141 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 171 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 204 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 240 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 279 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 321 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 366 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 414 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 465 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 519 ]) );
+acado_setBlockH11( 1, 2, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 576 ]) );
acado_zeroBlockH11( 1, 3 );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 648 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 738 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 834 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 936 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1044 ]) );
-acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1158 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QE[ 27 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 39 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 54 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 72 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 93 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 117 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 144 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 174 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 207 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 243 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 282 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 324 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 369 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 417 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 468 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 522 ]) );
+acado_setBlockH11( 1, 3, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 579 ]) );
acado_zeroBlockH11( 1, 4 );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1164 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 372 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 420 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_setBlockH11( 1, 4, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 582 ]) );
acado_zeroBlockH11( 1, 5 );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 1, 5, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 1, 6 );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 1, 6, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 1, 7 );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 1, 7, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 1, 8 );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 1, 8, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 1, 9 );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 1, 9, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 1, 10 );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 1, 10, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 1, 11 );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 1, 11, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 1, 12 );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 1, 12, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 1, 13 );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 1, 13, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 1, 14 );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 1, 14, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 1, 15 );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 1, 15, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 1, 16 );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 1, 16, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 1, 17 );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 1, 17, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 1, 18 );
-acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 1, 18, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 1, 19 );
-acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 1, 19, &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 2, 2, &(acadoWorkspace.R1[ 2 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QE[ 30 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 48 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 138 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 408 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 642 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 732 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 828 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 930 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1038 ]) );
-acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1152 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QE[ 15 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 24 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 36 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 51 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 69 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 90 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 141 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 171 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 279 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 321 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 465 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 519 ]) );
+acado_setBlockH11( 2, 2, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
acado_zeroBlockH11( 2, 3 );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 108 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 648 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 738 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 834 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 936 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1044 ]) );
-acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1158 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QE[ 27 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 39 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 54 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 72 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 93 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 117 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 144 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 174 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 207 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 243 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 282 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 324 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 369 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 417 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 468 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 522 ]) );
+acado_setBlockH11( 2, 3, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 579 ]) );
acado_zeroBlockH11( 2, 4 );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1164 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 372 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 420 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_setBlockH11( 2, 4, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 582 ]) );
acado_zeroBlockH11( 2, 5 );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 2, 5, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 2, 6 );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 2, 6, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 2, 7 );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 2, 7, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 2, 8 );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 2, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 2, 9 );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 2, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 2, 10 );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 2, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 2, 11 );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 2, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 2, 12 );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 2, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 2, 13 );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 2, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 2, 14 );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 2, 14, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 2, 15 );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 2, 15, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 2, 16 );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 2, 16, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 2, 17 );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 2, 17, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 2, 18 );
-acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 2, 18, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 2, 19 );
-acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 2, 19, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 3, 3, &(acadoWorkspace.R1[ 3 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QE[ 27 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 39 ]) );
acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 54 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 108 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 72 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 93 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 117 ]) );
acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 144 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 234 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 414 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 648 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 738 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 834 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 936 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1044 ]) );
-acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1158 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 174 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 207 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 243 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 282 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 369 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 417 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 468 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
+acado_setBlockH11( 3, 3, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 579 ]) );
acado_zeroBlockH11( 3, 4 );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 354 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1164 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 372 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 420 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_setBlockH11( 3, 4, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 582 ]) );
acado_zeroBlockH11( 3, 5 );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 3, 5, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 3, 6 );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 3, 6, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 3, 7 );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 3, 7, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 3, 8 );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 3, 8, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 3, 9 );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 3, 9, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 3, 10 );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 3, 10, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 3, 11 );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 3, 11, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 3, 12 );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 3, 12, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 3, 13 );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 3, 13, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 3, 14 );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 3, 14, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 3, 15 );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 3, 15, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 3, 16 );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 3, 16, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 3, 17 );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 3, 17, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 3, 18 );
-acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 3, 18, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 3, 19 );
-acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 3, 19, &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 4, 4, &(acadoWorkspace.R1[ 4 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QE[ 84 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 114 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 240 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QE[ 42 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 57 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 75 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 96 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 147 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 177 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 285 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 327 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 420 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 570 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 654 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 744 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 840 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 942 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1050 ]) );
-acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1164 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 471 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 525 ]) );
+acado_setBlockH11( 4, 4, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
acado_zeroBlockH11( 4, 5 );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 4, 5, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 4, 6 );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 4, 6, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 4, 7 );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 4, 7, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 4, 8 );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 4, 8, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 4, 9 );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 4, 9, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 4, 10 );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 4, 10, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 4, 11 );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 4, 11, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 4, 12 );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 4, 12, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 4, 13 );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 4, 13, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 4, 14 );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 4, 14, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 4, 15 );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 4, 15, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 4, 16 );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 4, 16, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 4, 17 );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 4, 17, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 4, 18 );
-acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 4, 18, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 4, 19 );
-acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 4, 19, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 5, 5, &(acadoWorkspace.R1[ 5 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QE[ 120 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 198 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 246 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 360 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 576 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 660 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 750 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 846 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 948 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1056 ]) );
-acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1170 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QE[ 60 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 78 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 99 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 123 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 150 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 180 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 213 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 249 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 288 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 330 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 375 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 423 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 474 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
+acado_setBlockH11( 5, 5, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 585 ]) );
acado_zeroBlockH11( 5, 6 );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 204 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 216 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 252 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 5, 6, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 5, 7 );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 5, 7, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 5, 8 );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 5, 8, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 5, 9 );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 5, 9, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 5, 10 );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 5, 10, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 5, 11 );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 5, 11, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 5, 12 );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 5, 12, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 5, 13 );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 5, 13, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 5, 14 );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 5, 14, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 5, 15 );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 5, 15, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 5, 16 );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 5, 16, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 5, 17 );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 5, 17, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 5, 18 );
-acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 5, 18, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 5, 19 );
-acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 5, 19, &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 6, 6, &(acadoWorkspace.R1[ 6 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 204 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QE[ 81 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 102 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 126 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 153 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 183 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 216 ]) );
acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 252 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 366 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 582 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 666 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 756 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 852 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 954 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1062 ]) );
-acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1176 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 291 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 333 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 426 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 477 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 531 ]) );
+acado_setBlockH11( 6, 6, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
acado_zeroBlockH11( 6, 7 );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 6, 7, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 6, 8 );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 6, 8, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 6, 9 );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 6, 9, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 6, 10 );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 6, 10, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 6, 11 );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 6, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 6, 12 );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 6, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 6, 13 );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 6, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 6, 14 );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 6, 14, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 6, 15 );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 6, 15, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 6, 16 );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 6, 16, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 6, 17 );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 6, 17, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 6, 18 );
-acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 6, 18, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 6, 19 );
-acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 6, 19, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 7, 7, &(acadoWorkspace.R1[ 7 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QE[ 210 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 372 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 588 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 672 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 762 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 858 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 960 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1068 ]) );
-acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1182 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QE[ 105 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 129 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 156 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 186 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 219 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 255 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 294 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 336 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 381 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 429 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 480 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
+acado_setBlockH11( 7, 7, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 591 ]) );
acado_zeroBlockH11( 7, 8 );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 516 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1188 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 537 ]) );
+acado_setBlockH11( 7, 8, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 594 ]) );
acado_zeroBlockH11( 7, 9 );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 7, 9, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 7, 10 );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 7, 10, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 7, 11 );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 7, 11, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 7, 12 );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 7, 12, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 7, 13 );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 7, 13, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 7, 14 );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 7, 14, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 7, 15 );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 7, 15, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 7, 16 );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 7, 16, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 7, 17 );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 7, 17, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 7, 18 );
-acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 7, 18, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 7, 19 );
-acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 7, 19, &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 8, 8, &(acadoWorkspace.R1[ 8 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 318 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 378 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 516 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QE[ 132 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 159 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 189 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 222 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 258 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 297 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 339 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 432 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 483 ]) );
+acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 537 ]) );
acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 594 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 678 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 768 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 864 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 966 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1074 ]) );
-acado_setBlockH11( 8, 8, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1188 ]) );
acado_zeroBlockH11( 8, 9 );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 8, 9, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 8, 10 );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 8, 10, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 8, 11 );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 8, 11, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 8, 12 );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 8, 12, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 8, 13 );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 8, 13, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 8, 14 );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 8, 14, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 8, 15 );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 8, 15, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 8, 16 );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 8, 16, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 8, 17 );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 8, 17, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 8, 18 );
-acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 8, 18, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 8, 19 );
-acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 8, 19, &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 9, 9, &(acadoWorkspace.R1[ 9 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QE[ 324 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 384 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 522 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 684 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 774 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 870 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 972 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1080 ]) );
-acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1194 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QE[ 162 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 192 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 225 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 261 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 300 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 342 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 387 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 435 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 486 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
+acado_setBlockH11( 9, 9, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 597 ]) );
acado_zeroBlockH11( 9, 10 );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 345 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 390 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 9, 10, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 9, 11 );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 9, 11, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 9, 12 );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 9, 12, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 9, 13 );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 9, 13, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 9, 14 );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 9, 14, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 9, 15 );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 9, 15, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 9, 16 );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 9, 16, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 9, 17 );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 9, 17, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 9, 18 );
-acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 9, 18, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 9, 19 );
-acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 9, 19, &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 10, 10, &(acadoWorkspace.R1[ 10 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QE[ 195 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 228 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 264 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 303 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 345 ]) );
acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 390 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 528 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 690 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 780 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 876 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 978 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1086 ]) );
-acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1200 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 438 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 489 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 543 ]) );
+acado_setBlockH11( 10, 10, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 600 ]) );
acado_zeroBlockH11( 10, 11 );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 10, 11, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 10, 12 );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 10, 12, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 10, 13 );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 10, 13, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 10, 14 );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 10, 14, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 10, 15 );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 10, 15, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 10, 16 );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 10, 16, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 10, 17 );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 10, 17, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 10, 18 );
-acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 10, 18, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 10, 19 );
-acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 10, 19, &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 11, 11, &(acadoWorkspace.R1[ 11 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QE[ 462 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 534 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 696 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 786 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 882 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 984 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1092 ]) );
-acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1206 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QE[ 231 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 267 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 306 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 348 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 393 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 441 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 492 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 546 ]) );
+acado_setBlockH11( 11, 11, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 603 ]) );
acado_zeroBlockH11( 11, 12 );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 11, 12, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 11, 13 );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 11, 13, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 11, 14 );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 11, 14, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 11, 15 );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 11, 15, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 11, 16 );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 11, 16, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 11, 17 );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 11, 17, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 11, 18 );
-acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 11, 18, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 11, 19 );
-acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 11, 19, &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 12, 12, &(acadoWorkspace.R1[ 12 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QE[ 540 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 702 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 792 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 888 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 990 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1098 ]) );
-acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1212 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QE[ 270 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 309 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 351 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 396 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 444 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 495 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 549 ]) );
+acado_setBlockH11( 12, 12, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 606 ]) );
acado_zeroBlockH11( 12, 13 );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 12, 13, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 12, 14 );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 12, 14, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 12, 15 );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 12, 15, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 12, 16 );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 12, 16, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 12, 17 );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 12, 17, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 12, 18 );
-acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 12, 18, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 12, 19 );
-acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 12, 19, &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 13, 13, &(acadoWorkspace.R1[ 13 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 708 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 798 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 894 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 996 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1104 ]) );
-acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1218 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QE[ 312 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 354 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 399 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 447 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 498 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 552 ]) );
+acado_setBlockH11( 13, 13, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 609 ]) );
acado_zeroBlockH11( 13, 14 );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 13, 14, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 13, 15 );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 13, 15, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 13, 16 );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 13, 16, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 13, 17 );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 13, 17, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 13, 18 );
-acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 13, 18, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 13, 19 );
-acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 13, 19, &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 14, 14, &(acadoWorkspace.R1[ 14 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QE[ 714 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 804 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 900 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1002 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1110 ]) );
-acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1224 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QE[ 357 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 402 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 450 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 501 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 555 ]) );
+acado_setBlockH11( 14, 14, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 612 ]) );
acado_zeroBlockH11( 14, 15 );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 14, 15, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 14, 16 );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 14, 16, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 14, 17 );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 14, 17, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 14, 18 );
-acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 14, 18, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 14, 19 );
-acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 14, 19, &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 15, 15, &(acadoWorkspace.R1[ 15 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QE[ 810 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 906 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1008 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1116 ]) );
-acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1230 ]) );
+acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QE[ 405 ]) );
+acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 453 ]) );
+acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 504 ]) );
+acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 558 ]) );
+acado_setBlockH11( 15, 15, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 615 ]) );
acado_zeroBlockH11( 15, 16 );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 15, 16, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 15, 17 );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 15, 17, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 15, 18 );
-acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 15, 18, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 15, 19 );
-acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 15, 19, &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 16, 16, &(acadoWorkspace.R1[ 16 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QE[ 912 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1014 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1122 ]) );
-acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1236 ]) );
+acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QE[ 456 ]) );
+acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 507 ]) );
+acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 561 ]) );
+acado_setBlockH11( 16, 16, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 618 ]) );
acado_zeroBlockH11( 16, 17 );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 16, 17, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 16, 18 );
-acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 16, 18, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 16, 19 );
-acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 16, 19, &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 17, 17, &(acadoWorkspace.R1[ 17 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QE[ 1020 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1128 ]) );
-acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1242 ]) );
+acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QE[ 510 ]) );
+acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 564 ]) );
+acado_setBlockH11( 17, 17, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 621 ]) );
acado_zeroBlockH11( 17, 18 );
-acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 17, 18, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 17, 19 );
-acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 17, 19, &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 18, 18, &(acadoWorkspace.R1[ 18 ]) );
-acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QE[ 1134 ]) );
-acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1248 ]) );
+acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QE[ 567 ]) );
+acado_setBlockH11( 18, 18, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 624 ]) );
acado_zeroBlockH11( 18, 19 );
-acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 18, 19, &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_setBlockH11_R1( 19, 19, &(acadoWorkspace.R1[ 19 ]) );
-acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QE[ 1254 ]) );
+acado_setBlockH11( 19, 19, &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QE[ 627 ]) );
acado_copyHTH( 1, 0 );
@@ -3804,433 +3439,370 @@ acado_copyHTH( 19, 16 );
acado_copyHTH( 19, 17 );
acado_copyHTH( 19, 18 );
-acadoWorkspace.H[156] = acadoWorkspace.H10[0];
-acadoWorkspace.H[157] = acadoWorkspace.H10[1];
-acadoWorkspace.H[158] = acadoWorkspace.H10[2];
-acadoWorkspace.H[159] = acadoWorkspace.H10[3];
-acadoWorkspace.H[160] = acadoWorkspace.H10[4];
-acadoWorkspace.H[161] = acadoWorkspace.H10[5];
-acadoWorkspace.H[182] = acadoWorkspace.H10[6];
-acadoWorkspace.H[183] = acadoWorkspace.H10[7];
-acadoWorkspace.H[184] = acadoWorkspace.H10[8];
-acadoWorkspace.H[185] = acadoWorkspace.H10[9];
-acadoWorkspace.H[186] = acadoWorkspace.H10[10];
-acadoWorkspace.H[187] = acadoWorkspace.H10[11];
-acadoWorkspace.H[208] = acadoWorkspace.H10[12];
-acadoWorkspace.H[209] = acadoWorkspace.H10[13];
-acadoWorkspace.H[210] = acadoWorkspace.H10[14];
-acadoWorkspace.H[211] = acadoWorkspace.H10[15];
-acadoWorkspace.H[212] = acadoWorkspace.H10[16];
-acadoWorkspace.H[213] = acadoWorkspace.H10[17];
-acadoWorkspace.H[234] = acadoWorkspace.H10[18];
-acadoWorkspace.H[235] = acadoWorkspace.H10[19];
-acadoWorkspace.H[236] = acadoWorkspace.H10[20];
-acadoWorkspace.H[237] = acadoWorkspace.H10[21];
-acadoWorkspace.H[238] = acadoWorkspace.H10[22];
-acadoWorkspace.H[239] = acadoWorkspace.H10[23];
-acadoWorkspace.H[260] = acadoWorkspace.H10[24];
-acadoWorkspace.H[261] = acadoWorkspace.H10[25];
-acadoWorkspace.H[262] = acadoWorkspace.H10[26];
-acadoWorkspace.H[263] = acadoWorkspace.H10[27];
-acadoWorkspace.H[264] = acadoWorkspace.H10[28];
-acadoWorkspace.H[265] = acadoWorkspace.H10[29];
-acadoWorkspace.H[286] = acadoWorkspace.H10[30];
-acadoWorkspace.H[287] = acadoWorkspace.H10[31];
-acadoWorkspace.H[288] = acadoWorkspace.H10[32];
-acadoWorkspace.H[289] = acadoWorkspace.H10[33];
-acadoWorkspace.H[290] = acadoWorkspace.H10[34];
-acadoWorkspace.H[291] = acadoWorkspace.H10[35];
-acadoWorkspace.H[312] = acadoWorkspace.H10[36];
-acadoWorkspace.H[313] = acadoWorkspace.H10[37];
-acadoWorkspace.H[314] = acadoWorkspace.H10[38];
-acadoWorkspace.H[315] = acadoWorkspace.H10[39];
-acadoWorkspace.H[316] = acadoWorkspace.H10[40];
-acadoWorkspace.H[317] = acadoWorkspace.H10[41];
-acadoWorkspace.H[338] = acadoWorkspace.H10[42];
-acadoWorkspace.H[339] = acadoWorkspace.H10[43];
-acadoWorkspace.H[340] = acadoWorkspace.H10[44];
-acadoWorkspace.H[341] = acadoWorkspace.H10[45];
-acadoWorkspace.H[342] = acadoWorkspace.H10[46];
-acadoWorkspace.H[343] = acadoWorkspace.H10[47];
-acadoWorkspace.H[364] = acadoWorkspace.H10[48];
-acadoWorkspace.H[365] = acadoWorkspace.H10[49];
-acadoWorkspace.H[366] = acadoWorkspace.H10[50];
-acadoWorkspace.H[367] = acadoWorkspace.H10[51];
-acadoWorkspace.H[368] = acadoWorkspace.H10[52];
-acadoWorkspace.H[369] = acadoWorkspace.H10[53];
-acadoWorkspace.H[390] = acadoWorkspace.H10[54];
-acadoWorkspace.H[391] = acadoWorkspace.H10[55];
-acadoWorkspace.H[392] = acadoWorkspace.H10[56];
-acadoWorkspace.H[393] = acadoWorkspace.H10[57];
-acadoWorkspace.H[394] = acadoWorkspace.H10[58];
-acadoWorkspace.H[395] = acadoWorkspace.H10[59];
-acadoWorkspace.H[416] = acadoWorkspace.H10[60];
-acadoWorkspace.H[417] = acadoWorkspace.H10[61];
-acadoWorkspace.H[418] = acadoWorkspace.H10[62];
-acadoWorkspace.H[419] = acadoWorkspace.H10[63];
-acadoWorkspace.H[420] = acadoWorkspace.H10[64];
-acadoWorkspace.H[421] = acadoWorkspace.H10[65];
-acadoWorkspace.H[442] = acadoWorkspace.H10[66];
-acadoWorkspace.H[443] = acadoWorkspace.H10[67];
-acadoWorkspace.H[444] = acadoWorkspace.H10[68];
-acadoWorkspace.H[445] = acadoWorkspace.H10[69];
-acadoWorkspace.H[446] = acadoWorkspace.H10[70];
-acadoWorkspace.H[447] = acadoWorkspace.H10[71];
-acadoWorkspace.H[468] = acadoWorkspace.H10[72];
-acadoWorkspace.H[469] = acadoWorkspace.H10[73];
-acadoWorkspace.H[470] = acadoWorkspace.H10[74];
-acadoWorkspace.H[471] = acadoWorkspace.H10[75];
-acadoWorkspace.H[472] = acadoWorkspace.H10[76];
-acadoWorkspace.H[473] = acadoWorkspace.H10[77];
-acadoWorkspace.H[494] = acadoWorkspace.H10[78];
-acadoWorkspace.H[495] = acadoWorkspace.H10[79];
-acadoWorkspace.H[496] = acadoWorkspace.H10[80];
-acadoWorkspace.H[497] = acadoWorkspace.H10[81];
-acadoWorkspace.H[498] = acadoWorkspace.H10[82];
-acadoWorkspace.H[499] = acadoWorkspace.H10[83];
-acadoWorkspace.H[520] = acadoWorkspace.H10[84];
-acadoWorkspace.H[521] = acadoWorkspace.H10[85];
-acadoWorkspace.H[522] = acadoWorkspace.H10[86];
-acadoWorkspace.H[523] = acadoWorkspace.H10[87];
-acadoWorkspace.H[524] = acadoWorkspace.H10[88];
-acadoWorkspace.H[525] = acadoWorkspace.H10[89];
-acadoWorkspace.H[546] = acadoWorkspace.H10[90];
-acadoWorkspace.H[547] = acadoWorkspace.H10[91];
-acadoWorkspace.H[548] = acadoWorkspace.H10[92];
-acadoWorkspace.H[549] = acadoWorkspace.H10[93];
-acadoWorkspace.H[550] = acadoWorkspace.H10[94];
-acadoWorkspace.H[551] = acadoWorkspace.H10[95];
-acadoWorkspace.H[572] = acadoWorkspace.H10[96];
-acadoWorkspace.H[573] = acadoWorkspace.H10[97];
-acadoWorkspace.H[574] = acadoWorkspace.H10[98];
-acadoWorkspace.H[575] = acadoWorkspace.H10[99];
-acadoWorkspace.H[576] = acadoWorkspace.H10[100];
-acadoWorkspace.H[577] = acadoWorkspace.H10[101];
-acadoWorkspace.H[598] = acadoWorkspace.H10[102];
-acadoWorkspace.H[599] = acadoWorkspace.H10[103];
-acadoWorkspace.H[600] = acadoWorkspace.H10[104];
-acadoWorkspace.H[601] = acadoWorkspace.H10[105];
-acadoWorkspace.H[602] = acadoWorkspace.H10[106];
-acadoWorkspace.H[603] = acadoWorkspace.H10[107];
-acadoWorkspace.H[624] = acadoWorkspace.H10[108];
-acadoWorkspace.H[625] = acadoWorkspace.H10[109];
-acadoWorkspace.H[626] = acadoWorkspace.H10[110];
-acadoWorkspace.H[627] = acadoWorkspace.H10[111];
-acadoWorkspace.H[628] = acadoWorkspace.H10[112];
-acadoWorkspace.H[629] = acadoWorkspace.H10[113];
-acadoWorkspace.H[650] = acadoWorkspace.H10[114];
-acadoWorkspace.H[651] = acadoWorkspace.H10[115];
-acadoWorkspace.H[652] = acadoWorkspace.H10[116];
-acadoWorkspace.H[653] = acadoWorkspace.H10[117];
-acadoWorkspace.H[654] = acadoWorkspace.H10[118];
-acadoWorkspace.H[655] = acadoWorkspace.H10[119];
-
-acado_multQ1d( &(acadoWorkspace.Q1[ 36 ]), acadoWorkspace.d, acadoWorkspace.Qd );
-acado_multQ1d( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.Qd[ 6 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.Qd[ 18 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 180 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 216 ]), &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.Qd[ 30 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 252 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 288 ]), &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.Qd[ 42 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 324 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 360 ]), &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.Qd[ 54 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 396 ]), &(acadoWorkspace.d[ 60 ]), &(acadoWorkspace.Qd[ 60 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 432 ]), &(acadoWorkspace.d[ 66 ]), &(acadoWorkspace.Qd[ 66 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 468 ]), &(acadoWorkspace.d[ 72 ]), &(acadoWorkspace.Qd[ 72 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 504 ]), &(acadoWorkspace.d[ 78 ]), &(acadoWorkspace.Qd[ 78 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 540 ]), &(acadoWorkspace.d[ 84 ]), &(acadoWorkspace.Qd[ 84 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 576 ]), &(acadoWorkspace.d[ 90 ]), &(acadoWorkspace.Qd[ 90 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 612 ]), &(acadoWorkspace.d[ 96 ]), &(acadoWorkspace.Qd[ 96 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 648 ]), &(acadoWorkspace.d[ 102 ]), &(acadoWorkspace.Qd[ 102 ]) );
-acado_multQ1d( &(acadoWorkspace.Q1[ 684 ]), &(acadoWorkspace.d[ 108 ]), &(acadoWorkspace.Qd[ 108 ]) );
-acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 114 ]), &(acadoWorkspace.Qd[ 114 ]) );
+acadoWorkspace.H[69] = acadoWorkspace.H10[0];
+acadoWorkspace.H[70] = acadoWorkspace.H10[1];
+acadoWorkspace.H[71] = acadoWorkspace.H10[2];
+acadoWorkspace.H[92] = acadoWorkspace.H10[3];
+acadoWorkspace.H[93] = acadoWorkspace.H10[4];
+acadoWorkspace.H[94] = acadoWorkspace.H10[5];
+acadoWorkspace.H[115] = acadoWorkspace.H10[6];
+acadoWorkspace.H[116] = acadoWorkspace.H10[7];
+acadoWorkspace.H[117] = acadoWorkspace.H10[8];
+acadoWorkspace.H[138] = acadoWorkspace.H10[9];
+acadoWorkspace.H[139] = acadoWorkspace.H10[10];
+acadoWorkspace.H[140] = acadoWorkspace.H10[11];
+acadoWorkspace.H[161] = acadoWorkspace.H10[12];
+acadoWorkspace.H[162] = acadoWorkspace.H10[13];
+acadoWorkspace.H[163] = acadoWorkspace.H10[14];
+acadoWorkspace.H[184] = acadoWorkspace.H10[15];
+acadoWorkspace.H[185] = acadoWorkspace.H10[16];
+acadoWorkspace.H[186] = acadoWorkspace.H10[17];
+acadoWorkspace.H[207] = acadoWorkspace.H10[18];
+acadoWorkspace.H[208] = acadoWorkspace.H10[19];
+acadoWorkspace.H[209] = acadoWorkspace.H10[20];
+acadoWorkspace.H[230] = acadoWorkspace.H10[21];
+acadoWorkspace.H[231] = acadoWorkspace.H10[22];
+acadoWorkspace.H[232] = acadoWorkspace.H10[23];
+acadoWorkspace.H[253] = acadoWorkspace.H10[24];
+acadoWorkspace.H[254] = acadoWorkspace.H10[25];
+acadoWorkspace.H[255] = acadoWorkspace.H10[26];
+acadoWorkspace.H[276] = acadoWorkspace.H10[27];
+acadoWorkspace.H[277] = acadoWorkspace.H10[28];
+acadoWorkspace.H[278] = acadoWorkspace.H10[29];
+acadoWorkspace.H[299] = acadoWorkspace.H10[30];
+acadoWorkspace.H[300] = acadoWorkspace.H10[31];
+acadoWorkspace.H[301] = acadoWorkspace.H10[32];
+acadoWorkspace.H[322] = acadoWorkspace.H10[33];
+acadoWorkspace.H[323] = acadoWorkspace.H10[34];
+acadoWorkspace.H[324] = acadoWorkspace.H10[35];
+acadoWorkspace.H[345] = acadoWorkspace.H10[36];
+acadoWorkspace.H[346] = acadoWorkspace.H10[37];
+acadoWorkspace.H[347] = acadoWorkspace.H10[38];
+acadoWorkspace.H[368] = acadoWorkspace.H10[39];
+acadoWorkspace.H[369] = acadoWorkspace.H10[40];
+acadoWorkspace.H[370] = acadoWorkspace.H10[41];
+acadoWorkspace.H[391] = acadoWorkspace.H10[42];
+acadoWorkspace.H[392] = acadoWorkspace.H10[43];
+acadoWorkspace.H[393] = acadoWorkspace.H10[44];
+acadoWorkspace.H[414] = acadoWorkspace.H10[45];
+acadoWorkspace.H[415] = acadoWorkspace.H10[46];
+acadoWorkspace.H[416] = acadoWorkspace.H10[47];
+acadoWorkspace.H[437] = acadoWorkspace.H10[48];
+acadoWorkspace.H[438] = acadoWorkspace.H10[49];
+acadoWorkspace.H[439] = acadoWorkspace.H10[50];
+acadoWorkspace.H[460] = acadoWorkspace.H10[51];
+acadoWorkspace.H[461] = acadoWorkspace.H10[52];
+acadoWorkspace.H[462] = acadoWorkspace.H10[53];
+acadoWorkspace.H[483] = acadoWorkspace.H10[54];
+acadoWorkspace.H[484] = acadoWorkspace.H10[55];
+acadoWorkspace.H[485] = acadoWorkspace.H10[56];
+acadoWorkspace.H[506] = acadoWorkspace.H10[57];
+acadoWorkspace.H[507] = acadoWorkspace.H10[58];
+acadoWorkspace.H[508] = acadoWorkspace.H10[59];
+
+acado_multQ1d( &(acadoWorkspace.Q1[ 9 ]), acadoWorkspace.d, acadoWorkspace.Qd );
+acado_multQ1d( &(acadoWorkspace.Q1[ 18 ]), &(acadoWorkspace.d[ 3 ]), &(acadoWorkspace.Qd[ 3 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 27 ]), &(acadoWorkspace.d[ 6 ]), &(acadoWorkspace.Qd[ 6 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 36 ]), &(acadoWorkspace.d[ 9 ]), &(acadoWorkspace.Qd[ 9 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 45 ]), &(acadoWorkspace.d[ 12 ]), &(acadoWorkspace.Qd[ 12 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 54 ]), &(acadoWorkspace.d[ 15 ]), &(acadoWorkspace.Qd[ 15 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 63 ]), &(acadoWorkspace.d[ 18 ]), &(acadoWorkspace.Qd[ 18 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 72 ]), &(acadoWorkspace.d[ 21 ]), &(acadoWorkspace.Qd[ 21 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 81 ]), &(acadoWorkspace.d[ 24 ]), &(acadoWorkspace.Qd[ 24 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 90 ]), &(acadoWorkspace.d[ 27 ]), &(acadoWorkspace.Qd[ 27 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 99 ]), &(acadoWorkspace.d[ 30 ]), &(acadoWorkspace.Qd[ 30 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 108 ]), &(acadoWorkspace.d[ 33 ]), &(acadoWorkspace.Qd[ 33 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 117 ]), &(acadoWorkspace.d[ 36 ]), &(acadoWorkspace.Qd[ 36 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 126 ]), &(acadoWorkspace.d[ 39 ]), &(acadoWorkspace.Qd[ 39 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 135 ]), &(acadoWorkspace.d[ 42 ]), &(acadoWorkspace.Qd[ 42 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 144 ]), &(acadoWorkspace.d[ 45 ]), &(acadoWorkspace.Qd[ 45 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 153 ]), &(acadoWorkspace.d[ 48 ]), &(acadoWorkspace.Qd[ 48 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 162 ]), &(acadoWorkspace.d[ 51 ]), &(acadoWorkspace.Qd[ 51 ]) );
+acado_multQ1d( &(acadoWorkspace.Q1[ 171 ]), &(acadoWorkspace.d[ 54 ]), &(acadoWorkspace.Qd[ 54 ]) );
+acado_multQN1d( acadoWorkspace.QN1, &(acadoWorkspace.d[ 57 ]), &(acadoWorkspace.Qd[ 57 ]) );
acado_macCTSlx( acadoWorkspace.evGx, acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 9 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 18 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 27 ]), acadoWorkspace.g );
acado_macCTSlx( &(acadoWorkspace.evGx[ 36 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 45 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 54 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 63 ]), acadoWorkspace.g );
acado_macCTSlx( &(acadoWorkspace.evGx[ 72 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 81 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 90 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 99 ]), acadoWorkspace.g );
acado_macCTSlx( &(acadoWorkspace.evGx[ 108 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 117 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 126 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 135 ]), acadoWorkspace.g );
acado_macCTSlx( &(acadoWorkspace.evGx[ 144 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 180 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 216 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 252 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 288 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 324 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 360 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 396 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 432 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 468 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 504 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 540 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 576 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 612 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 648 ]), acadoWorkspace.g );
-acado_macCTSlx( &(acadoWorkspace.evGx[ 684 ]), acadoWorkspace.g );
-acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 153 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 162 ]), acadoWorkspace.g );
+acado_macCTSlx( &(acadoWorkspace.evGx[ 171 ]), acadoWorkspace.g );
+acado_macETSlu( acadoWorkspace.QE, &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 3 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 9 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 18 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 45 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 63 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 135 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 165 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 273 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 315 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 459 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 513 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 6 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 21 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 33 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 87 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 111 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 168 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 201 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 237 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 363 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 411 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 573 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 15 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 36 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 51 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 69 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 90 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 141 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 171 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 279 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 321 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 465 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 519 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 27 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 39 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 93 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 117 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 207 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 243 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 369 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 417 ]), &(acadoWorkspace.g[ 6 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 468 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 630 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 720 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 816 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 918 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1026 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1140 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 12 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 24 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 579 ]), &(acadoWorkspace.g[ 6 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 42 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 66 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 57 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 75 ]), &(acadoWorkspace.g[ 7 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 96 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 174 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 276 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 636 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 726 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 822 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 924 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1032 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1146 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 30 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 48 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 72 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 138 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 147 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 177 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 285 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 327 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 471 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 525 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 99 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 123 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.g[ 8 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 180 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 282 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 408 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 642 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 732 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 828 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 930 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1038 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1152 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 54 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 78 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 108 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 144 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 234 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 414 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 648 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 738 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 834 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 936 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1044 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1158 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 84 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 114 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 150 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 240 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 213 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 249 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 288 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 330 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 375 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 423 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 474 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 585 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 81 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 102 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 126 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 153 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 183 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 216 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 291 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 333 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 477 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 531 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 105 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 129 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 186 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 219 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 255 ]), &(acadoWorkspace.g[ 10 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 294 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 420 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 570 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 654 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 744 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 840 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 942 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1050 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1164 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 120 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 156 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 198 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 246 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 360 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 426 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 576 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 660 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 750 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 846 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 948 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1056 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1170 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 336 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 381 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 429 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 480 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 591 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 132 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 159 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 189 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 222 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 297 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 339 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 483 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 537 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.g[ 11 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 162 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 204 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 252 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 366 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 432 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 582 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 666 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 756 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 852 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 954 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1062 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1176 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 210 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 258 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 372 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 192 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 225 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 261 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 300 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 342 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 387 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 435 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 486 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 597 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 195 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 228 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 303 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 345 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.g[ 13 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 438 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 588 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 672 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 762 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 858 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 960 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1068 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1182 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 264 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 318 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 378 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 516 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 594 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 678 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 768 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 864 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 966 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1074 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1188 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 324 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 384 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 522 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 684 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 774 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 870 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 972 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1080 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1194 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 390 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 528 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 690 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 780 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 876 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 978 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1086 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1200 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 462 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 534 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 489 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 543 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 600 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 231 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 267 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 306 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 348 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 393 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 441 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 492 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 546 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 603 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 270 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 309 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 351 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 396 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 444 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 495 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 549 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 606 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 312 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 354 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 399 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 447 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 498 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 552 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 609 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 357 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 402 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 450 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 501 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 555 ]), &(acadoWorkspace.g[ 17 ]) );
acado_macETSlu( &(acadoWorkspace.QE[ 612 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 696 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 786 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 882 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 984 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1092 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1206 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 540 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 702 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 792 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 888 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 990 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1098 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1212 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 708 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 798 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 894 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 996 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1104 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1218 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 714 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 804 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 900 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1002 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1110 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1224 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 810 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 906 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1008 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1116 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1230 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 912 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1014 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1122 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1236 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1020 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1128 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1242 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1134 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1248 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_macETSlu( &(acadoWorkspace.QE[ 1254 ]), &(acadoWorkspace.g[ 25 ]) );
-acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.lb[23] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.lb[24] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.lb[25] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
-acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
-acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
-acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
-acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
-acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
-acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
-acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
-acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
-acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
-acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
-acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
-acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
-acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
-acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
-acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
-acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
-acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
-acadoWorkspace.ub[23] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
-acadoWorkspace.ub[24] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
-acadoWorkspace.ub[25] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
+acado_macETSlu( &(acadoWorkspace.QE[ 405 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 453 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 504 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 558 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 615 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 456 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 507 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 561 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 618 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 510 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 564 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 621 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 567 ]), &(acadoWorkspace.g[ 21 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 624 ]), &(acadoWorkspace.g[ 21 ]) );
+acado_macETSlu( &(acadoWorkspace.QE[ 627 ]), &(acadoWorkspace.g[ 22 ]) );
+acadoWorkspace.lb[3] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[0];
+acadoWorkspace.lb[4] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[1];
+acadoWorkspace.lb[5] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[2];
+acadoWorkspace.lb[6] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[3];
+acadoWorkspace.lb[7] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[4];
+acadoWorkspace.lb[8] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[5];
+acadoWorkspace.lb[9] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[6];
+acadoWorkspace.lb[10] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[7];
+acadoWorkspace.lb[11] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[8];
+acadoWorkspace.lb[12] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[9];
+acadoWorkspace.lb[13] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[10];
+acadoWorkspace.lb[14] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[11];
+acadoWorkspace.lb[15] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[12];
+acadoWorkspace.lb[16] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[13];
+acadoWorkspace.lb[17] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[14];
+acadoWorkspace.lb[18] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[15];
+acadoWorkspace.lb[19] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[16];
+acadoWorkspace.lb[20] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[17];
+acadoWorkspace.lb[21] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[18];
+acadoWorkspace.lb[22] = (real_t)-1.0000000000000000e+12 - acadoVariables.u[19];
+acadoWorkspace.ub[3] = (real_t)1.0000000000000000e+12 - acadoVariables.u[0];
+acadoWorkspace.ub[4] = (real_t)1.0000000000000000e+12 - acadoVariables.u[1];
+acadoWorkspace.ub[5] = (real_t)1.0000000000000000e+12 - acadoVariables.u[2];
+acadoWorkspace.ub[6] = (real_t)1.0000000000000000e+12 - acadoVariables.u[3];
+acadoWorkspace.ub[7] = (real_t)1.0000000000000000e+12 - acadoVariables.u[4];
+acadoWorkspace.ub[8] = (real_t)1.0000000000000000e+12 - acadoVariables.u[5];
+acadoWorkspace.ub[9] = (real_t)1.0000000000000000e+12 - acadoVariables.u[6];
+acadoWorkspace.ub[10] = (real_t)1.0000000000000000e+12 - acadoVariables.u[7];
+acadoWorkspace.ub[11] = (real_t)1.0000000000000000e+12 - acadoVariables.u[8];
+acadoWorkspace.ub[12] = (real_t)1.0000000000000000e+12 - acadoVariables.u[9];
+acadoWorkspace.ub[13] = (real_t)1.0000000000000000e+12 - acadoVariables.u[10];
+acadoWorkspace.ub[14] = (real_t)1.0000000000000000e+12 - acadoVariables.u[11];
+acadoWorkspace.ub[15] = (real_t)1.0000000000000000e+12 - acadoVariables.u[12];
+acadoWorkspace.ub[16] = (real_t)1.0000000000000000e+12 - acadoVariables.u[13];
+acadoWorkspace.ub[17] = (real_t)1.0000000000000000e+12 - acadoVariables.u[14];
+acadoWorkspace.ub[18] = (real_t)1.0000000000000000e+12 - acadoVariables.u[15];
+acadoWorkspace.ub[19] = (real_t)1.0000000000000000e+12 - acadoVariables.u[16];
+acadoWorkspace.ub[20] = (real_t)1.0000000000000000e+12 - acadoVariables.u[17];
+acadoWorkspace.ub[21] = (real_t)1.0000000000000000e+12 - acadoVariables.u[18];
+acadoWorkspace.ub[22] = (real_t)1.0000000000000000e+12 - acadoVariables.u[19];
for (lRun1 = 0; lRun1 < 20; ++lRun1)
{
-lRun3 = xBoundIndices[ lRun1 ] - 6;
-lRun4 = ((lRun3) / (6)) + (1);
-acadoWorkspace.A[lRun1 * 26] = acadoWorkspace.evGx[lRun3 * 6];
-acadoWorkspace.A[lRun1 * 26 + 1] = acadoWorkspace.evGx[lRun3 * 6 + 1];
-acadoWorkspace.A[lRun1 * 26 + 2] = acadoWorkspace.evGx[lRun3 * 6 + 2];
-acadoWorkspace.A[lRun1 * 26 + 3] = acadoWorkspace.evGx[lRun3 * 6 + 3];
-acadoWorkspace.A[lRun1 * 26 + 4] = acadoWorkspace.evGx[lRun3 * 6 + 4];
-acadoWorkspace.A[lRun1 * 26 + 5] = acadoWorkspace.evGx[lRun3 * 6 + 5];
+lRun3 = xBoundIndices[ lRun1 ] - 3;
+lRun4 = ((lRun3) / (3)) + (1);
+acadoWorkspace.A[lRun1 * 23] = acadoWorkspace.evGx[lRun3 * 3];
+acadoWorkspace.A[lRun1 * 23 + 1] = acadoWorkspace.evGx[lRun3 * 3 + 1];
+acadoWorkspace.A[lRun1 * 23 + 2] = acadoWorkspace.evGx[lRun3 * 3 + 2];
for (lRun2 = 0; lRun2 < lRun4; ++lRun2)
{
-lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (6)) + ((lRun3) % (6));
-acadoWorkspace.A[(lRun1 * 26) + (lRun2 + 6)] = acadoWorkspace.E[lRun5];
+lRun5 = (((((lRun4) * (lRun4-1)) / (2)) + (lRun2)) * (3)) + ((lRun3) % (3));
+acadoWorkspace.A[(lRun1 * 23) + (lRun2 + 3)] = acadoWorkspace.E[lRun5];
}
}
@@ -4243,9 +3815,6 @@ real_t tmp;
acadoWorkspace.Dx0[0] = acadoVariables.x0[0] - acadoVariables.x[0];
acadoWorkspace.Dx0[1] = acadoVariables.x0[1] - acadoVariables.x[1];
acadoWorkspace.Dx0[2] = acadoVariables.x0[2] - acadoVariables.x[2];
-acadoWorkspace.Dx0[3] = acadoVariables.x0[3] - acadoVariables.x[3];
-acadoWorkspace.Dx0[4] = acadoVariables.x0[4] - acadoVariables.x[4];
-acadoWorkspace.Dx0[5] = acadoVariables.x0[5] - acadoVariables.x[5];
acadoWorkspace.Dy[0] -= acadoVariables.y[0];
acadoWorkspace.Dy[1] -= acadoVariables.y[1];
@@ -4331,465 +3900,393 @@ acadoWorkspace.DyN[0] -= acadoVariables.yN[0];
acadoWorkspace.DyN[1] -= acadoVariables.yN[1];
acadoWorkspace.DyN[2] -= acadoVariables.yN[2];
-acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 6 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 4 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 8 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 16 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 28 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 32 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 44 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 48 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 52 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 56 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 64 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 68 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 72 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_multRDy( &(acadoWorkspace.R2[ 76 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.g[ 25 ]) );
+acado_multRDy( acadoWorkspace.R2, acadoWorkspace.Dy, &(acadoWorkspace.g[ 3 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 4 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 8 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 12 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 16 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 20 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 24 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 28 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 32 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 36 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 40 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 44 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 48 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 52 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 56 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 60 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 64 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 68 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 72 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.g[ 21 ]) );
+acado_multRDy( &(acadoWorkspace.R2[ 76 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.g[ 22 ]) );
acado_multQDy( acadoWorkspace.Q2, acadoWorkspace.Dy, acadoWorkspace.QDy );
-acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.QDy[ 6 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.QDy[ 12 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 18 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.QDy[ 24 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 30 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 36 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.QDy[ 42 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 192 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.QDy[ 48 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 216 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 54 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 240 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 60 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 264 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.QDy[ 66 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 288 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.QDy[ 72 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 312 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.QDy[ 78 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 336 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.QDy[ 84 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 360 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 90 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 384 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.QDy[ 96 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 408 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.QDy[ 102 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 432 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.QDy[ 108 ]) );
-acado_multQDy( &(acadoWorkspace.Q2[ 456 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.QDy[ 114 ]) );
-
-acadoWorkspace.QDy[120] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[121] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[122] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[123] = + acadoWorkspace.QN2[9]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[10]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[11]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[124] = + acadoWorkspace.QN2[12]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[13]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[14]*acadoWorkspace.DyN[2];
-acadoWorkspace.QDy[125] = + acadoWorkspace.QN2[15]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[16]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[17]*acadoWorkspace.DyN[2];
-
-acadoWorkspace.QDy[6] += acadoWorkspace.Qd[0];
-acadoWorkspace.QDy[7] += acadoWorkspace.Qd[1];
-acadoWorkspace.QDy[8] += acadoWorkspace.Qd[2];
-acadoWorkspace.QDy[9] += acadoWorkspace.Qd[3];
-acadoWorkspace.QDy[10] += acadoWorkspace.Qd[4];
-acadoWorkspace.QDy[11] += acadoWorkspace.Qd[5];
-acadoWorkspace.QDy[12] += acadoWorkspace.Qd[6];
-acadoWorkspace.QDy[13] += acadoWorkspace.Qd[7];
-acadoWorkspace.QDy[14] += acadoWorkspace.Qd[8];
-acadoWorkspace.QDy[15] += acadoWorkspace.Qd[9];
-acadoWorkspace.QDy[16] += acadoWorkspace.Qd[10];
-acadoWorkspace.QDy[17] += acadoWorkspace.Qd[11];
-acadoWorkspace.QDy[18] += acadoWorkspace.Qd[12];
-acadoWorkspace.QDy[19] += acadoWorkspace.Qd[13];
-acadoWorkspace.QDy[20] += acadoWorkspace.Qd[14];
-acadoWorkspace.QDy[21] += acadoWorkspace.Qd[15];
-acadoWorkspace.QDy[22] += acadoWorkspace.Qd[16];
-acadoWorkspace.QDy[23] += acadoWorkspace.Qd[17];
-acadoWorkspace.QDy[24] += acadoWorkspace.Qd[18];
-acadoWorkspace.QDy[25] += acadoWorkspace.Qd[19];
-acadoWorkspace.QDy[26] += acadoWorkspace.Qd[20];
-acadoWorkspace.QDy[27] += acadoWorkspace.Qd[21];
-acadoWorkspace.QDy[28] += acadoWorkspace.Qd[22];
-acadoWorkspace.QDy[29] += acadoWorkspace.Qd[23];
-acadoWorkspace.QDy[30] += acadoWorkspace.Qd[24];
-acadoWorkspace.QDy[31] += acadoWorkspace.Qd[25];
-acadoWorkspace.QDy[32] += acadoWorkspace.Qd[26];
-acadoWorkspace.QDy[33] += acadoWorkspace.Qd[27];
-acadoWorkspace.QDy[34] += acadoWorkspace.Qd[28];
-acadoWorkspace.QDy[35] += acadoWorkspace.Qd[29];
-acadoWorkspace.QDy[36] += acadoWorkspace.Qd[30];
-acadoWorkspace.QDy[37] += acadoWorkspace.Qd[31];
-acadoWorkspace.QDy[38] += acadoWorkspace.Qd[32];
-acadoWorkspace.QDy[39] += acadoWorkspace.Qd[33];
-acadoWorkspace.QDy[40] += acadoWorkspace.Qd[34];
-acadoWorkspace.QDy[41] += acadoWorkspace.Qd[35];
-acadoWorkspace.QDy[42] += acadoWorkspace.Qd[36];
-acadoWorkspace.QDy[43] += acadoWorkspace.Qd[37];
-acadoWorkspace.QDy[44] += acadoWorkspace.Qd[38];
-acadoWorkspace.QDy[45] += acadoWorkspace.Qd[39];
-acadoWorkspace.QDy[46] += acadoWorkspace.Qd[40];
-acadoWorkspace.QDy[47] += acadoWorkspace.Qd[41];
-acadoWorkspace.QDy[48] += acadoWorkspace.Qd[42];
-acadoWorkspace.QDy[49] += acadoWorkspace.Qd[43];
-acadoWorkspace.QDy[50] += acadoWorkspace.Qd[44];
-acadoWorkspace.QDy[51] += acadoWorkspace.Qd[45];
-acadoWorkspace.QDy[52] += acadoWorkspace.Qd[46];
-acadoWorkspace.QDy[53] += acadoWorkspace.Qd[47];
-acadoWorkspace.QDy[54] += acadoWorkspace.Qd[48];
-acadoWorkspace.QDy[55] += acadoWorkspace.Qd[49];
-acadoWorkspace.QDy[56] += acadoWorkspace.Qd[50];
-acadoWorkspace.QDy[57] += acadoWorkspace.Qd[51];
-acadoWorkspace.QDy[58] += acadoWorkspace.Qd[52];
-acadoWorkspace.QDy[59] += acadoWorkspace.Qd[53];
-acadoWorkspace.QDy[60] += acadoWorkspace.Qd[54];
-acadoWorkspace.QDy[61] += acadoWorkspace.Qd[55];
-acadoWorkspace.QDy[62] += acadoWorkspace.Qd[56];
-acadoWorkspace.QDy[63] += acadoWorkspace.Qd[57];
-acadoWorkspace.QDy[64] += acadoWorkspace.Qd[58];
-acadoWorkspace.QDy[65] += acadoWorkspace.Qd[59];
-acadoWorkspace.QDy[66] += acadoWorkspace.Qd[60];
-acadoWorkspace.QDy[67] += acadoWorkspace.Qd[61];
-acadoWorkspace.QDy[68] += acadoWorkspace.Qd[62];
-acadoWorkspace.QDy[69] += acadoWorkspace.Qd[63];
-acadoWorkspace.QDy[70] += acadoWorkspace.Qd[64];
-acadoWorkspace.QDy[71] += acadoWorkspace.Qd[65];
-acadoWorkspace.QDy[72] += acadoWorkspace.Qd[66];
-acadoWorkspace.QDy[73] += acadoWorkspace.Qd[67];
-acadoWorkspace.QDy[74] += acadoWorkspace.Qd[68];
-acadoWorkspace.QDy[75] += acadoWorkspace.Qd[69];
-acadoWorkspace.QDy[76] += acadoWorkspace.Qd[70];
-acadoWorkspace.QDy[77] += acadoWorkspace.Qd[71];
-acadoWorkspace.QDy[78] += acadoWorkspace.Qd[72];
-acadoWorkspace.QDy[79] += acadoWorkspace.Qd[73];
-acadoWorkspace.QDy[80] += acadoWorkspace.Qd[74];
-acadoWorkspace.QDy[81] += acadoWorkspace.Qd[75];
-acadoWorkspace.QDy[82] += acadoWorkspace.Qd[76];
-acadoWorkspace.QDy[83] += acadoWorkspace.Qd[77];
-acadoWorkspace.QDy[84] += acadoWorkspace.Qd[78];
-acadoWorkspace.QDy[85] += acadoWorkspace.Qd[79];
-acadoWorkspace.QDy[86] += acadoWorkspace.Qd[80];
-acadoWorkspace.QDy[87] += acadoWorkspace.Qd[81];
-acadoWorkspace.QDy[88] += acadoWorkspace.Qd[82];
-acadoWorkspace.QDy[89] += acadoWorkspace.Qd[83];
-acadoWorkspace.QDy[90] += acadoWorkspace.Qd[84];
-acadoWorkspace.QDy[91] += acadoWorkspace.Qd[85];
-acadoWorkspace.QDy[92] += acadoWorkspace.Qd[86];
-acadoWorkspace.QDy[93] += acadoWorkspace.Qd[87];
-acadoWorkspace.QDy[94] += acadoWorkspace.Qd[88];
-acadoWorkspace.QDy[95] += acadoWorkspace.Qd[89];
-acadoWorkspace.QDy[96] += acadoWorkspace.Qd[90];
-acadoWorkspace.QDy[97] += acadoWorkspace.Qd[91];
-acadoWorkspace.QDy[98] += acadoWorkspace.Qd[92];
-acadoWorkspace.QDy[99] += acadoWorkspace.Qd[93];
-acadoWorkspace.QDy[100] += acadoWorkspace.Qd[94];
-acadoWorkspace.QDy[101] += acadoWorkspace.Qd[95];
-acadoWorkspace.QDy[102] += acadoWorkspace.Qd[96];
-acadoWorkspace.QDy[103] += acadoWorkspace.Qd[97];
-acadoWorkspace.QDy[104] += acadoWorkspace.Qd[98];
-acadoWorkspace.QDy[105] += acadoWorkspace.Qd[99];
-acadoWorkspace.QDy[106] += acadoWorkspace.Qd[100];
-acadoWorkspace.QDy[107] += acadoWorkspace.Qd[101];
-acadoWorkspace.QDy[108] += acadoWorkspace.Qd[102];
-acadoWorkspace.QDy[109] += acadoWorkspace.Qd[103];
-acadoWorkspace.QDy[110] += acadoWorkspace.Qd[104];
-acadoWorkspace.QDy[111] += acadoWorkspace.Qd[105];
-acadoWorkspace.QDy[112] += acadoWorkspace.Qd[106];
-acadoWorkspace.QDy[113] += acadoWorkspace.Qd[107];
-acadoWorkspace.QDy[114] += acadoWorkspace.Qd[108];
-acadoWorkspace.QDy[115] += acadoWorkspace.Qd[109];
-acadoWorkspace.QDy[116] += acadoWorkspace.Qd[110];
-acadoWorkspace.QDy[117] += acadoWorkspace.Qd[111];
-acadoWorkspace.QDy[118] += acadoWorkspace.Qd[112];
-acadoWorkspace.QDy[119] += acadoWorkspace.Qd[113];
-acadoWorkspace.QDy[120] += acadoWorkspace.Qd[114];
-acadoWorkspace.QDy[121] += acadoWorkspace.Qd[115];
-acadoWorkspace.QDy[122] += acadoWorkspace.Qd[116];
-acadoWorkspace.QDy[123] += acadoWorkspace.Qd[117];
-acadoWorkspace.QDy[124] += acadoWorkspace.Qd[118];
-acadoWorkspace.QDy[125] += acadoWorkspace.Qd[119];
-
-acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[180]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[186]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[192]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[198]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[204]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[210]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[216]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[222]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[228]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[234]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[240]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[246]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[252]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[258]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[264]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[270]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[276]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[282]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[288]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[294]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[300]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[306]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[312]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[318]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[324]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[330]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[336]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[342]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[348]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[354]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[360]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[366]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[372]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[378]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[384]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[390]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[396]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[402]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[408]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[414]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[420]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[426]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[432]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[438]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[444]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[450]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[456]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[462]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[468]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[474]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[480]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[486]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[492]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[498]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[504]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[510]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[516]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[522]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[528]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[534]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[540]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[546]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[552]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[558]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[564]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[570]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[576]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[582]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[588]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[594]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[600]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[606]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[612]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[618]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[624]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[630]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[636]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[642]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[648]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[654]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[660]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[666]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[672]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[678]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[684]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[690]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[696]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[702]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[708]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[714]*acadoWorkspace.QDy[125];
-acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[181]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[187]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[193]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[199]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[205]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[211]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[217]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[223]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[229]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[235]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[241]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[247]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[253]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[259]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[265]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[271]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[277]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[283]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[289]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[295]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[301]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[307]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[313]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[319]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[325]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[331]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[337]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[343]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[349]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[355]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[361]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[367]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[373]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[379]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[385]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[391]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[397]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[403]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[409]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[415]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[421]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[427]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[433]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[439]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[445]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[451]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[457]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[463]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[469]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[475]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[481]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[487]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[493]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[499]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[505]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[511]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[517]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[523]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[529]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[535]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[541]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[547]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[553]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[559]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[565]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[571]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[577]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[583]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[589]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[595]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[601]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[607]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[613]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[619]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[625]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[631]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[637]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[643]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[649]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[655]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[661]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[667]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[673]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[679]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[685]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[691]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[697]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[703]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[709]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[715]*acadoWorkspace.QDy[125];
-acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[182]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[188]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[194]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[200]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[206]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[212]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[218]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[224]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[230]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[236]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[242]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[248]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[254]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[260]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[266]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[272]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[278]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[284]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[290]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[296]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[302]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[308]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[314]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[320]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[326]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[332]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[338]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[344]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[350]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[356]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[362]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[368]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[374]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[380]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[386]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[392]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[398]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[404]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[410]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[416]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[422]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[428]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[434]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[440]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[446]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[452]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[458]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[464]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[470]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[476]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[482]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[488]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[494]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[500]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[506]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[512]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[518]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[524]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[530]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[536]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[542]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[548]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[554]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[560]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[566]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[572]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[578]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[584]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[590]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[596]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[602]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[608]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[614]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[620]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[626]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[632]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[638]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[644]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[650]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[656]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[662]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[668]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[674]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[680]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[686]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[692]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[698]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[704]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[710]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[716]*acadoWorkspace.QDy[125];
-acadoWorkspace.g[3] = + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[183]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[189]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[195]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[201]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[207]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[213]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[219]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[225]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[231]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[237]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[243]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[249]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[255]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[261]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[267]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[273]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[279]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[285]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[291]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[297]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[303]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[309]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[315]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[321]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[327]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[333]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[339]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[345]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[351]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[357]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[363]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[369]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[375]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[381]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[387]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[393]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[399]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[405]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[411]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[417]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[423]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[429]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[435]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[441]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[447]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[453]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[459]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[465]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[471]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[477]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[483]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[489]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[495]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[501]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[507]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[513]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[519]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[525]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[531]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[537]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[543]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[549]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[555]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[561]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[567]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[573]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[579]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[585]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[591]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[597]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[603]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[609]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[615]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[621]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[627]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[633]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[639]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[645]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[651]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[657]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[663]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[669]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[675]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[681]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[687]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[693]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[699]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[705]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[711]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[717]*acadoWorkspace.QDy[125];
-acadoWorkspace.g[4] = + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[184]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[190]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[196]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[202]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[208]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[214]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[220]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[226]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[232]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[238]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[244]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[250]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[256]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[262]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[268]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[274]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[280]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[286]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[292]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[298]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[304]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[310]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[316]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[322]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[328]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[334]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[340]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[346]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[352]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[358]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[364]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[370]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[376]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[382]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[388]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[394]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[400]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[406]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[412]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[418]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[424]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[430]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[436]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[442]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[448]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[454]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[460]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[466]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[472]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[478]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[484]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[490]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[496]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[502]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[508]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[514]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[520]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[526]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[532]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[538]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[544]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[550]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[556]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[562]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[568]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[574]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[580]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[586]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[592]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[598]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[604]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[610]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[616]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[622]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[628]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[634]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[640]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[646]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[652]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[658]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[664]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[670]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[676]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[682]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[688]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[694]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[700]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[706]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[712]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[718]*acadoWorkspace.QDy[125];
-acadoWorkspace.g[5] = + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[185]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[191]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[197]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[203]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[209]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[215]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[221]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[227]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[233]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[239]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[245]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[251]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[257]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[263]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[269]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[275]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[281]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[287]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[293]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[299]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[305]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[311]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[317]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[323]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[329]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[335]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[341]*acadoWorkspace.QDy[62] + acadoWorkspace.evGx[347]*acadoWorkspace.QDy[63] + acadoWorkspace.evGx[353]*acadoWorkspace.QDy[64] + acadoWorkspace.evGx[359]*acadoWorkspace.QDy[65] + acadoWorkspace.evGx[365]*acadoWorkspace.QDy[66] + acadoWorkspace.evGx[371]*acadoWorkspace.QDy[67] + acadoWorkspace.evGx[377]*acadoWorkspace.QDy[68] + acadoWorkspace.evGx[383]*acadoWorkspace.QDy[69] + acadoWorkspace.evGx[389]*acadoWorkspace.QDy[70] + acadoWorkspace.evGx[395]*acadoWorkspace.QDy[71] + acadoWorkspace.evGx[401]*acadoWorkspace.QDy[72] + acadoWorkspace.evGx[407]*acadoWorkspace.QDy[73] + acadoWorkspace.evGx[413]*acadoWorkspace.QDy[74] + acadoWorkspace.evGx[419]*acadoWorkspace.QDy[75] + acadoWorkspace.evGx[425]*acadoWorkspace.QDy[76] + acadoWorkspace.evGx[431]*acadoWorkspace.QDy[77] + acadoWorkspace.evGx[437]*acadoWorkspace.QDy[78] + acadoWorkspace.evGx[443]*acadoWorkspace.QDy[79] + acadoWorkspace.evGx[449]*acadoWorkspace.QDy[80] + acadoWorkspace.evGx[455]*acadoWorkspace.QDy[81] + acadoWorkspace.evGx[461]*acadoWorkspace.QDy[82] + acadoWorkspace.evGx[467]*acadoWorkspace.QDy[83] + acadoWorkspace.evGx[473]*acadoWorkspace.QDy[84] + acadoWorkspace.evGx[479]*acadoWorkspace.QDy[85] + acadoWorkspace.evGx[485]*acadoWorkspace.QDy[86] + acadoWorkspace.evGx[491]*acadoWorkspace.QDy[87] + acadoWorkspace.evGx[497]*acadoWorkspace.QDy[88] + acadoWorkspace.evGx[503]*acadoWorkspace.QDy[89] + acadoWorkspace.evGx[509]*acadoWorkspace.QDy[90] + acadoWorkspace.evGx[515]*acadoWorkspace.QDy[91] + acadoWorkspace.evGx[521]*acadoWorkspace.QDy[92] + acadoWorkspace.evGx[527]*acadoWorkspace.QDy[93] + acadoWorkspace.evGx[533]*acadoWorkspace.QDy[94] + acadoWorkspace.evGx[539]*acadoWorkspace.QDy[95] + acadoWorkspace.evGx[545]*acadoWorkspace.QDy[96] + acadoWorkspace.evGx[551]*acadoWorkspace.QDy[97] + acadoWorkspace.evGx[557]*acadoWorkspace.QDy[98] + acadoWorkspace.evGx[563]*acadoWorkspace.QDy[99] + acadoWorkspace.evGx[569]*acadoWorkspace.QDy[100] + acadoWorkspace.evGx[575]*acadoWorkspace.QDy[101] + acadoWorkspace.evGx[581]*acadoWorkspace.QDy[102] + acadoWorkspace.evGx[587]*acadoWorkspace.QDy[103] + acadoWorkspace.evGx[593]*acadoWorkspace.QDy[104] + acadoWorkspace.evGx[599]*acadoWorkspace.QDy[105] + acadoWorkspace.evGx[605]*acadoWorkspace.QDy[106] + acadoWorkspace.evGx[611]*acadoWorkspace.QDy[107] + acadoWorkspace.evGx[617]*acadoWorkspace.QDy[108] + acadoWorkspace.evGx[623]*acadoWorkspace.QDy[109] + acadoWorkspace.evGx[629]*acadoWorkspace.QDy[110] + acadoWorkspace.evGx[635]*acadoWorkspace.QDy[111] + acadoWorkspace.evGx[641]*acadoWorkspace.QDy[112] + acadoWorkspace.evGx[647]*acadoWorkspace.QDy[113] + acadoWorkspace.evGx[653]*acadoWorkspace.QDy[114] + acadoWorkspace.evGx[659]*acadoWorkspace.QDy[115] + acadoWorkspace.evGx[665]*acadoWorkspace.QDy[116] + acadoWorkspace.evGx[671]*acadoWorkspace.QDy[117] + acadoWorkspace.evGx[677]*acadoWorkspace.QDy[118] + acadoWorkspace.evGx[683]*acadoWorkspace.QDy[119] + acadoWorkspace.evGx[689]*acadoWorkspace.QDy[120] + acadoWorkspace.evGx[695]*acadoWorkspace.QDy[121] + acadoWorkspace.evGx[701]*acadoWorkspace.QDy[122] + acadoWorkspace.evGx[707]*acadoWorkspace.QDy[123] + acadoWorkspace.evGx[713]*acadoWorkspace.QDy[124] + acadoWorkspace.evGx[719]*acadoWorkspace.QDy[125];
-
-
-acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 6 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 7 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 8 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 9 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 10 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 11 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 12 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 13 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 14 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 15 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QDy[ 66 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 16 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QDy[ 72 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 17 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 78 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 18 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 84 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 19 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.QDy[ 90 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 20 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.QDy[ 96 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 21 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.QDy[ 102 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 22 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.QDy[ 108 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 23 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.QDy[ 114 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 24 ]) );
-acado_multEQDy( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.QDy[ 120 ]), &(acadoWorkspace.g[ 25 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 12 ]), &(acadoWorkspace.Dy[ 4 ]), &(acadoWorkspace.QDy[ 3 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 24 ]), &(acadoWorkspace.Dy[ 8 ]), &(acadoWorkspace.QDy[ 6 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 36 ]), &(acadoWorkspace.Dy[ 12 ]), &(acadoWorkspace.QDy[ 9 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 48 ]), &(acadoWorkspace.Dy[ 16 ]), &(acadoWorkspace.QDy[ 12 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 60 ]), &(acadoWorkspace.Dy[ 20 ]), &(acadoWorkspace.QDy[ 15 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 72 ]), &(acadoWorkspace.Dy[ 24 ]), &(acadoWorkspace.QDy[ 18 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 84 ]), &(acadoWorkspace.Dy[ 28 ]), &(acadoWorkspace.QDy[ 21 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 96 ]), &(acadoWorkspace.Dy[ 32 ]), &(acadoWorkspace.QDy[ 24 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 108 ]), &(acadoWorkspace.Dy[ 36 ]), &(acadoWorkspace.QDy[ 27 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 120 ]), &(acadoWorkspace.Dy[ 40 ]), &(acadoWorkspace.QDy[ 30 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 132 ]), &(acadoWorkspace.Dy[ 44 ]), &(acadoWorkspace.QDy[ 33 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 144 ]), &(acadoWorkspace.Dy[ 48 ]), &(acadoWorkspace.QDy[ 36 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 156 ]), &(acadoWorkspace.Dy[ 52 ]), &(acadoWorkspace.QDy[ 39 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 168 ]), &(acadoWorkspace.Dy[ 56 ]), &(acadoWorkspace.QDy[ 42 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 180 ]), &(acadoWorkspace.Dy[ 60 ]), &(acadoWorkspace.QDy[ 45 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 192 ]), &(acadoWorkspace.Dy[ 64 ]), &(acadoWorkspace.QDy[ 48 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 204 ]), &(acadoWorkspace.Dy[ 68 ]), &(acadoWorkspace.QDy[ 51 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 216 ]), &(acadoWorkspace.Dy[ 72 ]), &(acadoWorkspace.QDy[ 54 ]) );
+acado_multQDy( &(acadoWorkspace.Q2[ 228 ]), &(acadoWorkspace.Dy[ 76 ]), &(acadoWorkspace.QDy[ 57 ]) );
+
+acadoWorkspace.QDy[60] = + acadoWorkspace.QN2[0]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[1]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[2]*acadoWorkspace.DyN[2];
+acadoWorkspace.QDy[61] = + acadoWorkspace.QN2[3]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[4]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[5]*acadoWorkspace.DyN[2];
+acadoWorkspace.QDy[62] = + acadoWorkspace.QN2[6]*acadoWorkspace.DyN[0] + acadoWorkspace.QN2[7]*acadoWorkspace.DyN[1] + acadoWorkspace.QN2[8]*acadoWorkspace.DyN[2];
+
+acadoWorkspace.QDy[3] += acadoWorkspace.Qd[0];
+acadoWorkspace.QDy[4] += acadoWorkspace.Qd[1];
+acadoWorkspace.QDy[5] += acadoWorkspace.Qd[2];
+acadoWorkspace.QDy[6] += acadoWorkspace.Qd[3];
+acadoWorkspace.QDy[7] += acadoWorkspace.Qd[4];
+acadoWorkspace.QDy[8] += acadoWorkspace.Qd[5];
+acadoWorkspace.QDy[9] += acadoWorkspace.Qd[6];
+acadoWorkspace.QDy[10] += acadoWorkspace.Qd[7];
+acadoWorkspace.QDy[11] += acadoWorkspace.Qd[8];
+acadoWorkspace.QDy[12] += acadoWorkspace.Qd[9];
+acadoWorkspace.QDy[13] += acadoWorkspace.Qd[10];
+acadoWorkspace.QDy[14] += acadoWorkspace.Qd[11];
+acadoWorkspace.QDy[15] += acadoWorkspace.Qd[12];
+acadoWorkspace.QDy[16] += acadoWorkspace.Qd[13];
+acadoWorkspace.QDy[17] += acadoWorkspace.Qd[14];
+acadoWorkspace.QDy[18] += acadoWorkspace.Qd[15];
+acadoWorkspace.QDy[19] += acadoWorkspace.Qd[16];
+acadoWorkspace.QDy[20] += acadoWorkspace.Qd[17];
+acadoWorkspace.QDy[21] += acadoWorkspace.Qd[18];
+acadoWorkspace.QDy[22] += acadoWorkspace.Qd[19];
+acadoWorkspace.QDy[23] += acadoWorkspace.Qd[20];
+acadoWorkspace.QDy[24] += acadoWorkspace.Qd[21];
+acadoWorkspace.QDy[25] += acadoWorkspace.Qd[22];
+acadoWorkspace.QDy[26] += acadoWorkspace.Qd[23];
+acadoWorkspace.QDy[27] += acadoWorkspace.Qd[24];
+acadoWorkspace.QDy[28] += acadoWorkspace.Qd[25];
+acadoWorkspace.QDy[29] += acadoWorkspace.Qd[26];
+acadoWorkspace.QDy[30] += acadoWorkspace.Qd[27];
+acadoWorkspace.QDy[31] += acadoWorkspace.Qd[28];
+acadoWorkspace.QDy[32] += acadoWorkspace.Qd[29];
+acadoWorkspace.QDy[33] += acadoWorkspace.Qd[30];
+acadoWorkspace.QDy[34] += acadoWorkspace.Qd[31];
+acadoWorkspace.QDy[35] += acadoWorkspace.Qd[32];
+acadoWorkspace.QDy[36] += acadoWorkspace.Qd[33];
+acadoWorkspace.QDy[37] += acadoWorkspace.Qd[34];
+acadoWorkspace.QDy[38] += acadoWorkspace.Qd[35];
+acadoWorkspace.QDy[39] += acadoWorkspace.Qd[36];
+acadoWorkspace.QDy[40] += acadoWorkspace.Qd[37];
+acadoWorkspace.QDy[41] += acadoWorkspace.Qd[38];
+acadoWorkspace.QDy[42] += acadoWorkspace.Qd[39];
+acadoWorkspace.QDy[43] += acadoWorkspace.Qd[40];
+acadoWorkspace.QDy[44] += acadoWorkspace.Qd[41];
+acadoWorkspace.QDy[45] += acadoWorkspace.Qd[42];
+acadoWorkspace.QDy[46] += acadoWorkspace.Qd[43];
+acadoWorkspace.QDy[47] += acadoWorkspace.Qd[44];
+acadoWorkspace.QDy[48] += acadoWorkspace.Qd[45];
+acadoWorkspace.QDy[49] += acadoWorkspace.Qd[46];
+acadoWorkspace.QDy[50] += acadoWorkspace.Qd[47];
+acadoWorkspace.QDy[51] += acadoWorkspace.Qd[48];
+acadoWorkspace.QDy[52] += acadoWorkspace.Qd[49];
+acadoWorkspace.QDy[53] += acadoWorkspace.Qd[50];
+acadoWorkspace.QDy[54] += acadoWorkspace.Qd[51];
+acadoWorkspace.QDy[55] += acadoWorkspace.Qd[52];
+acadoWorkspace.QDy[56] += acadoWorkspace.Qd[53];
+acadoWorkspace.QDy[57] += acadoWorkspace.Qd[54];
+acadoWorkspace.QDy[58] += acadoWorkspace.Qd[55];
+acadoWorkspace.QDy[59] += acadoWorkspace.Qd[56];
+acadoWorkspace.QDy[60] += acadoWorkspace.Qd[57];
+acadoWorkspace.QDy[61] += acadoWorkspace.Qd[58];
+acadoWorkspace.QDy[62] += acadoWorkspace.Qd[59];
+
+acadoWorkspace.g[0] = + acadoWorkspace.evGx[0]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[3]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[6]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[9]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[12]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[15]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[18]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[21]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[24]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[27]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[30]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[33]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[36]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[39]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[42]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[45]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[48]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[51]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[54]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[57]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[60]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[63]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[66]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[69]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[72]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[75]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[78]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[81]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[84]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[87]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[90]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[93]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[96]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[99]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[102]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[105]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[108]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[111]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[114]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[117]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[120]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[123]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[126]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[129]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[132]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[135]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[138]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[141]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[144]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[147]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[150]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[153]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[156]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[159]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[162]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[165]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[168]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[171]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[174]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[177]*acadoWorkspace.QDy[62];
+acadoWorkspace.g[1] = + acadoWorkspace.evGx[1]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[4]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[7]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[10]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[13]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[16]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[19]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[22]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[25]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[28]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[31]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[34]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[37]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[40]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[43]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[46]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[49]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[52]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[55]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[58]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[61]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[64]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[67]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[70]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[73]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[76]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[79]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[82]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[85]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[88]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[91]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[94]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[97]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[100]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[103]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[106]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[109]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[112]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[115]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[118]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[121]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[124]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[127]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[130]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[133]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[136]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[139]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[142]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[145]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[148]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[151]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[154]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[157]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[160]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[163]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[166]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[169]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[172]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[175]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[178]*acadoWorkspace.QDy[62];
+acadoWorkspace.g[2] = + acadoWorkspace.evGx[2]*acadoWorkspace.QDy[3] + acadoWorkspace.evGx[5]*acadoWorkspace.QDy[4] + acadoWorkspace.evGx[8]*acadoWorkspace.QDy[5] + acadoWorkspace.evGx[11]*acadoWorkspace.QDy[6] + acadoWorkspace.evGx[14]*acadoWorkspace.QDy[7] + acadoWorkspace.evGx[17]*acadoWorkspace.QDy[8] + acadoWorkspace.evGx[20]*acadoWorkspace.QDy[9] + acadoWorkspace.evGx[23]*acadoWorkspace.QDy[10] + acadoWorkspace.evGx[26]*acadoWorkspace.QDy[11] + acadoWorkspace.evGx[29]*acadoWorkspace.QDy[12] + acadoWorkspace.evGx[32]*acadoWorkspace.QDy[13] + acadoWorkspace.evGx[35]*acadoWorkspace.QDy[14] + acadoWorkspace.evGx[38]*acadoWorkspace.QDy[15] + acadoWorkspace.evGx[41]*acadoWorkspace.QDy[16] + acadoWorkspace.evGx[44]*acadoWorkspace.QDy[17] + acadoWorkspace.evGx[47]*acadoWorkspace.QDy[18] + acadoWorkspace.evGx[50]*acadoWorkspace.QDy[19] + acadoWorkspace.evGx[53]*acadoWorkspace.QDy[20] + acadoWorkspace.evGx[56]*acadoWorkspace.QDy[21] + acadoWorkspace.evGx[59]*acadoWorkspace.QDy[22] + acadoWorkspace.evGx[62]*acadoWorkspace.QDy[23] + acadoWorkspace.evGx[65]*acadoWorkspace.QDy[24] + acadoWorkspace.evGx[68]*acadoWorkspace.QDy[25] + acadoWorkspace.evGx[71]*acadoWorkspace.QDy[26] + acadoWorkspace.evGx[74]*acadoWorkspace.QDy[27] + acadoWorkspace.evGx[77]*acadoWorkspace.QDy[28] + acadoWorkspace.evGx[80]*acadoWorkspace.QDy[29] + acadoWorkspace.evGx[83]*acadoWorkspace.QDy[30] + acadoWorkspace.evGx[86]*acadoWorkspace.QDy[31] + acadoWorkspace.evGx[89]*acadoWorkspace.QDy[32] + acadoWorkspace.evGx[92]*acadoWorkspace.QDy[33] + acadoWorkspace.evGx[95]*acadoWorkspace.QDy[34] + acadoWorkspace.evGx[98]*acadoWorkspace.QDy[35] + acadoWorkspace.evGx[101]*acadoWorkspace.QDy[36] + acadoWorkspace.evGx[104]*acadoWorkspace.QDy[37] + acadoWorkspace.evGx[107]*acadoWorkspace.QDy[38] + acadoWorkspace.evGx[110]*acadoWorkspace.QDy[39] + acadoWorkspace.evGx[113]*acadoWorkspace.QDy[40] + acadoWorkspace.evGx[116]*acadoWorkspace.QDy[41] + acadoWorkspace.evGx[119]*acadoWorkspace.QDy[42] + acadoWorkspace.evGx[122]*acadoWorkspace.QDy[43] + acadoWorkspace.evGx[125]*acadoWorkspace.QDy[44] + acadoWorkspace.evGx[128]*acadoWorkspace.QDy[45] + acadoWorkspace.evGx[131]*acadoWorkspace.QDy[46] + acadoWorkspace.evGx[134]*acadoWorkspace.QDy[47] + acadoWorkspace.evGx[137]*acadoWorkspace.QDy[48] + acadoWorkspace.evGx[140]*acadoWorkspace.QDy[49] + acadoWorkspace.evGx[143]*acadoWorkspace.QDy[50] + acadoWorkspace.evGx[146]*acadoWorkspace.QDy[51] + acadoWorkspace.evGx[149]*acadoWorkspace.QDy[52] + acadoWorkspace.evGx[152]*acadoWorkspace.QDy[53] + acadoWorkspace.evGx[155]*acadoWorkspace.QDy[54] + acadoWorkspace.evGx[158]*acadoWorkspace.QDy[55] + acadoWorkspace.evGx[161]*acadoWorkspace.QDy[56] + acadoWorkspace.evGx[164]*acadoWorkspace.QDy[57] + acadoWorkspace.evGx[167]*acadoWorkspace.QDy[58] + acadoWorkspace.evGx[170]*acadoWorkspace.QDy[59] + acadoWorkspace.evGx[173]*acadoWorkspace.QDy[60] + acadoWorkspace.evGx[176]*acadoWorkspace.QDy[61] + acadoWorkspace.evGx[179]*acadoWorkspace.QDy[62];
+
+
+acado_multEQDy( acadoWorkspace.E, &(acadoWorkspace.QDy[ 3 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 3 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.QDy[ 6 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 4 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.QDy[ 9 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 5 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.QDy[ 12 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 6 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.QDy[ 15 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 7 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.QDy[ 18 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 8 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.QDy[ 21 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 9 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.QDy[ 24 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 10 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.QDy[ 27 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 11 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.QDy[ 30 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 12 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.QDy[ 33 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 13 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.QDy[ 36 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 14 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.QDy[ 39 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 15 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.QDy[ 42 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 16 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.QDy[ 45 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 17 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.QDy[ 48 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 18 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.QDy[ 51 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 19 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.QDy[ 54 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 20 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.QDy[ 57 ]), &(acadoWorkspace.g[ 21 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 21 ]) );
+acado_multEQDy( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.QDy[ 60 ]), &(acadoWorkspace.g[ 22 ]) );
acadoWorkspace.lb[0] = acadoWorkspace.Dx0[0];
acadoWorkspace.lb[1] = acadoWorkspace.Dx0[1];
acadoWorkspace.lb[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.lb[3] = acadoWorkspace.Dx0[3];
-acadoWorkspace.lb[4] = acadoWorkspace.Dx0[4];
-acadoWorkspace.lb[5] = acadoWorkspace.Dx0[5];
acadoWorkspace.ub[0] = acadoWorkspace.Dx0[0];
acadoWorkspace.ub[1] = acadoWorkspace.Dx0[1];
acadoWorkspace.ub[2] = acadoWorkspace.Dx0[2];
-acadoWorkspace.ub[3] = acadoWorkspace.Dx0[3];
-acadoWorkspace.ub[4] = acadoWorkspace.Dx0[4];
-acadoWorkspace.ub[5] = acadoWorkspace.Dx0[5];
-tmp = acadoVariables.x[7] + acadoWorkspace.d[1];
+tmp = acadoVariables.x[4] + acadoWorkspace.d[1];
acadoWorkspace.lbA[0] = - tmp;
acadoWorkspace.ubA[0] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[13] + acadoWorkspace.d[7];
+tmp = acadoVariables.x[7] + acadoWorkspace.d[4];
acadoWorkspace.lbA[1] = - tmp;
acadoWorkspace.ubA[1] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[19] + acadoWorkspace.d[13];
+tmp = acadoVariables.x[10] + acadoWorkspace.d[7];
acadoWorkspace.lbA[2] = - tmp;
acadoWorkspace.ubA[2] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[25] + acadoWorkspace.d[19];
+tmp = acadoVariables.x[13] + acadoWorkspace.d[10];
acadoWorkspace.lbA[3] = - tmp;
acadoWorkspace.ubA[3] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[31] + acadoWorkspace.d[25];
+tmp = acadoVariables.x[16] + acadoWorkspace.d[13];
acadoWorkspace.lbA[4] = - tmp;
acadoWorkspace.ubA[4] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[37] + acadoWorkspace.d[31];
+tmp = acadoVariables.x[19] + acadoWorkspace.d[16];
acadoWorkspace.lbA[5] = - tmp;
acadoWorkspace.ubA[5] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[43] + acadoWorkspace.d[37];
+tmp = acadoVariables.x[22] + acadoWorkspace.d[19];
acadoWorkspace.lbA[6] = - tmp;
acadoWorkspace.ubA[6] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[49] + acadoWorkspace.d[43];
+tmp = acadoVariables.x[25] + acadoWorkspace.d[22];
acadoWorkspace.lbA[7] = - tmp;
acadoWorkspace.ubA[7] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[55] + acadoWorkspace.d[49];
+tmp = acadoVariables.x[28] + acadoWorkspace.d[25];
acadoWorkspace.lbA[8] = - tmp;
acadoWorkspace.ubA[8] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[61] + acadoWorkspace.d[55];
+tmp = acadoVariables.x[31] + acadoWorkspace.d[28];
acadoWorkspace.lbA[9] = - tmp;
acadoWorkspace.ubA[9] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[67] + acadoWorkspace.d[61];
+tmp = acadoVariables.x[34] + acadoWorkspace.d[31];
acadoWorkspace.lbA[10] = - tmp;
acadoWorkspace.ubA[10] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[73] + acadoWorkspace.d[67];
+tmp = acadoVariables.x[37] + acadoWorkspace.d[34];
acadoWorkspace.lbA[11] = - tmp;
acadoWorkspace.ubA[11] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[79] + acadoWorkspace.d[73];
+tmp = acadoVariables.x[40] + acadoWorkspace.d[37];
acadoWorkspace.lbA[12] = - tmp;
acadoWorkspace.ubA[12] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[85] + acadoWorkspace.d[79];
+tmp = acadoVariables.x[43] + acadoWorkspace.d[40];
acadoWorkspace.lbA[13] = - tmp;
acadoWorkspace.ubA[13] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[91] + acadoWorkspace.d[85];
+tmp = acadoVariables.x[46] + acadoWorkspace.d[43];
acadoWorkspace.lbA[14] = - tmp;
acadoWorkspace.ubA[14] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[97] + acadoWorkspace.d[91];
+tmp = acadoVariables.x[49] + acadoWorkspace.d[46];
acadoWorkspace.lbA[15] = - tmp;
acadoWorkspace.ubA[15] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[103] + acadoWorkspace.d[97];
+tmp = acadoVariables.x[52] + acadoWorkspace.d[49];
acadoWorkspace.lbA[16] = - tmp;
acadoWorkspace.ubA[16] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[109] + acadoWorkspace.d[103];
+tmp = acadoVariables.x[55] + acadoWorkspace.d[52];
acadoWorkspace.lbA[17] = - tmp;
acadoWorkspace.ubA[17] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[115] + acadoWorkspace.d[109];
+tmp = acadoVariables.x[58] + acadoWorkspace.d[55];
acadoWorkspace.lbA[18] = - tmp;
acadoWorkspace.ubA[18] = (real_t)1.0000000000000000e+12 - tmp;
-tmp = acadoVariables.x[121] + acadoWorkspace.d[115];
+tmp = acadoVariables.x[61] + acadoWorkspace.d[58];
acadoWorkspace.lbA[19] = - tmp;
acadoWorkspace.ubA[19] = (real_t)1.0000000000000000e+12 - tmp;
@@ -4800,362 +4297,299 @@ void acado_expand( )
acadoVariables.x[0] += acadoWorkspace.x[0];
acadoVariables.x[1] += acadoWorkspace.x[1];
acadoVariables.x[2] += acadoWorkspace.x[2];
-acadoVariables.x[3] += acadoWorkspace.x[3];
-acadoVariables.x[4] += acadoWorkspace.x[4];
-acadoVariables.x[5] += acadoWorkspace.x[5];
-
-acadoVariables.u[0] += acadoWorkspace.x[6];
-acadoVariables.u[1] += acadoWorkspace.x[7];
-acadoVariables.u[2] += acadoWorkspace.x[8];
-acadoVariables.u[3] += acadoWorkspace.x[9];
-acadoVariables.u[4] += acadoWorkspace.x[10];
-acadoVariables.u[5] += acadoWorkspace.x[11];
-acadoVariables.u[6] += acadoWorkspace.x[12];
-acadoVariables.u[7] += acadoWorkspace.x[13];
-acadoVariables.u[8] += acadoWorkspace.x[14];
-acadoVariables.u[9] += acadoWorkspace.x[15];
-acadoVariables.u[10] += acadoWorkspace.x[16];
-acadoVariables.u[11] += acadoWorkspace.x[17];
-acadoVariables.u[12] += acadoWorkspace.x[18];
-acadoVariables.u[13] += acadoWorkspace.x[19];
-acadoVariables.u[14] += acadoWorkspace.x[20];
-acadoVariables.u[15] += acadoWorkspace.x[21];
-acadoVariables.u[16] += acadoWorkspace.x[22];
-acadoVariables.u[17] += acadoWorkspace.x[23];
-acadoVariables.u[18] += acadoWorkspace.x[24];
-acadoVariables.u[19] += acadoWorkspace.x[25];
-
-acadoVariables.x[6] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.evGx[3]*acadoWorkspace.x[3] + acadoWorkspace.evGx[4]*acadoWorkspace.x[4] + acadoWorkspace.evGx[5]*acadoWorkspace.x[5] + acadoWorkspace.d[0];
-acadoVariables.x[7] += + acadoWorkspace.evGx[6]*acadoWorkspace.x[0] + acadoWorkspace.evGx[7]*acadoWorkspace.x[1] + acadoWorkspace.evGx[8]*acadoWorkspace.x[2] + acadoWorkspace.evGx[9]*acadoWorkspace.x[3] + acadoWorkspace.evGx[10]*acadoWorkspace.x[4] + acadoWorkspace.evGx[11]*acadoWorkspace.x[5] + acadoWorkspace.d[1];
-acadoVariables.x[8] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.evGx[15]*acadoWorkspace.x[3] + acadoWorkspace.evGx[16]*acadoWorkspace.x[4] + acadoWorkspace.evGx[17]*acadoWorkspace.x[5] + acadoWorkspace.d[2];
-acadoVariables.x[9] += + acadoWorkspace.evGx[18]*acadoWorkspace.x[0] + acadoWorkspace.evGx[19]*acadoWorkspace.x[1] + acadoWorkspace.evGx[20]*acadoWorkspace.x[2] + acadoWorkspace.evGx[21]*acadoWorkspace.x[3] + acadoWorkspace.evGx[22]*acadoWorkspace.x[4] + acadoWorkspace.evGx[23]*acadoWorkspace.x[5] + acadoWorkspace.d[3];
-acadoVariables.x[10] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.evGx[27]*acadoWorkspace.x[3] + acadoWorkspace.evGx[28]*acadoWorkspace.x[4] + acadoWorkspace.evGx[29]*acadoWorkspace.x[5] + acadoWorkspace.d[4];
-acadoVariables.x[11] += + acadoWorkspace.evGx[30]*acadoWorkspace.x[0] + acadoWorkspace.evGx[31]*acadoWorkspace.x[1] + acadoWorkspace.evGx[32]*acadoWorkspace.x[2] + acadoWorkspace.evGx[33]*acadoWorkspace.x[3] + acadoWorkspace.evGx[34]*acadoWorkspace.x[4] + acadoWorkspace.evGx[35]*acadoWorkspace.x[5] + acadoWorkspace.d[5];
-acadoVariables.x[12] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.evGx[39]*acadoWorkspace.x[3] + acadoWorkspace.evGx[40]*acadoWorkspace.x[4] + acadoWorkspace.evGx[41]*acadoWorkspace.x[5] + acadoWorkspace.d[6];
-acadoVariables.x[13] += + acadoWorkspace.evGx[42]*acadoWorkspace.x[0] + acadoWorkspace.evGx[43]*acadoWorkspace.x[1] + acadoWorkspace.evGx[44]*acadoWorkspace.x[2] + acadoWorkspace.evGx[45]*acadoWorkspace.x[3] + acadoWorkspace.evGx[46]*acadoWorkspace.x[4] + acadoWorkspace.evGx[47]*acadoWorkspace.x[5] + acadoWorkspace.d[7];
-acadoVariables.x[14] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.evGx[51]*acadoWorkspace.x[3] + acadoWorkspace.evGx[52]*acadoWorkspace.x[4] + acadoWorkspace.evGx[53]*acadoWorkspace.x[5] + acadoWorkspace.d[8];
-acadoVariables.x[15] += + acadoWorkspace.evGx[54]*acadoWorkspace.x[0] + acadoWorkspace.evGx[55]*acadoWorkspace.x[1] + acadoWorkspace.evGx[56]*acadoWorkspace.x[2] + acadoWorkspace.evGx[57]*acadoWorkspace.x[3] + acadoWorkspace.evGx[58]*acadoWorkspace.x[4] + acadoWorkspace.evGx[59]*acadoWorkspace.x[5] + acadoWorkspace.d[9];
-acadoVariables.x[16] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.evGx[63]*acadoWorkspace.x[3] + acadoWorkspace.evGx[64]*acadoWorkspace.x[4] + acadoWorkspace.evGx[65]*acadoWorkspace.x[5] + acadoWorkspace.d[10];
-acadoVariables.x[17] += + acadoWorkspace.evGx[66]*acadoWorkspace.x[0] + acadoWorkspace.evGx[67]*acadoWorkspace.x[1] + acadoWorkspace.evGx[68]*acadoWorkspace.x[2] + acadoWorkspace.evGx[69]*acadoWorkspace.x[3] + acadoWorkspace.evGx[70]*acadoWorkspace.x[4] + acadoWorkspace.evGx[71]*acadoWorkspace.x[5] + acadoWorkspace.d[11];
-acadoVariables.x[18] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.evGx[75]*acadoWorkspace.x[3] + acadoWorkspace.evGx[76]*acadoWorkspace.x[4] + acadoWorkspace.evGx[77]*acadoWorkspace.x[5] + acadoWorkspace.d[12];
-acadoVariables.x[19] += + acadoWorkspace.evGx[78]*acadoWorkspace.x[0] + acadoWorkspace.evGx[79]*acadoWorkspace.x[1] + acadoWorkspace.evGx[80]*acadoWorkspace.x[2] + acadoWorkspace.evGx[81]*acadoWorkspace.x[3] + acadoWorkspace.evGx[82]*acadoWorkspace.x[4] + acadoWorkspace.evGx[83]*acadoWorkspace.x[5] + acadoWorkspace.d[13];
-acadoVariables.x[20] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.evGx[87]*acadoWorkspace.x[3] + acadoWorkspace.evGx[88]*acadoWorkspace.x[4] + acadoWorkspace.evGx[89]*acadoWorkspace.x[5] + acadoWorkspace.d[14];
-acadoVariables.x[21] += + acadoWorkspace.evGx[90]*acadoWorkspace.x[0] + acadoWorkspace.evGx[91]*acadoWorkspace.x[1] + acadoWorkspace.evGx[92]*acadoWorkspace.x[2] + acadoWorkspace.evGx[93]*acadoWorkspace.x[3] + acadoWorkspace.evGx[94]*acadoWorkspace.x[4] + acadoWorkspace.evGx[95]*acadoWorkspace.x[5] + acadoWorkspace.d[15];
-acadoVariables.x[22] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.evGx[99]*acadoWorkspace.x[3] + acadoWorkspace.evGx[100]*acadoWorkspace.x[4] + acadoWorkspace.evGx[101]*acadoWorkspace.x[5] + acadoWorkspace.d[16];
-acadoVariables.x[23] += + acadoWorkspace.evGx[102]*acadoWorkspace.x[0] + acadoWorkspace.evGx[103]*acadoWorkspace.x[1] + acadoWorkspace.evGx[104]*acadoWorkspace.x[2] + acadoWorkspace.evGx[105]*acadoWorkspace.x[3] + acadoWorkspace.evGx[106]*acadoWorkspace.x[4] + acadoWorkspace.evGx[107]*acadoWorkspace.x[5] + acadoWorkspace.d[17];
-acadoVariables.x[24] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.evGx[111]*acadoWorkspace.x[3] + acadoWorkspace.evGx[112]*acadoWorkspace.x[4] + acadoWorkspace.evGx[113]*acadoWorkspace.x[5] + acadoWorkspace.d[18];
-acadoVariables.x[25] += + acadoWorkspace.evGx[114]*acadoWorkspace.x[0] + acadoWorkspace.evGx[115]*acadoWorkspace.x[1] + acadoWorkspace.evGx[116]*acadoWorkspace.x[2] + acadoWorkspace.evGx[117]*acadoWorkspace.x[3] + acadoWorkspace.evGx[118]*acadoWorkspace.x[4] + acadoWorkspace.evGx[119]*acadoWorkspace.x[5] + acadoWorkspace.d[19];
-acadoVariables.x[26] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.evGx[123]*acadoWorkspace.x[3] + acadoWorkspace.evGx[124]*acadoWorkspace.x[4] + acadoWorkspace.evGx[125]*acadoWorkspace.x[5] + acadoWorkspace.d[20];
-acadoVariables.x[27] += + acadoWorkspace.evGx[126]*acadoWorkspace.x[0] + acadoWorkspace.evGx[127]*acadoWorkspace.x[1] + acadoWorkspace.evGx[128]*acadoWorkspace.x[2] + acadoWorkspace.evGx[129]*acadoWorkspace.x[3] + acadoWorkspace.evGx[130]*acadoWorkspace.x[4] + acadoWorkspace.evGx[131]*acadoWorkspace.x[5] + acadoWorkspace.d[21];
-acadoVariables.x[28] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.evGx[135]*acadoWorkspace.x[3] + acadoWorkspace.evGx[136]*acadoWorkspace.x[4] + acadoWorkspace.evGx[137]*acadoWorkspace.x[5] + acadoWorkspace.d[22];
-acadoVariables.x[29] += + acadoWorkspace.evGx[138]*acadoWorkspace.x[0] + acadoWorkspace.evGx[139]*acadoWorkspace.x[1] + acadoWorkspace.evGx[140]*acadoWorkspace.x[2] + acadoWorkspace.evGx[141]*acadoWorkspace.x[3] + acadoWorkspace.evGx[142]*acadoWorkspace.x[4] + acadoWorkspace.evGx[143]*acadoWorkspace.x[5] + acadoWorkspace.d[23];
-acadoVariables.x[30] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.evGx[147]*acadoWorkspace.x[3] + acadoWorkspace.evGx[148]*acadoWorkspace.x[4] + acadoWorkspace.evGx[149]*acadoWorkspace.x[5] + acadoWorkspace.d[24];
-acadoVariables.x[31] += + acadoWorkspace.evGx[150]*acadoWorkspace.x[0] + acadoWorkspace.evGx[151]*acadoWorkspace.x[1] + acadoWorkspace.evGx[152]*acadoWorkspace.x[2] + acadoWorkspace.evGx[153]*acadoWorkspace.x[3] + acadoWorkspace.evGx[154]*acadoWorkspace.x[4] + acadoWorkspace.evGx[155]*acadoWorkspace.x[5] + acadoWorkspace.d[25];
-acadoVariables.x[32] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.evGx[159]*acadoWorkspace.x[3] + acadoWorkspace.evGx[160]*acadoWorkspace.x[4] + acadoWorkspace.evGx[161]*acadoWorkspace.x[5] + acadoWorkspace.d[26];
-acadoVariables.x[33] += + acadoWorkspace.evGx[162]*acadoWorkspace.x[0] + acadoWorkspace.evGx[163]*acadoWorkspace.x[1] + acadoWorkspace.evGx[164]*acadoWorkspace.x[2] + acadoWorkspace.evGx[165]*acadoWorkspace.x[3] + acadoWorkspace.evGx[166]*acadoWorkspace.x[4] + acadoWorkspace.evGx[167]*acadoWorkspace.x[5] + acadoWorkspace.d[27];
-acadoVariables.x[34] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.evGx[171]*acadoWorkspace.x[3] + acadoWorkspace.evGx[172]*acadoWorkspace.x[4] + acadoWorkspace.evGx[173]*acadoWorkspace.x[5] + acadoWorkspace.d[28];
-acadoVariables.x[35] += + acadoWorkspace.evGx[174]*acadoWorkspace.x[0] + acadoWorkspace.evGx[175]*acadoWorkspace.x[1] + acadoWorkspace.evGx[176]*acadoWorkspace.x[2] + acadoWorkspace.evGx[177]*acadoWorkspace.x[3] + acadoWorkspace.evGx[178]*acadoWorkspace.x[4] + acadoWorkspace.evGx[179]*acadoWorkspace.x[5] + acadoWorkspace.d[29];
-acadoVariables.x[36] += + acadoWorkspace.evGx[180]*acadoWorkspace.x[0] + acadoWorkspace.evGx[181]*acadoWorkspace.x[1] + acadoWorkspace.evGx[182]*acadoWorkspace.x[2] + acadoWorkspace.evGx[183]*acadoWorkspace.x[3] + acadoWorkspace.evGx[184]*acadoWorkspace.x[4] + acadoWorkspace.evGx[185]*acadoWorkspace.x[5] + acadoWorkspace.d[30];
-acadoVariables.x[37] += + acadoWorkspace.evGx[186]*acadoWorkspace.x[0] + acadoWorkspace.evGx[187]*acadoWorkspace.x[1] + acadoWorkspace.evGx[188]*acadoWorkspace.x[2] + acadoWorkspace.evGx[189]*acadoWorkspace.x[3] + acadoWorkspace.evGx[190]*acadoWorkspace.x[4] + acadoWorkspace.evGx[191]*acadoWorkspace.x[5] + acadoWorkspace.d[31];
-acadoVariables.x[38] += + acadoWorkspace.evGx[192]*acadoWorkspace.x[0] + acadoWorkspace.evGx[193]*acadoWorkspace.x[1] + acadoWorkspace.evGx[194]*acadoWorkspace.x[2] + acadoWorkspace.evGx[195]*acadoWorkspace.x[3] + acadoWorkspace.evGx[196]*acadoWorkspace.x[4] + acadoWorkspace.evGx[197]*acadoWorkspace.x[5] + acadoWorkspace.d[32];
-acadoVariables.x[39] += + acadoWorkspace.evGx[198]*acadoWorkspace.x[0] + acadoWorkspace.evGx[199]*acadoWorkspace.x[1] + acadoWorkspace.evGx[200]*acadoWorkspace.x[2] + acadoWorkspace.evGx[201]*acadoWorkspace.x[3] + acadoWorkspace.evGx[202]*acadoWorkspace.x[4] + acadoWorkspace.evGx[203]*acadoWorkspace.x[5] + acadoWorkspace.d[33];
-acadoVariables.x[40] += + acadoWorkspace.evGx[204]*acadoWorkspace.x[0] + acadoWorkspace.evGx[205]*acadoWorkspace.x[1] + acadoWorkspace.evGx[206]*acadoWorkspace.x[2] + acadoWorkspace.evGx[207]*acadoWorkspace.x[3] + acadoWorkspace.evGx[208]*acadoWorkspace.x[4] + acadoWorkspace.evGx[209]*acadoWorkspace.x[5] + acadoWorkspace.d[34];
-acadoVariables.x[41] += + acadoWorkspace.evGx[210]*acadoWorkspace.x[0] + acadoWorkspace.evGx[211]*acadoWorkspace.x[1] + acadoWorkspace.evGx[212]*acadoWorkspace.x[2] + acadoWorkspace.evGx[213]*acadoWorkspace.x[3] + acadoWorkspace.evGx[214]*acadoWorkspace.x[4] + acadoWorkspace.evGx[215]*acadoWorkspace.x[5] + acadoWorkspace.d[35];
-acadoVariables.x[42] += + acadoWorkspace.evGx[216]*acadoWorkspace.x[0] + acadoWorkspace.evGx[217]*acadoWorkspace.x[1] + acadoWorkspace.evGx[218]*acadoWorkspace.x[2] + acadoWorkspace.evGx[219]*acadoWorkspace.x[3] + acadoWorkspace.evGx[220]*acadoWorkspace.x[4] + acadoWorkspace.evGx[221]*acadoWorkspace.x[5] + acadoWorkspace.d[36];
-acadoVariables.x[43] += + acadoWorkspace.evGx[222]*acadoWorkspace.x[0] + acadoWorkspace.evGx[223]*acadoWorkspace.x[1] + acadoWorkspace.evGx[224]*acadoWorkspace.x[2] + acadoWorkspace.evGx[225]*acadoWorkspace.x[3] + acadoWorkspace.evGx[226]*acadoWorkspace.x[4] + acadoWorkspace.evGx[227]*acadoWorkspace.x[5] + acadoWorkspace.d[37];
-acadoVariables.x[44] += + acadoWorkspace.evGx[228]*acadoWorkspace.x[0] + acadoWorkspace.evGx[229]*acadoWorkspace.x[1] + acadoWorkspace.evGx[230]*acadoWorkspace.x[2] + acadoWorkspace.evGx[231]*acadoWorkspace.x[3] + acadoWorkspace.evGx[232]*acadoWorkspace.x[4] + acadoWorkspace.evGx[233]*acadoWorkspace.x[5] + acadoWorkspace.d[38];
-acadoVariables.x[45] += + acadoWorkspace.evGx[234]*acadoWorkspace.x[0] + acadoWorkspace.evGx[235]*acadoWorkspace.x[1] + acadoWorkspace.evGx[236]*acadoWorkspace.x[2] + acadoWorkspace.evGx[237]*acadoWorkspace.x[3] + acadoWorkspace.evGx[238]*acadoWorkspace.x[4] + acadoWorkspace.evGx[239]*acadoWorkspace.x[5] + acadoWorkspace.d[39];
-acadoVariables.x[46] += + acadoWorkspace.evGx[240]*acadoWorkspace.x[0] + acadoWorkspace.evGx[241]*acadoWorkspace.x[1] + acadoWorkspace.evGx[242]*acadoWorkspace.x[2] + acadoWorkspace.evGx[243]*acadoWorkspace.x[3] + acadoWorkspace.evGx[244]*acadoWorkspace.x[4] + acadoWorkspace.evGx[245]*acadoWorkspace.x[5] + acadoWorkspace.d[40];
-acadoVariables.x[47] += + acadoWorkspace.evGx[246]*acadoWorkspace.x[0] + acadoWorkspace.evGx[247]*acadoWorkspace.x[1] + acadoWorkspace.evGx[248]*acadoWorkspace.x[2] + acadoWorkspace.evGx[249]*acadoWorkspace.x[3] + acadoWorkspace.evGx[250]*acadoWorkspace.x[4] + acadoWorkspace.evGx[251]*acadoWorkspace.x[5] + acadoWorkspace.d[41];
-acadoVariables.x[48] += + acadoWorkspace.evGx[252]*acadoWorkspace.x[0] + acadoWorkspace.evGx[253]*acadoWorkspace.x[1] + acadoWorkspace.evGx[254]*acadoWorkspace.x[2] + acadoWorkspace.evGx[255]*acadoWorkspace.x[3] + acadoWorkspace.evGx[256]*acadoWorkspace.x[4] + acadoWorkspace.evGx[257]*acadoWorkspace.x[5] + acadoWorkspace.d[42];
-acadoVariables.x[49] += + acadoWorkspace.evGx[258]*acadoWorkspace.x[0] + acadoWorkspace.evGx[259]*acadoWorkspace.x[1] + acadoWorkspace.evGx[260]*acadoWorkspace.x[2] + acadoWorkspace.evGx[261]*acadoWorkspace.x[3] + acadoWorkspace.evGx[262]*acadoWorkspace.x[4] + acadoWorkspace.evGx[263]*acadoWorkspace.x[5] + acadoWorkspace.d[43];
-acadoVariables.x[50] += + acadoWorkspace.evGx[264]*acadoWorkspace.x[0] + acadoWorkspace.evGx[265]*acadoWorkspace.x[1] + acadoWorkspace.evGx[266]*acadoWorkspace.x[2] + acadoWorkspace.evGx[267]*acadoWorkspace.x[3] + acadoWorkspace.evGx[268]*acadoWorkspace.x[4] + acadoWorkspace.evGx[269]*acadoWorkspace.x[5] + acadoWorkspace.d[44];
-acadoVariables.x[51] += + acadoWorkspace.evGx[270]*acadoWorkspace.x[0] + acadoWorkspace.evGx[271]*acadoWorkspace.x[1] + acadoWorkspace.evGx[272]*acadoWorkspace.x[2] + acadoWorkspace.evGx[273]*acadoWorkspace.x[3] + acadoWorkspace.evGx[274]*acadoWorkspace.x[4] + acadoWorkspace.evGx[275]*acadoWorkspace.x[5] + acadoWorkspace.d[45];
-acadoVariables.x[52] += + acadoWorkspace.evGx[276]*acadoWorkspace.x[0] + acadoWorkspace.evGx[277]*acadoWorkspace.x[1] + acadoWorkspace.evGx[278]*acadoWorkspace.x[2] + acadoWorkspace.evGx[279]*acadoWorkspace.x[3] + acadoWorkspace.evGx[280]*acadoWorkspace.x[4] + acadoWorkspace.evGx[281]*acadoWorkspace.x[5] + acadoWorkspace.d[46];
-acadoVariables.x[53] += + acadoWorkspace.evGx[282]*acadoWorkspace.x[0] + acadoWorkspace.evGx[283]*acadoWorkspace.x[1] + acadoWorkspace.evGx[284]*acadoWorkspace.x[2] + acadoWorkspace.evGx[285]*acadoWorkspace.x[3] + acadoWorkspace.evGx[286]*acadoWorkspace.x[4] + acadoWorkspace.evGx[287]*acadoWorkspace.x[5] + acadoWorkspace.d[47];
-acadoVariables.x[54] += + acadoWorkspace.evGx[288]*acadoWorkspace.x[0] + acadoWorkspace.evGx[289]*acadoWorkspace.x[1] + acadoWorkspace.evGx[290]*acadoWorkspace.x[2] + acadoWorkspace.evGx[291]*acadoWorkspace.x[3] + acadoWorkspace.evGx[292]*acadoWorkspace.x[4] + acadoWorkspace.evGx[293]*acadoWorkspace.x[5] + acadoWorkspace.d[48];
-acadoVariables.x[55] += + acadoWorkspace.evGx[294]*acadoWorkspace.x[0] + acadoWorkspace.evGx[295]*acadoWorkspace.x[1] + acadoWorkspace.evGx[296]*acadoWorkspace.x[2] + acadoWorkspace.evGx[297]*acadoWorkspace.x[3] + acadoWorkspace.evGx[298]*acadoWorkspace.x[4] + acadoWorkspace.evGx[299]*acadoWorkspace.x[5] + acadoWorkspace.d[49];
-acadoVariables.x[56] += + acadoWorkspace.evGx[300]*acadoWorkspace.x[0] + acadoWorkspace.evGx[301]*acadoWorkspace.x[1] + acadoWorkspace.evGx[302]*acadoWorkspace.x[2] + acadoWorkspace.evGx[303]*acadoWorkspace.x[3] + acadoWorkspace.evGx[304]*acadoWorkspace.x[4] + acadoWorkspace.evGx[305]*acadoWorkspace.x[5] + acadoWorkspace.d[50];
-acadoVariables.x[57] += + acadoWorkspace.evGx[306]*acadoWorkspace.x[0] + acadoWorkspace.evGx[307]*acadoWorkspace.x[1] + acadoWorkspace.evGx[308]*acadoWorkspace.x[2] + acadoWorkspace.evGx[309]*acadoWorkspace.x[3] + acadoWorkspace.evGx[310]*acadoWorkspace.x[4] + acadoWorkspace.evGx[311]*acadoWorkspace.x[5] + acadoWorkspace.d[51];
-acadoVariables.x[58] += + acadoWorkspace.evGx[312]*acadoWorkspace.x[0] + acadoWorkspace.evGx[313]*acadoWorkspace.x[1] + acadoWorkspace.evGx[314]*acadoWorkspace.x[2] + acadoWorkspace.evGx[315]*acadoWorkspace.x[3] + acadoWorkspace.evGx[316]*acadoWorkspace.x[4] + acadoWorkspace.evGx[317]*acadoWorkspace.x[5] + acadoWorkspace.d[52];
-acadoVariables.x[59] += + acadoWorkspace.evGx[318]*acadoWorkspace.x[0] + acadoWorkspace.evGx[319]*acadoWorkspace.x[1] + acadoWorkspace.evGx[320]*acadoWorkspace.x[2] + acadoWorkspace.evGx[321]*acadoWorkspace.x[3] + acadoWorkspace.evGx[322]*acadoWorkspace.x[4] + acadoWorkspace.evGx[323]*acadoWorkspace.x[5] + acadoWorkspace.d[53];
-acadoVariables.x[60] += + acadoWorkspace.evGx[324]*acadoWorkspace.x[0] + acadoWorkspace.evGx[325]*acadoWorkspace.x[1] + acadoWorkspace.evGx[326]*acadoWorkspace.x[2] + acadoWorkspace.evGx[327]*acadoWorkspace.x[3] + acadoWorkspace.evGx[328]*acadoWorkspace.x[4] + acadoWorkspace.evGx[329]*acadoWorkspace.x[5] + acadoWorkspace.d[54];
-acadoVariables.x[61] += + acadoWorkspace.evGx[330]*acadoWorkspace.x[0] + acadoWorkspace.evGx[331]*acadoWorkspace.x[1] + acadoWorkspace.evGx[332]*acadoWorkspace.x[2] + acadoWorkspace.evGx[333]*acadoWorkspace.x[3] + acadoWorkspace.evGx[334]*acadoWorkspace.x[4] + acadoWorkspace.evGx[335]*acadoWorkspace.x[5] + acadoWorkspace.d[55];
-acadoVariables.x[62] += + acadoWorkspace.evGx[336]*acadoWorkspace.x[0] + acadoWorkspace.evGx[337]*acadoWorkspace.x[1] + acadoWorkspace.evGx[338]*acadoWorkspace.x[2] + acadoWorkspace.evGx[339]*acadoWorkspace.x[3] + acadoWorkspace.evGx[340]*acadoWorkspace.x[4] + acadoWorkspace.evGx[341]*acadoWorkspace.x[5] + acadoWorkspace.d[56];
-acadoVariables.x[63] += + acadoWorkspace.evGx[342]*acadoWorkspace.x[0] + acadoWorkspace.evGx[343]*acadoWorkspace.x[1] + acadoWorkspace.evGx[344]*acadoWorkspace.x[2] + acadoWorkspace.evGx[345]*acadoWorkspace.x[3] + acadoWorkspace.evGx[346]*acadoWorkspace.x[4] + acadoWorkspace.evGx[347]*acadoWorkspace.x[5] + acadoWorkspace.d[57];
-acadoVariables.x[64] += + acadoWorkspace.evGx[348]*acadoWorkspace.x[0] + acadoWorkspace.evGx[349]*acadoWorkspace.x[1] + acadoWorkspace.evGx[350]*acadoWorkspace.x[2] + acadoWorkspace.evGx[351]*acadoWorkspace.x[3] + acadoWorkspace.evGx[352]*acadoWorkspace.x[4] + acadoWorkspace.evGx[353]*acadoWorkspace.x[5] + acadoWorkspace.d[58];
-acadoVariables.x[65] += + acadoWorkspace.evGx[354]*acadoWorkspace.x[0] + acadoWorkspace.evGx[355]*acadoWorkspace.x[1] + acadoWorkspace.evGx[356]*acadoWorkspace.x[2] + acadoWorkspace.evGx[357]*acadoWorkspace.x[3] + acadoWorkspace.evGx[358]*acadoWorkspace.x[4] + acadoWorkspace.evGx[359]*acadoWorkspace.x[5] + acadoWorkspace.d[59];
-acadoVariables.x[66] += + acadoWorkspace.evGx[360]*acadoWorkspace.x[0] + acadoWorkspace.evGx[361]*acadoWorkspace.x[1] + acadoWorkspace.evGx[362]*acadoWorkspace.x[2] + acadoWorkspace.evGx[363]*acadoWorkspace.x[3] + acadoWorkspace.evGx[364]*acadoWorkspace.x[4] + acadoWorkspace.evGx[365]*acadoWorkspace.x[5] + acadoWorkspace.d[60];
-acadoVariables.x[67] += + acadoWorkspace.evGx[366]*acadoWorkspace.x[0] + acadoWorkspace.evGx[367]*acadoWorkspace.x[1] + acadoWorkspace.evGx[368]*acadoWorkspace.x[2] + acadoWorkspace.evGx[369]*acadoWorkspace.x[3] + acadoWorkspace.evGx[370]*acadoWorkspace.x[4] + acadoWorkspace.evGx[371]*acadoWorkspace.x[5] + acadoWorkspace.d[61];
-acadoVariables.x[68] += + acadoWorkspace.evGx[372]*acadoWorkspace.x[0] + acadoWorkspace.evGx[373]*acadoWorkspace.x[1] + acadoWorkspace.evGx[374]*acadoWorkspace.x[2] + acadoWorkspace.evGx[375]*acadoWorkspace.x[3] + acadoWorkspace.evGx[376]*acadoWorkspace.x[4] + acadoWorkspace.evGx[377]*acadoWorkspace.x[5] + acadoWorkspace.d[62];
-acadoVariables.x[69] += + acadoWorkspace.evGx[378]*acadoWorkspace.x[0] + acadoWorkspace.evGx[379]*acadoWorkspace.x[1] + acadoWorkspace.evGx[380]*acadoWorkspace.x[2] + acadoWorkspace.evGx[381]*acadoWorkspace.x[3] + acadoWorkspace.evGx[382]*acadoWorkspace.x[4] + acadoWorkspace.evGx[383]*acadoWorkspace.x[5] + acadoWorkspace.d[63];
-acadoVariables.x[70] += + acadoWorkspace.evGx[384]*acadoWorkspace.x[0] + acadoWorkspace.evGx[385]*acadoWorkspace.x[1] + acadoWorkspace.evGx[386]*acadoWorkspace.x[2] + acadoWorkspace.evGx[387]*acadoWorkspace.x[3] + acadoWorkspace.evGx[388]*acadoWorkspace.x[4] + acadoWorkspace.evGx[389]*acadoWorkspace.x[5] + acadoWorkspace.d[64];
-acadoVariables.x[71] += + acadoWorkspace.evGx[390]*acadoWorkspace.x[0] + acadoWorkspace.evGx[391]*acadoWorkspace.x[1] + acadoWorkspace.evGx[392]*acadoWorkspace.x[2] + acadoWorkspace.evGx[393]*acadoWorkspace.x[3] + acadoWorkspace.evGx[394]*acadoWorkspace.x[4] + acadoWorkspace.evGx[395]*acadoWorkspace.x[5] + acadoWorkspace.d[65];
-acadoVariables.x[72] += + acadoWorkspace.evGx[396]*acadoWorkspace.x[0] + acadoWorkspace.evGx[397]*acadoWorkspace.x[1] + acadoWorkspace.evGx[398]*acadoWorkspace.x[2] + acadoWorkspace.evGx[399]*acadoWorkspace.x[3] + acadoWorkspace.evGx[400]*acadoWorkspace.x[4] + acadoWorkspace.evGx[401]*acadoWorkspace.x[5] + acadoWorkspace.d[66];
-acadoVariables.x[73] += + acadoWorkspace.evGx[402]*acadoWorkspace.x[0] + acadoWorkspace.evGx[403]*acadoWorkspace.x[1] + acadoWorkspace.evGx[404]*acadoWorkspace.x[2] + acadoWorkspace.evGx[405]*acadoWorkspace.x[3] + acadoWorkspace.evGx[406]*acadoWorkspace.x[4] + acadoWorkspace.evGx[407]*acadoWorkspace.x[5] + acadoWorkspace.d[67];
-acadoVariables.x[74] += + acadoWorkspace.evGx[408]*acadoWorkspace.x[0] + acadoWorkspace.evGx[409]*acadoWorkspace.x[1] + acadoWorkspace.evGx[410]*acadoWorkspace.x[2] + acadoWorkspace.evGx[411]*acadoWorkspace.x[3] + acadoWorkspace.evGx[412]*acadoWorkspace.x[4] + acadoWorkspace.evGx[413]*acadoWorkspace.x[5] + acadoWorkspace.d[68];
-acadoVariables.x[75] += + acadoWorkspace.evGx[414]*acadoWorkspace.x[0] + acadoWorkspace.evGx[415]*acadoWorkspace.x[1] + acadoWorkspace.evGx[416]*acadoWorkspace.x[2] + acadoWorkspace.evGx[417]*acadoWorkspace.x[3] + acadoWorkspace.evGx[418]*acadoWorkspace.x[4] + acadoWorkspace.evGx[419]*acadoWorkspace.x[5] + acadoWorkspace.d[69];
-acadoVariables.x[76] += + acadoWorkspace.evGx[420]*acadoWorkspace.x[0] + acadoWorkspace.evGx[421]*acadoWorkspace.x[1] + acadoWorkspace.evGx[422]*acadoWorkspace.x[2] + acadoWorkspace.evGx[423]*acadoWorkspace.x[3] + acadoWorkspace.evGx[424]*acadoWorkspace.x[4] + acadoWorkspace.evGx[425]*acadoWorkspace.x[5] + acadoWorkspace.d[70];
-acadoVariables.x[77] += + acadoWorkspace.evGx[426]*acadoWorkspace.x[0] + acadoWorkspace.evGx[427]*acadoWorkspace.x[1] + acadoWorkspace.evGx[428]*acadoWorkspace.x[2] + acadoWorkspace.evGx[429]*acadoWorkspace.x[3] + acadoWorkspace.evGx[430]*acadoWorkspace.x[4] + acadoWorkspace.evGx[431]*acadoWorkspace.x[5] + acadoWorkspace.d[71];
-acadoVariables.x[78] += + acadoWorkspace.evGx[432]*acadoWorkspace.x[0] + acadoWorkspace.evGx[433]*acadoWorkspace.x[1] + acadoWorkspace.evGx[434]*acadoWorkspace.x[2] + acadoWorkspace.evGx[435]*acadoWorkspace.x[3] + acadoWorkspace.evGx[436]*acadoWorkspace.x[4] + acadoWorkspace.evGx[437]*acadoWorkspace.x[5] + acadoWorkspace.d[72];
-acadoVariables.x[79] += + acadoWorkspace.evGx[438]*acadoWorkspace.x[0] + acadoWorkspace.evGx[439]*acadoWorkspace.x[1] + acadoWorkspace.evGx[440]*acadoWorkspace.x[2] + acadoWorkspace.evGx[441]*acadoWorkspace.x[3] + acadoWorkspace.evGx[442]*acadoWorkspace.x[4] + acadoWorkspace.evGx[443]*acadoWorkspace.x[5] + acadoWorkspace.d[73];
-acadoVariables.x[80] += + acadoWorkspace.evGx[444]*acadoWorkspace.x[0] + acadoWorkspace.evGx[445]*acadoWorkspace.x[1] + acadoWorkspace.evGx[446]*acadoWorkspace.x[2] + acadoWorkspace.evGx[447]*acadoWorkspace.x[3] + acadoWorkspace.evGx[448]*acadoWorkspace.x[4] + acadoWorkspace.evGx[449]*acadoWorkspace.x[5] + acadoWorkspace.d[74];
-acadoVariables.x[81] += + acadoWorkspace.evGx[450]*acadoWorkspace.x[0] + acadoWorkspace.evGx[451]*acadoWorkspace.x[1] + acadoWorkspace.evGx[452]*acadoWorkspace.x[2] + acadoWorkspace.evGx[453]*acadoWorkspace.x[3] + acadoWorkspace.evGx[454]*acadoWorkspace.x[4] + acadoWorkspace.evGx[455]*acadoWorkspace.x[5] + acadoWorkspace.d[75];
-acadoVariables.x[82] += + acadoWorkspace.evGx[456]*acadoWorkspace.x[0] + acadoWorkspace.evGx[457]*acadoWorkspace.x[1] + acadoWorkspace.evGx[458]*acadoWorkspace.x[2] + acadoWorkspace.evGx[459]*acadoWorkspace.x[3] + acadoWorkspace.evGx[460]*acadoWorkspace.x[4] + acadoWorkspace.evGx[461]*acadoWorkspace.x[5] + acadoWorkspace.d[76];
-acadoVariables.x[83] += + acadoWorkspace.evGx[462]*acadoWorkspace.x[0] + acadoWorkspace.evGx[463]*acadoWorkspace.x[1] + acadoWorkspace.evGx[464]*acadoWorkspace.x[2] + acadoWorkspace.evGx[465]*acadoWorkspace.x[3] + acadoWorkspace.evGx[466]*acadoWorkspace.x[4] + acadoWorkspace.evGx[467]*acadoWorkspace.x[5] + acadoWorkspace.d[77];
-acadoVariables.x[84] += + acadoWorkspace.evGx[468]*acadoWorkspace.x[0] + acadoWorkspace.evGx[469]*acadoWorkspace.x[1] + acadoWorkspace.evGx[470]*acadoWorkspace.x[2] + acadoWorkspace.evGx[471]*acadoWorkspace.x[3] + acadoWorkspace.evGx[472]*acadoWorkspace.x[4] + acadoWorkspace.evGx[473]*acadoWorkspace.x[5] + acadoWorkspace.d[78];
-acadoVariables.x[85] += + acadoWorkspace.evGx[474]*acadoWorkspace.x[0] + acadoWorkspace.evGx[475]*acadoWorkspace.x[1] + acadoWorkspace.evGx[476]*acadoWorkspace.x[2] + acadoWorkspace.evGx[477]*acadoWorkspace.x[3] + acadoWorkspace.evGx[478]*acadoWorkspace.x[4] + acadoWorkspace.evGx[479]*acadoWorkspace.x[5] + acadoWorkspace.d[79];
-acadoVariables.x[86] += + acadoWorkspace.evGx[480]*acadoWorkspace.x[0] + acadoWorkspace.evGx[481]*acadoWorkspace.x[1] + acadoWorkspace.evGx[482]*acadoWorkspace.x[2] + acadoWorkspace.evGx[483]*acadoWorkspace.x[3] + acadoWorkspace.evGx[484]*acadoWorkspace.x[4] + acadoWorkspace.evGx[485]*acadoWorkspace.x[5] + acadoWorkspace.d[80];
-acadoVariables.x[87] += + acadoWorkspace.evGx[486]*acadoWorkspace.x[0] + acadoWorkspace.evGx[487]*acadoWorkspace.x[1] + acadoWorkspace.evGx[488]*acadoWorkspace.x[2] + acadoWorkspace.evGx[489]*acadoWorkspace.x[3] + acadoWorkspace.evGx[490]*acadoWorkspace.x[4] + acadoWorkspace.evGx[491]*acadoWorkspace.x[5] + acadoWorkspace.d[81];
-acadoVariables.x[88] += + acadoWorkspace.evGx[492]*acadoWorkspace.x[0] + acadoWorkspace.evGx[493]*acadoWorkspace.x[1] + acadoWorkspace.evGx[494]*acadoWorkspace.x[2] + acadoWorkspace.evGx[495]*acadoWorkspace.x[3] + acadoWorkspace.evGx[496]*acadoWorkspace.x[4] + acadoWorkspace.evGx[497]*acadoWorkspace.x[5] + acadoWorkspace.d[82];
-acadoVariables.x[89] += + acadoWorkspace.evGx[498]*acadoWorkspace.x[0] + acadoWorkspace.evGx[499]*acadoWorkspace.x[1] + acadoWorkspace.evGx[500]*acadoWorkspace.x[2] + acadoWorkspace.evGx[501]*acadoWorkspace.x[3] + acadoWorkspace.evGx[502]*acadoWorkspace.x[4] + acadoWorkspace.evGx[503]*acadoWorkspace.x[5] + acadoWorkspace.d[83];
-acadoVariables.x[90] += + acadoWorkspace.evGx[504]*acadoWorkspace.x[0] + acadoWorkspace.evGx[505]*acadoWorkspace.x[1] + acadoWorkspace.evGx[506]*acadoWorkspace.x[2] + acadoWorkspace.evGx[507]*acadoWorkspace.x[3] + acadoWorkspace.evGx[508]*acadoWorkspace.x[4] + acadoWorkspace.evGx[509]*acadoWorkspace.x[5] + acadoWorkspace.d[84];
-acadoVariables.x[91] += + acadoWorkspace.evGx[510]*acadoWorkspace.x[0] + acadoWorkspace.evGx[511]*acadoWorkspace.x[1] + acadoWorkspace.evGx[512]*acadoWorkspace.x[2] + acadoWorkspace.evGx[513]*acadoWorkspace.x[3] + acadoWorkspace.evGx[514]*acadoWorkspace.x[4] + acadoWorkspace.evGx[515]*acadoWorkspace.x[5] + acadoWorkspace.d[85];
-acadoVariables.x[92] += + acadoWorkspace.evGx[516]*acadoWorkspace.x[0] + acadoWorkspace.evGx[517]*acadoWorkspace.x[1] + acadoWorkspace.evGx[518]*acadoWorkspace.x[2] + acadoWorkspace.evGx[519]*acadoWorkspace.x[3] + acadoWorkspace.evGx[520]*acadoWorkspace.x[4] + acadoWorkspace.evGx[521]*acadoWorkspace.x[5] + acadoWorkspace.d[86];
-acadoVariables.x[93] += + acadoWorkspace.evGx[522]*acadoWorkspace.x[0] + acadoWorkspace.evGx[523]*acadoWorkspace.x[1] + acadoWorkspace.evGx[524]*acadoWorkspace.x[2] + acadoWorkspace.evGx[525]*acadoWorkspace.x[3] + acadoWorkspace.evGx[526]*acadoWorkspace.x[4] + acadoWorkspace.evGx[527]*acadoWorkspace.x[5] + acadoWorkspace.d[87];
-acadoVariables.x[94] += + acadoWorkspace.evGx[528]*acadoWorkspace.x[0] + acadoWorkspace.evGx[529]*acadoWorkspace.x[1] + acadoWorkspace.evGx[530]*acadoWorkspace.x[2] + acadoWorkspace.evGx[531]*acadoWorkspace.x[3] + acadoWorkspace.evGx[532]*acadoWorkspace.x[4] + acadoWorkspace.evGx[533]*acadoWorkspace.x[5] + acadoWorkspace.d[88];
-acadoVariables.x[95] += + acadoWorkspace.evGx[534]*acadoWorkspace.x[0] + acadoWorkspace.evGx[535]*acadoWorkspace.x[1] + acadoWorkspace.evGx[536]*acadoWorkspace.x[2] + acadoWorkspace.evGx[537]*acadoWorkspace.x[3] + acadoWorkspace.evGx[538]*acadoWorkspace.x[4] + acadoWorkspace.evGx[539]*acadoWorkspace.x[5] + acadoWorkspace.d[89];
-acadoVariables.x[96] += + acadoWorkspace.evGx[540]*acadoWorkspace.x[0] + acadoWorkspace.evGx[541]*acadoWorkspace.x[1] + acadoWorkspace.evGx[542]*acadoWorkspace.x[2] + acadoWorkspace.evGx[543]*acadoWorkspace.x[3] + acadoWorkspace.evGx[544]*acadoWorkspace.x[4] + acadoWorkspace.evGx[545]*acadoWorkspace.x[5] + acadoWorkspace.d[90];
-acadoVariables.x[97] += + acadoWorkspace.evGx[546]*acadoWorkspace.x[0] + acadoWorkspace.evGx[547]*acadoWorkspace.x[1] + acadoWorkspace.evGx[548]*acadoWorkspace.x[2] + acadoWorkspace.evGx[549]*acadoWorkspace.x[3] + acadoWorkspace.evGx[550]*acadoWorkspace.x[4] + acadoWorkspace.evGx[551]*acadoWorkspace.x[5] + acadoWorkspace.d[91];
-acadoVariables.x[98] += + acadoWorkspace.evGx[552]*acadoWorkspace.x[0] + acadoWorkspace.evGx[553]*acadoWorkspace.x[1] + acadoWorkspace.evGx[554]*acadoWorkspace.x[2] + acadoWorkspace.evGx[555]*acadoWorkspace.x[3] + acadoWorkspace.evGx[556]*acadoWorkspace.x[4] + acadoWorkspace.evGx[557]*acadoWorkspace.x[5] + acadoWorkspace.d[92];
-acadoVariables.x[99] += + acadoWorkspace.evGx[558]*acadoWorkspace.x[0] + acadoWorkspace.evGx[559]*acadoWorkspace.x[1] + acadoWorkspace.evGx[560]*acadoWorkspace.x[2] + acadoWorkspace.evGx[561]*acadoWorkspace.x[3] + acadoWorkspace.evGx[562]*acadoWorkspace.x[4] + acadoWorkspace.evGx[563]*acadoWorkspace.x[5] + acadoWorkspace.d[93];
-acadoVariables.x[100] += + acadoWorkspace.evGx[564]*acadoWorkspace.x[0] + acadoWorkspace.evGx[565]*acadoWorkspace.x[1] + acadoWorkspace.evGx[566]*acadoWorkspace.x[2] + acadoWorkspace.evGx[567]*acadoWorkspace.x[3] + acadoWorkspace.evGx[568]*acadoWorkspace.x[4] + acadoWorkspace.evGx[569]*acadoWorkspace.x[5] + acadoWorkspace.d[94];
-acadoVariables.x[101] += + acadoWorkspace.evGx[570]*acadoWorkspace.x[0] + acadoWorkspace.evGx[571]*acadoWorkspace.x[1] + acadoWorkspace.evGx[572]*acadoWorkspace.x[2] + acadoWorkspace.evGx[573]*acadoWorkspace.x[3] + acadoWorkspace.evGx[574]*acadoWorkspace.x[4] + acadoWorkspace.evGx[575]*acadoWorkspace.x[5] + acadoWorkspace.d[95];
-acadoVariables.x[102] += + acadoWorkspace.evGx[576]*acadoWorkspace.x[0] + acadoWorkspace.evGx[577]*acadoWorkspace.x[1] + acadoWorkspace.evGx[578]*acadoWorkspace.x[2] + acadoWorkspace.evGx[579]*acadoWorkspace.x[3] + acadoWorkspace.evGx[580]*acadoWorkspace.x[4] + acadoWorkspace.evGx[581]*acadoWorkspace.x[5] + acadoWorkspace.d[96];
-acadoVariables.x[103] += + acadoWorkspace.evGx[582]*acadoWorkspace.x[0] + acadoWorkspace.evGx[583]*acadoWorkspace.x[1] + acadoWorkspace.evGx[584]*acadoWorkspace.x[2] + acadoWorkspace.evGx[585]*acadoWorkspace.x[3] + acadoWorkspace.evGx[586]*acadoWorkspace.x[4] + acadoWorkspace.evGx[587]*acadoWorkspace.x[5] + acadoWorkspace.d[97];
-acadoVariables.x[104] += + acadoWorkspace.evGx[588]*acadoWorkspace.x[0] + acadoWorkspace.evGx[589]*acadoWorkspace.x[1] + acadoWorkspace.evGx[590]*acadoWorkspace.x[2] + acadoWorkspace.evGx[591]*acadoWorkspace.x[3] + acadoWorkspace.evGx[592]*acadoWorkspace.x[4] + acadoWorkspace.evGx[593]*acadoWorkspace.x[5] + acadoWorkspace.d[98];
-acadoVariables.x[105] += + acadoWorkspace.evGx[594]*acadoWorkspace.x[0] + acadoWorkspace.evGx[595]*acadoWorkspace.x[1] + acadoWorkspace.evGx[596]*acadoWorkspace.x[2] + acadoWorkspace.evGx[597]*acadoWorkspace.x[3] + acadoWorkspace.evGx[598]*acadoWorkspace.x[4] + acadoWorkspace.evGx[599]*acadoWorkspace.x[5] + acadoWorkspace.d[99];
-acadoVariables.x[106] += + acadoWorkspace.evGx[600]*acadoWorkspace.x[0] + acadoWorkspace.evGx[601]*acadoWorkspace.x[1] + acadoWorkspace.evGx[602]*acadoWorkspace.x[2] + acadoWorkspace.evGx[603]*acadoWorkspace.x[3] + acadoWorkspace.evGx[604]*acadoWorkspace.x[4] + acadoWorkspace.evGx[605]*acadoWorkspace.x[5] + acadoWorkspace.d[100];
-acadoVariables.x[107] += + acadoWorkspace.evGx[606]*acadoWorkspace.x[0] + acadoWorkspace.evGx[607]*acadoWorkspace.x[1] + acadoWorkspace.evGx[608]*acadoWorkspace.x[2] + acadoWorkspace.evGx[609]*acadoWorkspace.x[3] + acadoWorkspace.evGx[610]*acadoWorkspace.x[4] + acadoWorkspace.evGx[611]*acadoWorkspace.x[5] + acadoWorkspace.d[101];
-acadoVariables.x[108] += + acadoWorkspace.evGx[612]*acadoWorkspace.x[0] + acadoWorkspace.evGx[613]*acadoWorkspace.x[1] + acadoWorkspace.evGx[614]*acadoWorkspace.x[2] + acadoWorkspace.evGx[615]*acadoWorkspace.x[3] + acadoWorkspace.evGx[616]*acadoWorkspace.x[4] + acadoWorkspace.evGx[617]*acadoWorkspace.x[5] + acadoWorkspace.d[102];
-acadoVariables.x[109] += + acadoWorkspace.evGx[618]*acadoWorkspace.x[0] + acadoWorkspace.evGx[619]*acadoWorkspace.x[1] + acadoWorkspace.evGx[620]*acadoWorkspace.x[2] + acadoWorkspace.evGx[621]*acadoWorkspace.x[3] + acadoWorkspace.evGx[622]*acadoWorkspace.x[4] + acadoWorkspace.evGx[623]*acadoWorkspace.x[5] + acadoWorkspace.d[103];
-acadoVariables.x[110] += + acadoWorkspace.evGx[624]*acadoWorkspace.x[0] + acadoWorkspace.evGx[625]*acadoWorkspace.x[1] + acadoWorkspace.evGx[626]*acadoWorkspace.x[2] + acadoWorkspace.evGx[627]*acadoWorkspace.x[3] + acadoWorkspace.evGx[628]*acadoWorkspace.x[4] + acadoWorkspace.evGx[629]*acadoWorkspace.x[5] + acadoWorkspace.d[104];
-acadoVariables.x[111] += + acadoWorkspace.evGx[630]*acadoWorkspace.x[0] + acadoWorkspace.evGx[631]*acadoWorkspace.x[1] + acadoWorkspace.evGx[632]*acadoWorkspace.x[2] + acadoWorkspace.evGx[633]*acadoWorkspace.x[3] + acadoWorkspace.evGx[634]*acadoWorkspace.x[4] + acadoWorkspace.evGx[635]*acadoWorkspace.x[5] + acadoWorkspace.d[105];
-acadoVariables.x[112] += + acadoWorkspace.evGx[636]*acadoWorkspace.x[0] + acadoWorkspace.evGx[637]*acadoWorkspace.x[1] + acadoWorkspace.evGx[638]*acadoWorkspace.x[2] + acadoWorkspace.evGx[639]*acadoWorkspace.x[3] + acadoWorkspace.evGx[640]*acadoWorkspace.x[4] + acadoWorkspace.evGx[641]*acadoWorkspace.x[5] + acadoWorkspace.d[106];
-acadoVariables.x[113] += + acadoWorkspace.evGx[642]*acadoWorkspace.x[0] + acadoWorkspace.evGx[643]*acadoWorkspace.x[1] + acadoWorkspace.evGx[644]*acadoWorkspace.x[2] + acadoWorkspace.evGx[645]*acadoWorkspace.x[3] + acadoWorkspace.evGx[646]*acadoWorkspace.x[4] + acadoWorkspace.evGx[647]*acadoWorkspace.x[5] + acadoWorkspace.d[107];
-acadoVariables.x[114] += + acadoWorkspace.evGx[648]*acadoWorkspace.x[0] + acadoWorkspace.evGx[649]*acadoWorkspace.x[1] + acadoWorkspace.evGx[650]*acadoWorkspace.x[2] + acadoWorkspace.evGx[651]*acadoWorkspace.x[3] + acadoWorkspace.evGx[652]*acadoWorkspace.x[4] + acadoWorkspace.evGx[653]*acadoWorkspace.x[5] + acadoWorkspace.d[108];
-acadoVariables.x[115] += + acadoWorkspace.evGx[654]*acadoWorkspace.x[0] + acadoWorkspace.evGx[655]*acadoWorkspace.x[1] + acadoWorkspace.evGx[656]*acadoWorkspace.x[2] + acadoWorkspace.evGx[657]*acadoWorkspace.x[3] + acadoWorkspace.evGx[658]*acadoWorkspace.x[4] + acadoWorkspace.evGx[659]*acadoWorkspace.x[5] + acadoWorkspace.d[109];
-acadoVariables.x[116] += + acadoWorkspace.evGx[660]*acadoWorkspace.x[0] + acadoWorkspace.evGx[661]*acadoWorkspace.x[1] + acadoWorkspace.evGx[662]*acadoWorkspace.x[2] + acadoWorkspace.evGx[663]*acadoWorkspace.x[3] + acadoWorkspace.evGx[664]*acadoWorkspace.x[4] + acadoWorkspace.evGx[665]*acadoWorkspace.x[5] + acadoWorkspace.d[110];
-acadoVariables.x[117] += + acadoWorkspace.evGx[666]*acadoWorkspace.x[0] + acadoWorkspace.evGx[667]*acadoWorkspace.x[1] + acadoWorkspace.evGx[668]*acadoWorkspace.x[2] + acadoWorkspace.evGx[669]*acadoWorkspace.x[3] + acadoWorkspace.evGx[670]*acadoWorkspace.x[4] + acadoWorkspace.evGx[671]*acadoWorkspace.x[5] + acadoWorkspace.d[111];
-acadoVariables.x[118] += + acadoWorkspace.evGx[672]*acadoWorkspace.x[0] + acadoWorkspace.evGx[673]*acadoWorkspace.x[1] + acadoWorkspace.evGx[674]*acadoWorkspace.x[2] + acadoWorkspace.evGx[675]*acadoWorkspace.x[3] + acadoWorkspace.evGx[676]*acadoWorkspace.x[4] + acadoWorkspace.evGx[677]*acadoWorkspace.x[5] + acadoWorkspace.d[112];
-acadoVariables.x[119] += + acadoWorkspace.evGx[678]*acadoWorkspace.x[0] + acadoWorkspace.evGx[679]*acadoWorkspace.x[1] + acadoWorkspace.evGx[680]*acadoWorkspace.x[2] + acadoWorkspace.evGx[681]*acadoWorkspace.x[3] + acadoWorkspace.evGx[682]*acadoWorkspace.x[4] + acadoWorkspace.evGx[683]*acadoWorkspace.x[5] + acadoWorkspace.d[113];
-acadoVariables.x[120] += + acadoWorkspace.evGx[684]*acadoWorkspace.x[0] + acadoWorkspace.evGx[685]*acadoWorkspace.x[1] + acadoWorkspace.evGx[686]*acadoWorkspace.x[2] + acadoWorkspace.evGx[687]*acadoWorkspace.x[3] + acadoWorkspace.evGx[688]*acadoWorkspace.x[4] + acadoWorkspace.evGx[689]*acadoWorkspace.x[5] + acadoWorkspace.d[114];
-acadoVariables.x[121] += + acadoWorkspace.evGx[690]*acadoWorkspace.x[0] + acadoWorkspace.evGx[691]*acadoWorkspace.x[1] + acadoWorkspace.evGx[692]*acadoWorkspace.x[2] + acadoWorkspace.evGx[693]*acadoWorkspace.x[3] + acadoWorkspace.evGx[694]*acadoWorkspace.x[4] + acadoWorkspace.evGx[695]*acadoWorkspace.x[5] + acadoWorkspace.d[115];
-acadoVariables.x[122] += + acadoWorkspace.evGx[696]*acadoWorkspace.x[0] + acadoWorkspace.evGx[697]*acadoWorkspace.x[1] + acadoWorkspace.evGx[698]*acadoWorkspace.x[2] + acadoWorkspace.evGx[699]*acadoWorkspace.x[3] + acadoWorkspace.evGx[700]*acadoWorkspace.x[4] + acadoWorkspace.evGx[701]*acadoWorkspace.x[5] + acadoWorkspace.d[116];
-acadoVariables.x[123] += + acadoWorkspace.evGx[702]*acadoWorkspace.x[0] + acadoWorkspace.evGx[703]*acadoWorkspace.x[1] + acadoWorkspace.evGx[704]*acadoWorkspace.x[2] + acadoWorkspace.evGx[705]*acadoWorkspace.x[3] + acadoWorkspace.evGx[706]*acadoWorkspace.x[4] + acadoWorkspace.evGx[707]*acadoWorkspace.x[5] + acadoWorkspace.d[117];
-acadoVariables.x[124] += + acadoWorkspace.evGx[708]*acadoWorkspace.x[0] + acadoWorkspace.evGx[709]*acadoWorkspace.x[1] + acadoWorkspace.evGx[710]*acadoWorkspace.x[2] + acadoWorkspace.evGx[711]*acadoWorkspace.x[3] + acadoWorkspace.evGx[712]*acadoWorkspace.x[4] + acadoWorkspace.evGx[713]*acadoWorkspace.x[5] + acadoWorkspace.d[118];
-acadoVariables.x[125] += + acadoWorkspace.evGx[714]*acadoWorkspace.x[0] + acadoWorkspace.evGx[715]*acadoWorkspace.x[1] + acadoWorkspace.evGx[716]*acadoWorkspace.x[2] + acadoWorkspace.evGx[717]*acadoWorkspace.x[3] + acadoWorkspace.evGx[718]*acadoWorkspace.x[4] + acadoWorkspace.evGx[719]*acadoWorkspace.x[5] + acadoWorkspace.d[119];
-
-acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 6 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 12 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 18 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 30 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 42 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 54 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 66 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 72 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 78 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 84 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 630 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 636 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 642 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 648 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 654 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 660 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 666 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 672 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 678 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 684 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 690 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 696 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 702 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 708 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 714 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 90 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 720 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 726 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 732 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 738 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 744 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 750 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 756 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 762 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 768 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 774 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 780 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 786 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 792 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 798 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 804 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 810 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 96 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 816 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 822 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 828 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 834 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 840 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 846 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 852 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 858 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 864 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 870 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 876 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 882 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 888 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 894 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 900 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 906 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 912 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 102 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 918 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 924 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 930 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 936 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 942 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 948 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 954 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 960 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 966 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 972 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 978 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 984 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 990 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 996 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1002 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1008 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1014 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1020 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 108 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1026 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1032 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1038 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1044 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1050 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1056 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1062 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1068 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1074 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1080 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1086 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1092 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1098 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1104 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1110 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1116 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1122 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1128 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1134 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVariables.x[ 114 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1140 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1146 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1152 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1158 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1164 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1170 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1176 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1182 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1188 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1194 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1200 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1206 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1212 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1218 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1224 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1230 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1236 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1242 ]), &(acadoWorkspace.x[ 23 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1248 ]), &(acadoWorkspace.x[ 24 ]), &(acadoVariables.x[ 120 ]) );
-acado_multEDu( &(acadoWorkspace.E[ 1254 ]), &(acadoWorkspace.x[ 25 ]), &(acadoVariables.x[ 120 ]) );
+
+acadoVariables.u[0] += acadoWorkspace.x[3];
+acadoVariables.u[1] += acadoWorkspace.x[4];
+acadoVariables.u[2] += acadoWorkspace.x[5];
+acadoVariables.u[3] += acadoWorkspace.x[6];
+acadoVariables.u[4] += acadoWorkspace.x[7];
+acadoVariables.u[5] += acadoWorkspace.x[8];
+acadoVariables.u[6] += acadoWorkspace.x[9];
+acadoVariables.u[7] += acadoWorkspace.x[10];
+acadoVariables.u[8] += acadoWorkspace.x[11];
+acadoVariables.u[9] += acadoWorkspace.x[12];
+acadoVariables.u[10] += acadoWorkspace.x[13];
+acadoVariables.u[11] += acadoWorkspace.x[14];
+acadoVariables.u[12] += acadoWorkspace.x[15];
+acadoVariables.u[13] += acadoWorkspace.x[16];
+acadoVariables.u[14] += acadoWorkspace.x[17];
+acadoVariables.u[15] += acadoWorkspace.x[18];
+acadoVariables.u[16] += acadoWorkspace.x[19];
+acadoVariables.u[17] += acadoWorkspace.x[20];
+acadoVariables.u[18] += acadoWorkspace.x[21];
+acadoVariables.u[19] += acadoWorkspace.x[22];
+
+acadoVariables.x[3] += + acadoWorkspace.evGx[0]*acadoWorkspace.x[0] + acadoWorkspace.evGx[1]*acadoWorkspace.x[1] + acadoWorkspace.evGx[2]*acadoWorkspace.x[2] + acadoWorkspace.d[0];
+acadoVariables.x[4] += + acadoWorkspace.evGx[3]*acadoWorkspace.x[0] + acadoWorkspace.evGx[4]*acadoWorkspace.x[1] + acadoWorkspace.evGx[5]*acadoWorkspace.x[2] + acadoWorkspace.d[1];
+acadoVariables.x[5] += + acadoWorkspace.evGx[6]*acadoWorkspace.x[0] + acadoWorkspace.evGx[7]*acadoWorkspace.x[1] + acadoWorkspace.evGx[8]*acadoWorkspace.x[2] + acadoWorkspace.d[2];
+acadoVariables.x[6] += + acadoWorkspace.evGx[9]*acadoWorkspace.x[0] + acadoWorkspace.evGx[10]*acadoWorkspace.x[1] + acadoWorkspace.evGx[11]*acadoWorkspace.x[2] + acadoWorkspace.d[3];
+acadoVariables.x[7] += + acadoWorkspace.evGx[12]*acadoWorkspace.x[0] + acadoWorkspace.evGx[13]*acadoWorkspace.x[1] + acadoWorkspace.evGx[14]*acadoWorkspace.x[2] + acadoWorkspace.d[4];
+acadoVariables.x[8] += + acadoWorkspace.evGx[15]*acadoWorkspace.x[0] + acadoWorkspace.evGx[16]*acadoWorkspace.x[1] + acadoWorkspace.evGx[17]*acadoWorkspace.x[2] + acadoWorkspace.d[5];
+acadoVariables.x[9] += + acadoWorkspace.evGx[18]*acadoWorkspace.x[0] + acadoWorkspace.evGx[19]*acadoWorkspace.x[1] + acadoWorkspace.evGx[20]*acadoWorkspace.x[2] + acadoWorkspace.d[6];
+acadoVariables.x[10] += + acadoWorkspace.evGx[21]*acadoWorkspace.x[0] + acadoWorkspace.evGx[22]*acadoWorkspace.x[1] + acadoWorkspace.evGx[23]*acadoWorkspace.x[2] + acadoWorkspace.d[7];
+acadoVariables.x[11] += + acadoWorkspace.evGx[24]*acadoWorkspace.x[0] + acadoWorkspace.evGx[25]*acadoWorkspace.x[1] + acadoWorkspace.evGx[26]*acadoWorkspace.x[2] + acadoWorkspace.d[8];
+acadoVariables.x[12] += + acadoWorkspace.evGx[27]*acadoWorkspace.x[0] + acadoWorkspace.evGx[28]*acadoWorkspace.x[1] + acadoWorkspace.evGx[29]*acadoWorkspace.x[2] + acadoWorkspace.d[9];
+acadoVariables.x[13] += + acadoWorkspace.evGx[30]*acadoWorkspace.x[0] + acadoWorkspace.evGx[31]*acadoWorkspace.x[1] + acadoWorkspace.evGx[32]*acadoWorkspace.x[2] + acadoWorkspace.d[10];
+acadoVariables.x[14] += + acadoWorkspace.evGx[33]*acadoWorkspace.x[0] + acadoWorkspace.evGx[34]*acadoWorkspace.x[1] + acadoWorkspace.evGx[35]*acadoWorkspace.x[2] + acadoWorkspace.d[11];
+acadoVariables.x[15] += + acadoWorkspace.evGx[36]*acadoWorkspace.x[0] + acadoWorkspace.evGx[37]*acadoWorkspace.x[1] + acadoWorkspace.evGx[38]*acadoWorkspace.x[2] + acadoWorkspace.d[12];
+acadoVariables.x[16] += + acadoWorkspace.evGx[39]*acadoWorkspace.x[0] + acadoWorkspace.evGx[40]*acadoWorkspace.x[1] + acadoWorkspace.evGx[41]*acadoWorkspace.x[2] + acadoWorkspace.d[13];
+acadoVariables.x[17] += + acadoWorkspace.evGx[42]*acadoWorkspace.x[0] + acadoWorkspace.evGx[43]*acadoWorkspace.x[1] + acadoWorkspace.evGx[44]*acadoWorkspace.x[2] + acadoWorkspace.d[14];
+acadoVariables.x[18] += + acadoWorkspace.evGx[45]*acadoWorkspace.x[0] + acadoWorkspace.evGx[46]*acadoWorkspace.x[1] + acadoWorkspace.evGx[47]*acadoWorkspace.x[2] + acadoWorkspace.d[15];
+acadoVariables.x[19] += + acadoWorkspace.evGx[48]*acadoWorkspace.x[0] + acadoWorkspace.evGx[49]*acadoWorkspace.x[1] + acadoWorkspace.evGx[50]*acadoWorkspace.x[2] + acadoWorkspace.d[16];
+acadoVariables.x[20] += + acadoWorkspace.evGx[51]*acadoWorkspace.x[0] + acadoWorkspace.evGx[52]*acadoWorkspace.x[1] + acadoWorkspace.evGx[53]*acadoWorkspace.x[2] + acadoWorkspace.d[17];
+acadoVariables.x[21] += + acadoWorkspace.evGx[54]*acadoWorkspace.x[0] + acadoWorkspace.evGx[55]*acadoWorkspace.x[1] + acadoWorkspace.evGx[56]*acadoWorkspace.x[2] + acadoWorkspace.d[18];
+acadoVariables.x[22] += + acadoWorkspace.evGx[57]*acadoWorkspace.x[0] + acadoWorkspace.evGx[58]*acadoWorkspace.x[1] + acadoWorkspace.evGx[59]*acadoWorkspace.x[2] + acadoWorkspace.d[19];
+acadoVariables.x[23] += + acadoWorkspace.evGx[60]*acadoWorkspace.x[0] + acadoWorkspace.evGx[61]*acadoWorkspace.x[1] + acadoWorkspace.evGx[62]*acadoWorkspace.x[2] + acadoWorkspace.d[20];
+acadoVariables.x[24] += + acadoWorkspace.evGx[63]*acadoWorkspace.x[0] + acadoWorkspace.evGx[64]*acadoWorkspace.x[1] + acadoWorkspace.evGx[65]*acadoWorkspace.x[2] + acadoWorkspace.d[21];
+acadoVariables.x[25] += + acadoWorkspace.evGx[66]*acadoWorkspace.x[0] + acadoWorkspace.evGx[67]*acadoWorkspace.x[1] + acadoWorkspace.evGx[68]*acadoWorkspace.x[2] + acadoWorkspace.d[22];
+acadoVariables.x[26] += + acadoWorkspace.evGx[69]*acadoWorkspace.x[0] + acadoWorkspace.evGx[70]*acadoWorkspace.x[1] + acadoWorkspace.evGx[71]*acadoWorkspace.x[2] + acadoWorkspace.d[23];
+acadoVariables.x[27] += + acadoWorkspace.evGx[72]*acadoWorkspace.x[0] + acadoWorkspace.evGx[73]*acadoWorkspace.x[1] + acadoWorkspace.evGx[74]*acadoWorkspace.x[2] + acadoWorkspace.d[24];
+acadoVariables.x[28] += + acadoWorkspace.evGx[75]*acadoWorkspace.x[0] + acadoWorkspace.evGx[76]*acadoWorkspace.x[1] + acadoWorkspace.evGx[77]*acadoWorkspace.x[2] + acadoWorkspace.d[25];
+acadoVariables.x[29] += + acadoWorkspace.evGx[78]*acadoWorkspace.x[0] + acadoWorkspace.evGx[79]*acadoWorkspace.x[1] + acadoWorkspace.evGx[80]*acadoWorkspace.x[2] + acadoWorkspace.d[26];
+acadoVariables.x[30] += + acadoWorkspace.evGx[81]*acadoWorkspace.x[0] + acadoWorkspace.evGx[82]*acadoWorkspace.x[1] + acadoWorkspace.evGx[83]*acadoWorkspace.x[2] + acadoWorkspace.d[27];
+acadoVariables.x[31] += + acadoWorkspace.evGx[84]*acadoWorkspace.x[0] + acadoWorkspace.evGx[85]*acadoWorkspace.x[1] + acadoWorkspace.evGx[86]*acadoWorkspace.x[2] + acadoWorkspace.d[28];
+acadoVariables.x[32] += + acadoWorkspace.evGx[87]*acadoWorkspace.x[0] + acadoWorkspace.evGx[88]*acadoWorkspace.x[1] + acadoWorkspace.evGx[89]*acadoWorkspace.x[2] + acadoWorkspace.d[29];
+acadoVariables.x[33] += + acadoWorkspace.evGx[90]*acadoWorkspace.x[0] + acadoWorkspace.evGx[91]*acadoWorkspace.x[1] + acadoWorkspace.evGx[92]*acadoWorkspace.x[2] + acadoWorkspace.d[30];
+acadoVariables.x[34] += + acadoWorkspace.evGx[93]*acadoWorkspace.x[0] + acadoWorkspace.evGx[94]*acadoWorkspace.x[1] + acadoWorkspace.evGx[95]*acadoWorkspace.x[2] + acadoWorkspace.d[31];
+acadoVariables.x[35] += + acadoWorkspace.evGx[96]*acadoWorkspace.x[0] + acadoWorkspace.evGx[97]*acadoWorkspace.x[1] + acadoWorkspace.evGx[98]*acadoWorkspace.x[2] + acadoWorkspace.d[32];
+acadoVariables.x[36] += + acadoWorkspace.evGx[99]*acadoWorkspace.x[0] + acadoWorkspace.evGx[100]*acadoWorkspace.x[1] + acadoWorkspace.evGx[101]*acadoWorkspace.x[2] + acadoWorkspace.d[33];
+acadoVariables.x[37] += + acadoWorkspace.evGx[102]*acadoWorkspace.x[0] + acadoWorkspace.evGx[103]*acadoWorkspace.x[1] + acadoWorkspace.evGx[104]*acadoWorkspace.x[2] + acadoWorkspace.d[34];
+acadoVariables.x[38] += + acadoWorkspace.evGx[105]*acadoWorkspace.x[0] + acadoWorkspace.evGx[106]*acadoWorkspace.x[1] + acadoWorkspace.evGx[107]*acadoWorkspace.x[2] + acadoWorkspace.d[35];
+acadoVariables.x[39] += + acadoWorkspace.evGx[108]*acadoWorkspace.x[0] + acadoWorkspace.evGx[109]*acadoWorkspace.x[1] + acadoWorkspace.evGx[110]*acadoWorkspace.x[2] + acadoWorkspace.d[36];
+acadoVariables.x[40] += + acadoWorkspace.evGx[111]*acadoWorkspace.x[0] + acadoWorkspace.evGx[112]*acadoWorkspace.x[1] + acadoWorkspace.evGx[113]*acadoWorkspace.x[2] + acadoWorkspace.d[37];
+acadoVariables.x[41] += + acadoWorkspace.evGx[114]*acadoWorkspace.x[0] + acadoWorkspace.evGx[115]*acadoWorkspace.x[1] + acadoWorkspace.evGx[116]*acadoWorkspace.x[2] + acadoWorkspace.d[38];
+acadoVariables.x[42] += + acadoWorkspace.evGx[117]*acadoWorkspace.x[0] + acadoWorkspace.evGx[118]*acadoWorkspace.x[1] + acadoWorkspace.evGx[119]*acadoWorkspace.x[2] + acadoWorkspace.d[39];
+acadoVariables.x[43] += + acadoWorkspace.evGx[120]*acadoWorkspace.x[0] + acadoWorkspace.evGx[121]*acadoWorkspace.x[1] + acadoWorkspace.evGx[122]*acadoWorkspace.x[2] + acadoWorkspace.d[40];
+acadoVariables.x[44] += + acadoWorkspace.evGx[123]*acadoWorkspace.x[0] + acadoWorkspace.evGx[124]*acadoWorkspace.x[1] + acadoWorkspace.evGx[125]*acadoWorkspace.x[2] + acadoWorkspace.d[41];
+acadoVariables.x[45] += + acadoWorkspace.evGx[126]*acadoWorkspace.x[0] + acadoWorkspace.evGx[127]*acadoWorkspace.x[1] + acadoWorkspace.evGx[128]*acadoWorkspace.x[2] + acadoWorkspace.d[42];
+acadoVariables.x[46] += + acadoWorkspace.evGx[129]*acadoWorkspace.x[0] + acadoWorkspace.evGx[130]*acadoWorkspace.x[1] + acadoWorkspace.evGx[131]*acadoWorkspace.x[2] + acadoWorkspace.d[43];
+acadoVariables.x[47] += + acadoWorkspace.evGx[132]*acadoWorkspace.x[0] + acadoWorkspace.evGx[133]*acadoWorkspace.x[1] + acadoWorkspace.evGx[134]*acadoWorkspace.x[2] + acadoWorkspace.d[44];
+acadoVariables.x[48] += + acadoWorkspace.evGx[135]*acadoWorkspace.x[0] + acadoWorkspace.evGx[136]*acadoWorkspace.x[1] + acadoWorkspace.evGx[137]*acadoWorkspace.x[2] + acadoWorkspace.d[45];
+acadoVariables.x[49] += + acadoWorkspace.evGx[138]*acadoWorkspace.x[0] + acadoWorkspace.evGx[139]*acadoWorkspace.x[1] + acadoWorkspace.evGx[140]*acadoWorkspace.x[2] + acadoWorkspace.d[46];
+acadoVariables.x[50] += + acadoWorkspace.evGx[141]*acadoWorkspace.x[0] + acadoWorkspace.evGx[142]*acadoWorkspace.x[1] + acadoWorkspace.evGx[143]*acadoWorkspace.x[2] + acadoWorkspace.d[47];
+acadoVariables.x[51] += + acadoWorkspace.evGx[144]*acadoWorkspace.x[0] + acadoWorkspace.evGx[145]*acadoWorkspace.x[1] + acadoWorkspace.evGx[146]*acadoWorkspace.x[2] + acadoWorkspace.d[48];
+acadoVariables.x[52] += + acadoWorkspace.evGx[147]*acadoWorkspace.x[0] + acadoWorkspace.evGx[148]*acadoWorkspace.x[1] + acadoWorkspace.evGx[149]*acadoWorkspace.x[2] + acadoWorkspace.d[49];
+acadoVariables.x[53] += + acadoWorkspace.evGx[150]*acadoWorkspace.x[0] + acadoWorkspace.evGx[151]*acadoWorkspace.x[1] + acadoWorkspace.evGx[152]*acadoWorkspace.x[2] + acadoWorkspace.d[50];
+acadoVariables.x[54] += + acadoWorkspace.evGx[153]*acadoWorkspace.x[0] + acadoWorkspace.evGx[154]*acadoWorkspace.x[1] + acadoWorkspace.evGx[155]*acadoWorkspace.x[2] + acadoWorkspace.d[51];
+acadoVariables.x[55] += + acadoWorkspace.evGx[156]*acadoWorkspace.x[0] + acadoWorkspace.evGx[157]*acadoWorkspace.x[1] + acadoWorkspace.evGx[158]*acadoWorkspace.x[2] + acadoWorkspace.d[52];
+acadoVariables.x[56] += + acadoWorkspace.evGx[159]*acadoWorkspace.x[0] + acadoWorkspace.evGx[160]*acadoWorkspace.x[1] + acadoWorkspace.evGx[161]*acadoWorkspace.x[2] + acadoWorkspace.d[53];
+acadoVariables.x[57] += + acadoWorkspace.evGx[162]*acadoWorkspace.x[0] + acadoWorkspace.evGx[163]*acadoWorkspace.x[1] + acadoWorkspace.evGx[164]*acadoWorkspace.x[2] + acadoWorkspace.d[54];
+acadoVariables.x[58] += + acadoWorkspace.evGx[165]*acadoWorkspace.x[0] + acadoWorkspace.evGx[166]*acadoWorkspace.x[1] + acadoWorkspace.evGx[167]*acadoWorkspace.x[2] + acadoWorkspace.d[55];
+acadoVariables.x[59] += + acadoWorkspace.evGx[168]*acadoWorkspace.x[0] + acadoWorkspace.evGx[169]*acadoWorkspace.x[1] + acadoWorkspace.evGx[170]*acadoWorkspace.x[2] + acadoWorkspace.d[56];
+acadoVariables.x[60] += + acadoWorkspace.evGx[171]*acadoWorkspace.x[0] + acadoWorkspace.evGx[172]*acadoWorkspace.x[1] + acadoWorkspace.evGx[173]*acadoWorkspace.x[2] + acadoWorkspace.d[57];
+acadoVariables.x[61] += + acadoWorkspace.evGx[174]*acadoWorkspace.x[0] + acadoWorkspace.evGx[175]*acadoWorkspace.x[1] + acadoWorkspace.evGx[176]*acadoWorkspace.x[2] + acadoWorkspace.d[58];
+acadoVariables.x[62] += + acadoWorkspace.evGx[177]*acadoWorkspace.x[0] + acadoWorkspace.evGx[178]*acadoWorkspace.x[1] + acadoWorkspace.evGx[179]*acadoWorkspace.x[2] + acadoWorkspace.d[59];
+
+acado_multEDu( acadoWorkspace.E, &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 3 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 3 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 6 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 6 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 6 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 9 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 9 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 12 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 9 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 15 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 9 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 18 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 12 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 21 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 12 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 24 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 12 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 27 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 12 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 30 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 15 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 33 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 15 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 36 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 15 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 39 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 15 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 42 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 15 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 45 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 48 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 51 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 54 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 57 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 60 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 18 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 63 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 66 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 69 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 72 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 75 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 78 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 81 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 21 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 84 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 87 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 90 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 93 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 96 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 99 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 102 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 105 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 24 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 108 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 111 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 114 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 117 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 120 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 123 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 126 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 129 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 132 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 27 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 135 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 138 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 141 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 144 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 147 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 150 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 153 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 156 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 159 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 162 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 30 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 165 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 168 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 171 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 174 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 177 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 180 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 183 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 186 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 189 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 192 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 195 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 33 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 198 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 201 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 204 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 207 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 210 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 213 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 216 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 219 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 222 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 225 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 228 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 231 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 36 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 234 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 237 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 240 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 243 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 246 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 249 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 252 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 255 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 258 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 261 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 264 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 267 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 270 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 39 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 273 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 276 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 279 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 282 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 285 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 288 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 291 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 294 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 297 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 300 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 303 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 306 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 309 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 312 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 42 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 315 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 318 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 321 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 324 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 327 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 330 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 333 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 336 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 339 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 342 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 345 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 348 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 351 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 354 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 357 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 45 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 360 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 363 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 366 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 369 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 372 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 375 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 378 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 381 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 384 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 387 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 390 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 393 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 396 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 399 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 402 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 405 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 48 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 408 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 411 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 414 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 417 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 420 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 423 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 426 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 429 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 432 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 435 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 438 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 441 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 444 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 447 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 450 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 453 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 456 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 51 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 459 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 462 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 465 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 468 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 471 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 474 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 477 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 480 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 483 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 486 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 489 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 492 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 495 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 498 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 501 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 504 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 507 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 510 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 54 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 513 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 516 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 519 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 522 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 525 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 528 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 531 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 534 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 537 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 540 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 543 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 546 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 549 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 552 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 555 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 558 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 561 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 564 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 567 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 57 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 570 ]), &(acadoWorkspace.x[ 3 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 573 ]), &(acadoWorkspace.x[ 4 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 576 ]), &(acadoWorkspace.x[ 5 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 579 ]), &(acadoWorkspace.x[ 6 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 582 ]), &(acadoWorkspace.x[ 7 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 585 ]), &(acadoWorkspace.x[ 8 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 588 ]), &(acadoWorkspace.x[ 9 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 591 ]), &(acadoWorkspace.x[ 10 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 594 ]), &(acadoWorkspace.x[ 11 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 597 ]), &(acadoWorkspace.x[ 12 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 600 ]), &(acadoWorkspace.x[ 13 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 603 ]), &(acadoWorkspace.x[ 14 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 606 ]), &(acadoWorkspace.x[ 15 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 609 ]), &(acadoWorkspace.x[ 16 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 612 ]), &(acadoWorkspace.x[ 17 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 615 ]), &(acadoWorkspace.x[ 18 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 618 ]), &(acadoWorkspace.x[ 19 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 621 ]), &(acadoWorkspace.x[ 20 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 624 ]), &(acadoWorkspace.x[ 21 ]), &(acadoVariables.x[ 60 ]) );
+acado_multEDu( &(acadoWorkspace.E[ 627 ]), &(acadoWorkspace.x[ 22 ]), &(acadoVariables.x[ 60 ]) );
}
int acado_preparationStep( )
@@ -5198,24 +4632,18 @@ void acado_initializeNodesByForwardSimulation( )
int index;
for (index = 0; index < 20; ++index)
{
-acadoWorkspace.state[0] = acadoVariables.x[index * 6];
-acadoWorkspace.state[1] = acadoVariables.x[index * 6 + 1];
-acadoWorkspace.state[2] = acadoVariables.x[index * 6 + 2];
-acadoWorkspace.state[3] = acadoVariables.x[index * 6 + 3];
-acadoWorkspace.state[4] = acadoVariables.x[index * 6 + 4];
-acadoWorkspace.state[5] = acadoVariables.x[index * 6 + 5];
-acadoWorkspace.state[48] = acadoVariables.u[index];
-acadoWorkspace.state[49] = acadoVariables.od[index * 2];
-acadoWorkspace.state[50] = acadoVariables.od[index * 2 + 1];
+acadoWorkspace.state[0] = acadoVariables.x[index * 3];
+acadoWorkspace.state[1] = acadoVariables.x[index * 3 + 1];
+acadoWorkspace.state[2] = acadoVariables.x[index * 3 + 2];
+acadoWorkspace.state[15] = acadoVariables.u[index];
+acadoWorkspace.state[16] = acadoVariables.od[index * 2];
+acadoWorkspace.state[17] = acadoVariables.od[index * 2 + 1];
acado_integrate(acadoWorkspace.state, index == 0, index);
-acadoVariables.x[index * 6 + 6] = acadoWorkspace.state[0];
-acadoVariables.x[index * 6 + 7] = acadoWorkspace.state[1];
-acadoVariables.x[index * 6 + 8] = acadoWorkspace.state[2];
-acadoVariables.x[index * 6 + 9] = acadoWorkspace.state[3];
-acadoVariables.x[index * 6 + 10] = acadoWorkspace.state[4];
-acadoVariables.x[index * 6 + 11] = acadoWorkspace.state[5];
+acadoVariables.x[index * 3 + 3] = acadoWorkspace.state[0];
+acadoVariables.x[index * 3 + 4] = acadoWorkspace.state[1];
+acadoVariables.x[index * 3 + 5] = acadoWorkspace.state[2];
}
}
@@ -5224,50 +4652,38 @@ void acado_shiftStates( int strategy, real_t* const xEnd, real_t* const uEnd )
int index;
for (index = 0; index < 20; ++index)
{
-acadoVariables.x[index * 6] = acadoVariables.x[index * 6 + 6];
-acadoVariables.x[index * 6 + 1] = acadoVariables.x[index * 6 + 7];
-acadoVariables.x[index * 6 + 2] = acadoVariables.x[index * 6 + 8];
-acadoVariables.x[index * 6 + 3] = acadoVariables.x[index * 6 + 9];
-acadoVariables.x[index * 6 + 4] = acadoVariables.x[index * 6 + 10];
-acadoVariables.x[index * 6 + 5] = acadoVariables.x[index * 6 + 11];
+acadoVariables.x[index * 3] = acadoVariables.x[index * 3 + 3];
+acadoVariables.x[index * 3 + 1] = acadoVariables.x[index * 3 + 4];
+acadoVariables.x[index * 3 + 2] = acadoVariables.x[index * 3 + 5];
}
if (strategy == 1 && xEnd != 0)
{
-acadoVariables.x[120] = xEnd[0];
-acadoVariables.x[121] = xEnd[1];
-acadoVariables.x[122] = xEnd[2];
-acadoVariables.x[123] = xEnd[3];
-acadoVariables.x[124] = xEnd[4];
-acadoVariables.x[125] = xEnd[5];
+acadoVariables.x[60] = xEnd[0];
+acadoVariables.x[61] = xEnd[1];
+acadoVariables.x[62] = xEnd[2];
}
else if (strategy == 2)
{
-acadoWorkspace.state[0] = acadoVariables.x[120];
-acadoWorkspace.state[1] = acadoVariables.x[121];
-acadoWorkspace.state[2] = acadoVariables.x[122];
-acadoWorkspace.state[3] = acadoVariables.x[123];
-acadoWorkspace.state[4] = acadoVariables.x[124];
-acadoWorkspace.state[5] = acadoVariables.x[125];
+acadoWorkspace.state[0] = acadoVariables.x[60];
+acadoWorkspace.state[1] = acadoVariables.x[61];
+acadoWorkspace.state[2] = acadoVariables.x[62];
if (uEnd != 0)
{
-acadoWorkspace.state[48] = uEnd[0];
+acadoWorkspace.state[15] = uEnd[0];
}
else
{
-acadoWorkspace.state[48] = acadoVariables.u[19];
+acadoWorkspace.state[15] = acadoVariables.u[19];
}
-acadoWorkspace.state[49] = acadoVariables.od[40];
-acadoWorkspace.state[50] = acadoVariables.od[41];
+acadoWorkspace.state[16] = acadoVariables.od[40];
+acadoWorkspace.state[17] = acadoVariables.od[41];
acado_integrate(acadoWorkspace.state, 1, 19);
-acadoVariables.x[120] = acadoWorkspace.state[0];
-acadoVariables.x[121] = acadoWorkspace.state[1];
-acadoVariables.x[122] = acadoWorkspace.state[2];
-acadoVariables.x[123] = acadoWorkspace.state[3];
-acadoVariables.x[124] = acadoWorkspace.state[4];
-acadoVariables.x[125] = acadoWorkspace.state[5];
+acadoVariables.x[60] = acadoWorkspace.state[0];
+acadoVariables.x[61] = acadoWorkspace.state[1];
+acadoVariables.x[62] = acadoWorkspace.state[2];
}
}
@@ -5292,9 +4708,9 @@ real_t kkt;
int index;
real_t prd;
-kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22] + acadoWorkspace.g[23]*acadoWorkspace.x[23] + acadoWorkspace.g[24]*acadoWorkspace.x[24] + acadoWorkspace.g[25]*acadoWorkspace.x[25];
+kkt = + acadoWorkspace.g[0]*acadoWorkspace.x[0] + acadoWorkspace.g[1]*acadoWorkspace.x[1] + acadoWorkspace.g[2]*acadoWorkspace.x[2] + acadoWorkspace.g[3]*acadoWorkspace.x[3] + acadoWorkspace.g[4]*acadoWorkspace.x[4] + acadoWorkspace.g[5]*acadoWorkspace.x[5] + acadoWorkspace.g[6]*acadoWorkspace.x[6] + acadoWorkspace.g[7]*acadoWorkspace.x[7] + acadoWorkspace.g[8]*acadoWorkspace.x[8] + acadoWorkspace.g[9]*acadoWorkspace.x[9] + acadoWorkspace.g[10]*acadoWorkspace.x[10] + acadoWorkspace.g[11]*acadoWorkspace.x[11] + acadoWorkspace.g[12]*acadoWorkspace.x[12] + acadoWorkspace.g[13]*acadoWorkspace.x[13] + acadoWorkspace.g[14]*acadoWorkspace.x[14] + acadoWorkspace.g[15]*acadoWorkspace.x[15] + acadoWorkspace.g[16]*acadoWorkspace.x[16] + acadoWorkspace.g[17]*acadoWorkspace.x[17] + acadoWorkspace.g[18]*acadoWorkspace.x[18] + acadoWorkspace.g[19]*acadoWorkspace.x[19] + acadoWorkspace.g[20]*acadoWorkspace.x[20] + acadoWorkspace.g[21]*acadoWorkspace.x[21] + acadoWorkspace.g[22]*acadoWorkspace.x[22];
kkt = fabs( kkt );
-for (index = 0; index < 26; ++index)
+for (index = 0; index < 23; ++index)
{
prd = acadoWorkspace.y[index];
if (prd > 1e-12)
@@ -5304,7 +4720,7 @@ kkt += fabs(acadoWorkspace.ub[index] * prd);
}
for (index = 0; index < 20; ++index)
{
-prd = acadoWorkspace.y[index + 26];
+prd = acadoWorkspace.y[index + 23];
if (prd > 1e-12)
kkt += fabs(acadoWorkspace.lbA[index] * prd);
else if (prd < -1e-12)
@@ -5326,15 +4742,12 @@ real_t tmpDyN[ 3 ];
for (lRun1 = 0; lRun1 < 20; ++lRun1)
{
-acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 6];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 6 + 1];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 6 + 2];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[lRun1 * 6 + 3];
-acadoWorkspace.objValueIn[4] = acadoVariables.x[lRun1 * 6 + 4];
-acadoWorkspace.objValueIn[5] = acadoVariables.x[lRun1 * 6 + 5];
-acadoWorkspace.objValueIn[6] = acadoVariables.u[lRun1];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[lRun1 * 2];
-acadoWorkspace.objValueIn[8] = acadoVariables.od[lRun1 * 2 + 1];
+acadoWorkspace.objValueIn[0] = acadoVariables.x[lRun1 * 3];
+acadoWorkspace.objValueIn[1] = acadoVariables.x[lRun1 * 3 + 1];
+acadoWorkspace.objValueIn[2] = acadoVariables.x[lRun1 * 3 + 2];
+acadoWorkspace.objValueIn[3] = acadoVariables.u[lRun1];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[lRun1 * 2];
+acadoWorkspace.objValueIn[5] = acadoVariables.od[lRun1 * 2 + 1];
acado_evaluateLSQ( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.Dy[lRun1 * 4] = acadoWorkspace.objValueOut[0] - acadoVariables.y[lRun1 * 4];
@@ -5342,14 +4755,11 @@ acadoWorkspace.Dy[lRun1 * 4 + 1] = acadoWorkspace.objValueOut[1] - acadoVariable
acadoWorkspace.Dy[lRun1 * 4 + 2] = acadoWorkspace.objValueOut[2] - acadoVariables.y[lRun1 * 4 + 2];
acadoWorkspace.Dy[lRun1 * 4 + 3] = acadoWorkspace.objValueOut[3] - acadoVariables.y[lRun1 * 4 + 3];
}
-acadoWorkspace.objValueIn[0] = acadoVariables.x[120];
-acadoWorkspace.objValueIn[1] = acadoVariables.x[121];
-acadoWorkspace.objValueIn[2] = acadoVariables.x[122];
-acadoWorkspace.objValueIn[3] = acadoVariables.x[123];
-acadoWorkspace.objValueIn[4] = acadoVariables.x[124];
-acadoWorkspace.objValueIn[5] = acadoVariables.x[125];
-acadoWorkspace.objValueIn[6] = acadoVariables.od[40];
-acadoWorkspace.objValueIn[7] = acadoVariables.od[41];
+acadoWorkspace.objValueIn[0] = acadoVariables.x[60];
+acadoWorkspace.objValueIn[1] = acadoVariables.x[61];
+acadoWorkspace.objValueIn[2] = acadoVariables.x[62];
+acadoWorkspace.objValueIn[3] = acadoVariables.od[40];
+acadoWorkspace.objValueIn[4] = acadoVariables.od[41];
acado_evaluateLSQEndTerm( acadoWorkspace.objValueIn, acadoWorkspace.objValueOut );
acadoWorkspace.DyN[0] = acadoWorkspace.objValueOut[0] - acadoVariables.yN[0];
acadoWorkspace.DyN[1] = acadoWorkspace.objValueOut[1] - acadoVariables.yN[1];
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o
index 8f2fcfbdf..435fd9f83 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o
index b770e3240..28794f818 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Bounds.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o
index 4d47fe901..81352513f 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Constraints.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o
index ec3e83d11..321db37ef 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/CyclingManager.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o
index 71cbd4ce3..1816ece0e 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/EXTRAS/SolutionAnalysis.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o
index 9ef68d5bf..c8bdbe979 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/Indexlist.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o
index 81a505555..0d79872a2 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblem.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o
index a85043924..9ad890018 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/QProblemB.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o
index 827237a68..f5b1695e7 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o and b/selfdrive/controls/lib/longitudinal_mpc/lib_qp/SubjectTo.o differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so
index 232e9a158..3e05e166d 100755
Binary files a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so and b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so differ
diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
index 36b6f8b7e..354a4a493 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
+++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py
@@ -32,6 +32,7 @@ def _get_libmpc(mpc_id):
double j_ego[21];
double x_l[21];
double v_l[21];
+ double a_l[21];
double t[21];
double cost;
} log_t;
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
index c0b43e53c..e903ccc75 100644
--- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
+++ b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.c
@@ -29,6 +29,7 @@ typedef struct {
double j_ego[N+1];
double x_l[N+1];
double v_l[N+1];
+ double a_l[N+1];
double t[N+1];
double cost;
} log_t;
@@ -68,17 +69,19 @@ void init(double ttcCost, double distanceCost, double accelerationCost, double j
void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0, double l){
int i;
- double x_ego = 0.0;
- double a_ego = 0.0;
double x_l = x_l_0;
double v_l = v_l_0;
double a_l = a_l_0;
+ double x_ego = 0.0;
+ double a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
- if (v_ego > v_l){
- a_ego = -(v_ego - v_l) * (v_ego - v_l) / (2.0 * x_l + 0.01) + a_l;
+ if (a_ego > 0){
+ a_ego = 0.0;
}
+
+
double dt = 0.2;
double t = 0.;
@@ -87,51 +90,66 @@ void init_with_simulation(double v_ego, double x_l_0, double v_l_0, double a_l_0
dt = 0.6;
}
- // Directly calculate a_l to prevent instabilites due to discretization
- a_l = a_l_0 * exp(-l * t * t / 2);
-
- /* printf("x_e: %.2f\t v_e: %.2f\t a_e: %.2f\t", x_ego, v_ego, a_ego); */
- /* printf("x_l: %.2f\t v_l: %.2f\t a_l: %.2f\n", x_l, v_l, a_l); */
-
+ /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_ego, v_ego, a_l); */
acadoVariables.x[i*NX] = x_ego;
acadoVariables.x[i*NX+1] = v_ego;
acadoVariables.x[i*NX+2] = a_ego;
- acadoVariables.x[i*NX+3] = x_l;
- acadoVariables.x[i*NX+4] = v_l;
- acadoVariables.x[i*NX+5] = t;
-
- x_ego += v_ego * dt;
v_ego += a_ego * dt;
- x_l += v_l * dt;
- v_l += a_l * dt;
- t += dt;
-
if (v_ego <= 0.0) {
v_ego = 0.0;
a_ego = 0.0;
}
+
+ x_ego += v_ego * dt;
+ t += dt;
}
+
for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.0;
for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0;
for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0;
}
int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
+ // Calculate lead vehicle predictions
int i;
+ double t = 0.;
+ double dt = 0.2;
+ double x_l = x0->x_l;
+ double v_l = x0->v_l;
+ double a_l = a_l_0;
+
+ /* printf("t\tx_l\t_v_l\t_al\n"); */
+ for (i = 0; i < N + 1; ++i){
+ if (i > 4){
+ dt = 0.6;
+ }
+
+ /* printf("%.2f\t%.2f\t%.2f\t%.2f\n", t, x_l, v_l, a_l); */
+
+ acadoVariables.od[i*NOD] = x_l;
+ acadoVariables.od[i*NOD+1] = v_l;
- for (i = 0; i <= NOD * N; i+= NOD){
- acadoVariables.od[i] = l;
- acadoVariables.od[i+1] = a_l_0;
+ solution->x_l[i] = x_l;
+ solution->v_l[i] = v_l;
+ solution->a_l[i] = a_l;
+ solution->t[i] = t;
+
+ a_l = a_l_0 * exp(-l * t * t / 2);
+ x_l += v_l * dt;
+ v_l += a_l * dt;
+ if (v_l < 0.0){
+ a_l = 0.0;
+ v_l = 0.0;
+ }
+
+ t += dt;
}
acadoVariables.x[0] = acadoVariables.x0[0] = x0->x_ego;
acadoVariables.x[1] = acadoVariables.x0[1] = x0->v_ego;
acadoVariables.x[2] = acadoVariables.x0[2] = x0->a_ego;
- acadoVariables.x[3] = acadoVariables.x0[3] = x0->x_l;
- acadoVariables.x[4] = acadoVariables.x0[4] = x0->v_l;
- acadoVariables.x[5] = acadoVariables.x0[5] = 0.0;
acado_preparationStep();
acado_feedbackStep();
@@ -140,10 +158,6 @@ int run_mpc(state_t * x0, log_t * solution, double l, double a_l_0){
solution->x_ego[i] = acadoVariables.x[i*NX];
solution->v_ego[i] = acadoVariables.x[i*NX+1];
solution->a_ego[i] = acadoVariables.x[i*NX+2];
- solution->x_l[i] = acadoVariables.x[i*NX+3];
- solution->v_l[i] = acadoVariables.x[i*NX+4];
- solution->t[i] = acadoVariables.x[i*NX+5];
-
solution->j_ego[i] = acadoVariables.u[i];
}
solution->cost = acado_getObjective();
diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o
index e899d7b78..3c0660d96 100644
Binary files a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o and b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o differ
diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py
index 55fd2afae..0161d2f94 100644
--- a/selfdrive/controls/lib/pathplanner.py
+++ b/selfdrive/controls/lib/pathplanner.py
@@ -46,15 +46,18 @@ class PathPlanner(object):
self.angle_steers_des_prev = 0.0
self.angle_steers_des_time = 0.0
- def update(self, CP, VM, CS, md, live100):
+ def update(self, CP, VM, CS, md, live100, live_parameters):
v_ego = CS.carState.vEgo
angle_steers = CS.carState.steeringAngle
active = live100.live100.active
- angle_offset = live100.live100.angleOffset
+
+ angle_offset_bias = live100.live100.angleModelBias + live_parameters.liveParameters.angleOffsetAverage
+
self.MP.update(v_ego, md)
# Run MPC
self.angle_steers_des_prev = self.angle_steers_des_mpc
+ VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio)
curvature_factor = VM.curvature_factor(v_ego)
l_poly = libmpc_py.ffi.new("double[4]", list(self.MP.l_poly))
@@ -62,7 +65,7 @@ class PathPlanner(object):
p_poly = libmpc_py.ffi.new("double[4]", list(self.MP.p_poly))
# account for actuation delay
- self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, CP.steerRatio, CP.steerActuatorDelay)
+ self.cur_state = calc_states_after_delay(self.cur_state, v_ego, angle_steers, curvature_factor, VM.sR, CP.steerActuatorDelay)
v_ego_mpc = max(v_ego, 5.0) # avoid mpc roughness due to low speed
self.libmpc.run_mpc(self.cur_state, self.mpc_solution,
@@ -72,19 +75,22 @@ class PathPlanner(object):
# reset to current steer angle if not active or overriding
if active:
delta_desired = self.mpc_solution[0].delta[1]
+ rate_desired = math.degrees(self.mpc_solution[0].rate[0] * VM.sR)
else:
- delta_desired = math.radians(angle_steers - angle_offset) / CP.steerRatio
+ delta_desired = math.radians(angle_steers - angle_offset_bias) / VM.sR
+ rate_desired = 0.0
self.cur_state[0].delta = delta_desired
- self.angle_steers_des_mpc = float(math.degrees(delta_desired * CP.steerRatio) + angle_offset)
+ self.angle_steers_des_mpc = float(math.degrees(delta_desired * VM.sR) + angle_offset_bias)
+
# Check for infeasable MPC solution
mpc_nans = np.any(np.isnan(list(self.mpc_solution[0].delta)))
t = sec_since_boot()
if mpc_nans:
self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, CP.steerRateCost)
- self.cur_state[0].delta = math.radians(angle_steers) / CP.steerRatio
+ self.cur_state[0].delta = math.radians(angle_steers) / VM.sR
if t > self.last_cloudlog_t + 5.0:
self.last_cloudlog_t = t
@@ -108,7 +114,10 @@ class PathPlanner(object):
plan_send.pathPlan.rPoly = map(float, r_poly)
plan_send.pathPlan.rProb = float(self.MP.r_prob)
plan_send.pathPlan.angleSteers = float(self.angle_steers_des_mpc)
+ plan_send.pathPlan.rateSteers = float(rate_desired)
+ plan_send.pathPlan.angleOffset = float(live_parameters.liveParameters.angleOffsetAverage)
plan_send.pathPlan.valid = bool(plan_valid)
+ plan_send.pathPlan.paramsValid = bool(live_parameters.liveParameters.valid)
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py
index e0a53e997..c26711f57 100755
--- a/selfdrive/controls/lib/planner.py
+++ b/selfdrive/controls/lib/planner.py
@@ -2,19 +2,18 @@
import zmq
import math
import numpy as np
-from collections import defaultdict
from common.params import Params
-from common.realtime import sec_since_boot
from common.numpy_fast import interp
+
import selfdrive.messaging as messaging
from selfdrive.swaglog import cloudlog
from selfdrive.config import Conversions as CV
from selfdrive.services import service_list
-from selfdrive.controls.lib.drive_helpers import create_event, MPC_COST_LONG, EventTypes as ET
-from selfdrive.controls.lib.longitudinal_mpc import libmpc_py
+from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET
from selfdrive.controls.lib.speed_smoother import speed_smoother
from selfdrive.controls.lib.longcontrol import LongCtrlState, MIN_CAN_SPEED
-from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
+from selfdrive.controls.lib.fcw import FCWChecker
+from selfdrive.controls.lib.long_mpc import LongitudinalMpc
NO_CURVATURE_SPEED = 200. * CV.MPH_TO_MS
@@ -37,9 +36,6 @@ _A_CRUISE_MAX_BP = [0., 5., 10., 20., 40.]
_A_TOTAL_MAX_V = [1.5, 1.9, 3.2]
_A_TOTAL_MAX_BP = [0., 20., 40.]
-_FCW_A_ACT_V = [-3., -2.]
-_FCW_A_ACT_BP = [0., 30.]
-
def calc_cruise_accel_limits(v_ego, following):
a_cruise_min = interp(v_ego, _A_CRUISE_MIN_BP, _A_CRUISE_MIN_V)
@@ -65,182 +61,6 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
return a_target
-class FCWChecker(object):
- def __init__(self):
- self.reset_lead(0.0)
-
- def reset_lead(self, cur_time):
- self.last_fcw_a = 0.0
- self.v_lead_max = 0.0
- self.lead_seen_t = cur_time
- self.last_fcw_time = 0.0
- self.last_min_a = 0.0
-
- self.counters = defaultdict(lambda: 0)
-
- @staticmethod
- def calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead):
- max_ttc = 5.0
-
- v_rel = v_ego - v_lead
- a_rel = a_ego - a_lead
-
- # assuming that closing gap ARel comes from lead vehicle decel,
- # then limit ARel so that v_lead will get to zero in no sooner than t_decel.
- # This helps underweighting ARel when v_lead is close to zero.
- t_decel = 2.
- a_rel = np.minimum(a_rel, v_lead / t_decel)
-
- # delta of the quadratic equation to solve for ttc
- delta = v_rel**2 + 2 * x_lead * a_rel
-
- # assign an arbitrary high ttc value if there is no solution to ttc
- if delta < 0.1 or (np.sqrt(delta) + v_rel < 0.1):
- ttc = max_ttc
- else:
- ttc = np.minimum(2 * x_lead / (np.sqrt(delta) + v_rel), max_ttc)
- return ttc
-
- def update(self, mpc_solution, cur_time, v_ego, a_ego, x_lead, v_lead, a_lead, y_lead, vlat_lead, fcw_lead, blinkers):
- mpc_solution_a = list(mpc_solution[0].a_ego)
- self.last_min_a = min(mpc_solution_a)
- self.v_lead_max = max(self.v_lead_max, v_lead)
-
- if (fcw_lead > 0.99):
- ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead)
- self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0
- self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0
- self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0
- self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0
- self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33
- self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0
- self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0
- self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0
-
- a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V)
- a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego)
-
- fcw_allowed = all(c >= 10 for c in self.counters.values())
- if (self.last_min_a < -3.0 or a_delta < a_thr) and fcw_allowed and self.last_fcw_time + 5.0 < cur_time:
- self.last_fcw_time = cur_time
- self.last_fcw_a = self.last_min_a
- return True
-
- return False
-
-
-class LongitudinalMpc(object):
- def __init__(self, mpc_id, live_longitudinal_mpc):
- self.live_longitudinal_mpc = live_longitudinal_mpc
- self.mpc_id = mpc_id
-
- self.setup_mpc()
- self.v_mpc = 0.0
- self.v_mpc_future = 0.0
- self.a_mpc = 0.0
- self.v_cruise = 0.0
- self.prev_lead_status = False
- self.prev_lead_x = 0.0
- self.new_lead = False
-
- self.last_cloudlog_t = 0.0
-
- def send_mpc_solution(self, qp_iterations, calculation_time):
- qp_iterations = max(0, qp_iterations)
- dat = messaging.new_message()
- dat.init('liveLongitudinalMpc')
- dat.liveLongitudinalMpc.xEgo = list(self.mpc_solution[0].x_ego)
- dat.liveLongitudinalMpc.vEgo = list(self.mpc_solution[0].v_ego)
- dat.liveLongitudinalMpc.aEgo = list(self.mpc_solution[0].a_ego)
- dat.liveLongitudinalMpc.xLead = list(self.mpc_solution[0].x_l)
- dat.liveLongitudinalMpc.vLead = list(self.mpc_solution[0].v_l)
- dat.liveLongitudinalMpc.cost = self.mpc_solution[0].cost
- dat.liveLongitudinalMpc.aLeadTau = self.a_lead_tau
- dat.liveLongitudinalMpc.qpIterations = qp_iterations
- dat.liveLongitudinalMpc.mpcId = self.mpc_id
- dat.liveLongitudinalMpc.calculationTime = calculation_time
- self.live_longitudinal_mpc.send(dat.to_bytes())
-
- def setup_mpc(self):
- ffi, self.libmpc = libmpc_py.get_libmpc(self.mpc_id)
- self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
- MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
-
- self.mpc_solution = ffi.new("log_t *")
- self.cur_state = ffi.new("state_t *")
- self.cur_state[0].v_ego = 0
- self.cur_state[0].a_ego = 0
- self.a_lead_tau = _LEAD_ACCEL_TAU
-
- def set_cur_state(self, v, a):
- self.cur_state[0].v_ego = v
- self.cur_state[0].a_ego = a
-
- def update(self, CS, lead, v_cruise_setpoint):
- v_ego = CS.carState.vEgo
-
- # Setup current mpc state
- self.cur_state[0].x_ego = 0.0
-
- if lead is not None and lead.status:
- x_lead = lead.dRel
- v_lead = max(0.0, lead.vLead)
- a_lead = lead.aLeadK
-
- if (v_lead < 0.1 or -a_lead / 2.0 > v_lead):
- v_lead = 0.0
- a_lead = 0.0
-
- self.a_lead_tau = max(lead.aLeadTau, (a_lead**2 * math.pi) / (2 * (v_lead + 0.01)**2))
- self.new_lead = False
- if not self.prev_lead_status or abs(x_lead - self.prev_lead_x) > 2.5:
- self.libmpc.init_with_simulation(self.v_mpc, x_lead, v_lead, a_lead, self.a_lead_tau)
- self.new_lead = True
-
- self.prev_lead_status = True
- self.prev_lead_x = x_lead
- self.cur_state[0].x_l = x_lead
- self.cur_state[0].v_l = v_lead
- else:
- self.prev_lead_status = False
- # Fake a fast lead car, so mpc keeps running
- self.cur_state[0].x_l = 50.0
- self.cur_state[0].v_l = v_ego + 10.0
- a_lead = 0.0
- self.a_lead_tau = _LEAD_ACCEL_TAU
-
- # Calculate mpc
- t = sec_since_boot()
- n_its = self.libmpc.run_mpc(self.cur_state, self.mpc_solution, self.a_lead_tau, a_lead)
- duration = int((sec_since_boot() - t) * 1e9)
- self.send_mpc_solution(n_its, duration)
-
- # Get solution. MPC timestep is 0.2 s, so interpolation to 0.05 s is needed
- self.v_mpc = self.mpc_solution[0].v_ego[1]
- self.a_mpc = self.mpc_solution[0].a_ego[1]
- self.v_mpc_future = self.mpc_solution[0].v_ego[10]
-
- # Reset if NaN or goes through lead car
- dls = np.array(list(self.mpc_solution[0].x_l)) - np.array(list(self.mpc_solution[0].x_ego))
- crashing = min(dls) < -50.0
- nans = np.any(np.isnan(list(self.mpc_solution[0].v_ego)))
- backwards = min(list(self.mpc_solution[0].v_ego)) < -0.01
-
- if ((backwards or crashing) and self.prev_lead_status) or nans:
- if t > self.last_cloudlog_t + 5.0:
- self.last_cloudlog_t = t
- cloudlog.warning("Longitudinal mpc %d reset - backwards: %s crashing: %s nan: %s" % (
- self.mpc_id, backwards, crashing, nans))
-
- self.libmpc.init(MPC_COST_LONG.TTC, MPC_COST_LONG.DISTANCE,
- MPC_COST_LONG.ACCELERATION, MPC_COST_LONG.JERK)
- self.cur_state[0].v_ego = v_ego
- self.cur_state[0].a_ego = 0.0
- self.v_mpc = v_ego
- self.a_mpc = CS.carState.aEgo
- self.prev_lead_status = False
-
-
class Planner(object):
def __init__(self, CP, fcw_enabled):
context = zmq.Context()
@@ -250,8 +70,6 @@ class Planner(object):
self.plan = messaging.pub_sock(context, service_list['plan'].port)
self.live_longitudinal_mpc = messaging.pub_sock(context, service_list['liveLongitudinalMpc'].port)
- self.radar_errors = []
-
self.mpc1 = LongitudinalMpc(1, self.live_longitudinal_mpc)
self.mpc2 = LongitudinalMpc(2, self.live_longitudinal_mpc)
@@ -264,19 +82,11 @@ class Planner(object):
self.v_cruise = 0.0
self.a_cruise = 0.0
- self.lead_1 = None
- self.lead_2 = None
-
self.longitudinalPlanSource = 'cruise'
- self.fcw = False
self.fcw_checker = FCWChecker()
self.fcw_enabled = fcw_enabled
self.params = Params()
- self.v_curvature = NO_CURVATURE_SPEED
- self.v_speedlimit = NO_CURVATURE_SPEED
- self.decel_for_turn = False
- self.map_valid = False
def choose_solution(self, v_cruise_setpoint, enabled):
if enabled:
@@ -313,19 +123,15 @@ class Planner(object):
force_slow_decel = live100.live100.forceDecel
v_cruise_setpoint = v_cruise_kph * CV.KPH_TO_MS
- self.last_md_ts = md.logMonoTime
-
- self.radar_errors = list(live20.live20.radarErrors)
-
- self.lead_1 = live20.live20.leadOne
- self.lead_2 = live20.live20.leadTwo
+ lead_1 = live20.live20.leadOne
+ lead_2 = live20.live20.leadTwo
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
- following = self.lead_1.status and self.lead_1.dRel < 45.0 and self.lead_1.vLeadK > v_ego and self.lead_1.aLeadK > 0.0
+ following = lead_1.status and lead_1.dRel < 45.0 and lead_1.vLeadK > v_ego and lead_1.aLeadK > 0.0
- self.v_speedlimit = NO_CURVATURE_SPEED
- self.v_curvature = NO_CURVATURE_SPEED
- self.map_valid = live_map_data.liveMapData.mapValid
+ v_speedlimit = NO_CURVATURE_SPEED
+ v_curvature = NO_CURVATURE_SPEED
+ map_valid = live_map_data.liveMapData.mapValid
# Speed limit and curvature
set_speed_limit_active = self.params.get("LimitSetSpeed") == "1" and self.params.get("SpeedLimitOffset") is not None
@@ -333,16 +139,16 @@ class Planner(object):
if live_map_data.liveMapData.speedLimitValid:
speed_limit = live_map_data.liveMapData.speedLimit
offset = float(self.params.get("SpeedLimitOffset"))
- self.v_speedlimit = speed_limit + offset
+ v_speedlimit = speed_limit + offset
if live_map_data.liveMapData.curvatureValid:
curvature = abs(live_map_data.liveMapData.curvature)
a_y_max = 2.975 - v_ego * 0.0375 # ~1.85 @ 75mph, ~2.6 @ 25mph
v_curvature = math.sqrt(a_y_max / max(1e-4, curvature))
- self.v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
+ v_curvature = min(NO_CURVATURE_SPEED, v_curvature)
- self.decel_for_turn = bool(self.v_curvature < min([v_cruise_setpoint, self.v_speedlimit, v_ego + 1.]))
- v_cruise_setpoint = min([v_cruise_setpoint, self.v_curvature, self.v_speedlimit])
+ decel_for_turn = bool(v_curvature < min([v_cruise_setpoint, v_speedlimit, v_ego + 1.]))
+ v_cruise_setpoint = min([v_cruise_setpoint, v_curvature, v_speedlimit])
# Calculate speed for normal cruise control
if enabled:
@@ -356,9 +162,9 @@ class Planner(object):
accel_limits[0] = min(accel_limits[0], accel_limits[1])
# Change accel limits based on time remaining to turn
- if self.decel_for_turn:
+ if decel_for_turn:
time_to_turn = max(1.0, live_map_data.liveMapData.distToTurn / max(self.v_cruise, 1.))
- required_decel = min(0, (self.v_curvature - self.v_cruise) / time_to_turn)
+ required_decel = min(0, (v_curvature - self.v_cruise) / time_to_turn)
accel_limits[0] = max(accel_limits[0], required_decel)
self.v_cruise, self.a_cruise = speed_smoother(self.v_acc_start, self.a_acc_start,
@@ -383,8 +189,8 @@ class Planner(object):
self.mpc1.set_cur_state(self.v_acc_start, self.a_acc_start)
self.mpc2.set_cur_state(self.v_acc_start, self.a_acc_start)
- self.mpc1.update(CS, self.lead_1, v_cruise_setpoint)
- self.mpc2.update(CS, self.lead_2, v_cruise_setpoint)
+ self.mpc1.update(CS, lead_1, v_cruise_setpoint)
+ self.mpc2.update(CS, lead_2, v_cruise_setpoint)
self.choose_solution(v_cruise_setpoint, enabled)
@@ -393,11 +199,11 @@ class Planner(object):
self.fcw_checker.reset_lead(cur_time)
blinkers = CS.carState.leftBlinker or CS.carState.rightBlinker
- self.fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
- self.lead_1.dRel, self.lead_1.vLead, self.lead_1.aLeadK,
- self.lead_1.yRel, self.lead_1.vLat,
- self.lead_1.fcw, blinkers) and not CS.carState.brakePressed
- if self.fcw:
+ fcw = self.fcw_checker.update(self.mpc1.mpc_solution, cur_time, v_ego, CS.carState.aEgo,
+ lead_1.dRel, lead_1.vLead, lead_1.aLeadK,
+ lead_1.yRel, lead_1.vLat,
+ lead_1.fcw, blinkers) and not CS.carState.brakePressed
+ if fcw:
cloudlog.info("FCW triggered %s", self.fcw_checker.counters)
model_dead = cur_time - (md.logMonoTime / 1e9) > 0.5
@@ -409,8 +215,12 @@ class Planner(object):
# TODO: Move all these events to controlsd. This has nothing to do with planning
events = []
if model_dead:
- events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE]))
- if 'fault' in self.radar_errors:
+ events.append(create_event('modelCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+
+ radar_errors = list(live20.live20.radarErrors)
+ if 'commIssue' in radar_errors:
+ events.append(create_event('radarCommIssue', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
+ if 'fault' in radar_errors:
events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE]))
plan_send.plan.events = events
@@ -428,12 +238,12 @@ class Planner(object):
plan_send.plan.hasLead = self.mpc1.prev_lead_status
plan_send.plan.longitudinalPlanSource = self.longitudinalPlanSource
- plan_send.plan.vCurvature = self.v_curvature
- plan_send.plan.decelForTurn = self.decel_for_turn
- plan_send.plan.mapValid = self.map_valid
+ plan_send.plan.vCurvature = v_curvature
+ plan_send.plan.decelForTurn = decel_for_turn
+ plan_send.plan.mapValid = map_valid
# Send out fcw
- fcw = self.fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
+ fcw = fcw and (self.fcw_enabled or long_control_state != LongCtrlState.off)
plan_send.plan.fcw = fcw
self.plan.send(plan_send.to_bytes())
diff --git a/selfdrive/controls/lib/vehicle_model.py b/selfdrive/controls/lib/vehicle_model.py
index 95e3ef378..1dc77bf37 100755
--- a/selfdrive/controls/lib/vehicle_model.py
+++ b/selfdrive/controls/lib/vehicle_model.py
@@ -102,11 +102,18 @@ class VehicleModel(object):
self.l = CP.wheelbase
self.aF = CP.centerToFront
self.aR = CP.wheelbase - CP.centerToFront
- self.cF = CP.tireStiffnessFront
- self.cR = CP.tireStiffnessRear
- self.sR = CP.steerRatio
self.chi = CP.steerRatioRear
+ self.cF_orig = CP.tireStiffnessFront
+ self.cR_orig = CP.tireStiffnessRear
+ self.update_params(1.0, CP.steerRatio)
+
+ def update_params(self, stiffness_factor, steer_ratio):
+ """Update the vehicle model with a new stiffness factor and steer ratio"""
+ self.cF = stiffness_factor * self.cF_orig
+ self.cR = stiffness_factor * self.cR_orig
+ self.sR = steer_ratio
+
def steady_state_sol(self, sa, u):
"""Returns the steady state solution.
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index c6f9040b2..2a9537540 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -33,6 +33,7 @@ def plannerd_thread():
live20_sock = messaging.sub_sock(context, service_list['live20'].port, conflate=True, poller=poller)
model_sock = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live_map_data_sock = messaging.sub_sock(context, service_list['liveMapData'].port, conflate=True, poller=poller)
+ live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
car_state = messaging.new_message()
car_state.init('carState')
@@ -45,15 +46,23 @@ def plannerd_thread():
live_map_data = messaging.new_message()
live_map_data.init('liveMapData')
+ live_parameters = messaging.new_message()
+ live_parameters.init('liveParameters')
+ live_parameters.liveParameters.valid = True
+ live_parameters.liveParameters.steerRatio = CP.steerRatio
+ live_parameters.liveParameters.stiffnessFactor = 1.0
+
while True:
for socket, event in poller.poll():
if socket is live100_sock:
live100 = messaging.recv_one(socket)
elif socket is car_state_sock:
car_state = messaging.recv_one(socket)
+ elif socket is live_parameters_sock:
+ live_parameters = messaging.recv_one(socket)
elif socket is model_sock:
model = messaging.recv_one(socket)
- PP.update(CP, VM, car_state, model, live100)
+ PP.update(CP, VM, car_state, model, live100, live_parameters)
elif socket is live_map_data_sock:
live_map_data = messaging.recv_one(socket)
elif socket is live20_sock:
diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py
index 3952a913a..4d44c96b9 100755
--- a/selfdrive/controls/radard.py
+++ b/selfdrive/controls/radard.py
@@ -3,8 +3,9 @@ import zmq
import numpy as np
import numpy.matlib
import importlib
-from collections import defaultdict
+from collections import defaultdict, deque
from fastcluster import linkage_vector
+
import selfdrive.messaging as messaging
from selfdrive.services import service_list
from selfdrive.controls.lib.latcontrol_helpers import calc_lookahead_offset
@@ -65,6 +66,14 @@ def radard_thread(gctx=None):
poller = zmq.Poller()
model = messaging.sub_sock(context, service_list['model'].port, conflate=True, poller=poller)
live100 = messaging.sub_sock(context, service_list['live100'].port, conflate=True, poller=poller)
+ live_parameters_sock = messaging.sub_sock(context, service_list['liveParameters'].port, conflate=True, poller=poller)
+
+ # Default parameters
+ live_parameters = messaging.new_message()
+ live_parameters.init('liveParameters')
+ live_parameters.liveParameters.valid = True
+ live_parameters.liveParameters.steerRatio = CP.steerRatio
+ live_parameters.liveParameters.stiffnessFactor = 1.0
MP = ModelParser()
RI = RadarInterface(CP)
@@ -95,7 +104,8 @@ def radard_thread(gctx=None):
# v_ego
v_ego = None
- v_ego_array = np.zeros([2, v_len])
+ v_ego_hist_t = deque(maxlen=v_len)
+ v_ego_hist_v = deque(maxlen=v_len)
v_ego_t_aligned = 0.
rk = Ratekeeper(rate, print_delay_threshold=np.inf)
@@ -115,6 +125,9 @@ def radard_thread(gctx=None):
l100 = messaging.recv_one(socket)
elif socket is model:
md = messaging.recv_one(socket)
+ elif socket is live_parameters_sock:
+ live_parameters = messaging.recv_one(socket)
+ VM.update_params(live_parameters.liveParameters.stiffnessFactor, live_parameters.liveParameters.steerRatio)
if l100 is not None:
active = l100.live100.active
@@ -122,8 +135,8 @@ def radard_thread(gctx=None):
steer_angle = l100.live100.angleSteers
steer_override = l100.live100.steerOverride
- v_ego_array = np.append(v_ego_array, [[v_ego], [float(rk.frame)/rate]], 1)
- v_ego_array = v_ego_array[:, 1:]
+ v_ego_hist_v.append(v_ego)
+ v_ego_hist_t.append(float(rk.frame)/rate)
last_l100_ts = l100.logMonoTime
@@ -165,7 +178,7 @@ def radard_thread(gctx=None):
path_y = np.polyval(MP.d_poly, path_x)
else:
# use path from steer, set angle_offset to 0 it does not only report the physical offset
- path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=0)[0]
+ path_y = calc_lookahead_offset(v_ego, steer_angle, path_x, VM, angle_offset=live_parameters.liveParameters.angleOffsetAverage)[0]
# *** remove missing points from meta data ***
for ids in tracks.keys():
@@ -181,7 +194,8 @@ def radard_thread(gctx=None):
# align v_ego by a fixed time to align it with the radar measurement
cur_time = float(rk.frame)/rate
- v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_array[1], v_ego_array[0])
+ v_ego_t_aligned = np.interp(cur_time - RI.delay, v_ego_hist_t, v_ego_hist_v)
+
d_path = np.sqrt(np.amin((path_x - rpt[0]) ** 2 + (path_y - rpt[1]) ** 2))
# add sign
d_path *= np.sign(rpt[1] - np.interp(rpt[0], path_x, path_y))
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index 20003ee9f..94bdfd125 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -1,6 +1,7 @@
"""Install exception handler for process crash."""
import os
import sys
+import threading
from selfdrive.version import version, dirty
from selfdrive.swaglog import cloudlog
@@ -38,3 +39,25 @@ else:
capture_exception(exc_info=exc_info)
__excepthook__(*exc_info)
sys.excepthook = handle_exception
+
+ """
+ Workaround for `sys.excepthook` thread bug from:
+ http://bugs.python.org/issue1230540
+ Call once from the main thread before creating any threads.
+ Source: https://stackoverflow.com/a/31622038
+ """
+ init_original = threading.Thread.__init__
+
+ def init(self, *args, **kwargs):
+ init_original(self, *args, **kwargs)
+ run_original = self.run
+
+ def run_with_except_hook(*args2, **kwargs2):
+ try:
+ run_original(*args2, **kwargs2)
+ except Exception:
+ sys.excepthook(*sys.exc_info())
+
+ self.run = run_with_except_hook
+
+ threading.Thread.__init__ = init
diff --git a/selfdrive/locationd/kalman/__init__.py b/selfdrive/locationd/kalman/__init__.py
new file mode 100644
index 000000000..e69de29bb
diff --git a/selfdrive/locationd/kalman/ekf_c.c b/selfdrive/locationd/kalman/ekf_c.c
new file mode 100644
index 000000000..1331036de
--- /dev/null
+++ b/selfdrive/locationd/kalman/ekf_c.c
@@ -0,0 +1,124 @@
+#include
+#include
+
+typedef Eigen::Matrix DDM;
+typedef Eigen::Matrix EEM;
+typedef Eigen::Matrix DEM;
+
+void predict(double *in_x, double *in_P, double *in_Q, double dt) {
+ typedef Eigen::Matrix RRM;
+
+ double nx[DIM] = {0};
+ double in_F[EDIM*EDIM] = {0};
+
+ // functions from sympy
+ f_fun(in_x, dt, nx);
+ F_fun(in_x, dt, in_F);
+
+
+ EEM F(in_F);
+ EEM P(in_P);
+ EEM Q(in_Q);
+
+ RRM F_main = F.topLeftCorner(MEDIM, MEDIM);
+ P.topLeftCorner(MEDIM, MEDIM) = (F_main * P.topLeftCorner(MEDIM, MEDIM)) * F_main.transpose();
+ P.topRightCorner(MEDIM, EDIM - MEDIM) = F_main * P.topRightCorner(MEDIM, EDIM - MEDIM);
+ P.bottomLeftCorner(EDIM - MEDIM, MEDIM) = P.bottomLeftCorner(EDIM - MEDIM, MEDIM) * F_main.transpose();
+
+ P = P + dt*Q;
+
+ // copy out state
+ memcpy(in_x, nx, DIM * sizeof(double));
+ memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+}
+
+// note: extra_args dim only correct when null space projecting
+// otherwise 1
+template
+void update(double *in_x, double *in_P, Hfun h_fun, Hfun H_fun, Hfun Hea_fun, double *in_z, double *in_R, double *in_ea, double MAHA_THRESHOLD) {
+ typedef Eigen::Matrix ZZM;
+ typedef Eigen::Matrix ZDM;
+ typedef Eigen::Matrix ZEM;
+ typedef Eigen::Matrix XEM;
+ typedef Eigen::Matrix EZM;
+ typedef Eigen::Matrix X1M;
+ typedef Eigen::Matrix XXM;
+
+ double in_hx[ZDIM] = {0};
+ double in_H[ZDIM * DIM] = {0};
+ double in_H_mod[EDIM * DIM] = {0};
+ double delta_x[EDIM] = {0};
+ double x_new[DIM] = {0};
+
+
+ // state x, P
+ Eigen::Matrix z(in_z);
+ EEM P(in_P);
+ ZZM pre_R(in_R);
+
+ // functions from sympy
+ h_fun(in_x, in_ea, in_hx);
+ H_fun(in_x, in_ea, in_H);
+ ZDM pre_H(in_H);
+
+ // get y (y = z - hx)
+ Eigen::Matrix pre_y(in_hx); pre_y = z - pre_y;
+ X1M y; XXM H; XXM R;
+ if (Hea_fun){
+ typedef Eigen::Matrix ZAM;
+ double in_Hea[ZDIM * EADIM] = {0};
+ Hea_fun(in_x, in_ea, in_Hea);
+ ZAM Hea(in_Hea);
+ XXM A = Hea.transpose().fullPivLu().kernel();
+
+
+ y = A.transpose() * pre_y;
+ H = A.transpose() * pre_H;
+ R = A.transpose() * pre_R * A;
+ } else {
+ y = pre_y;
+ H = pre_H;
+ R = pre_R;
+ }
+ // get modified H
+ H_mod_fun(in_x, in_H_mod);
+ DEM H_mod(in_H_mod);
+ XEM H_err = H * H_mod;
+
+ // Do mahalobis distance test
+ if (MAHA_TEST){
+ XXM a = (H_err * P * H_err.transpose() + R).inverse();
+ double maha_dist = y.transpose() * a * y;
+ if (maha_dist > MAHA_THRESHOLD){
+ R = 1.0e16 * R;
+ }
+ }
+
+ // Outlier resilient weighting
+ double weight = 1;//(1.5)/(1 + y.squaredNorm()/R.sum());
+
+ // kalman gains and I_KH
+ XXM S = ((H_err * P) * H_err.transpose()) + R/weight;
+ XEM KT = S.fullPivLu().solve(H_err * P.transpose());
+ //EZM K = KT.transpose(); TODO: WHY DOES THIS NOT COMPILE?
+ //EZM K = S.fullPivLu().solve(H_err * P.transpose()).transpose();
+ //std::cout << "Here is the matrix rot:\n" << K << std::endl;
+ EEM I_KH = Eigen::Matrix::Identity() - (KT.transpose() * H_err);
+
+ // update state by injecting dx
+ Eigen::Matrix dx(delta_x);
+ dx = (KT.transpose() * y);
+ memcpy(delta_x, dx.data(), EDIM * sizeof(double));
+ err_fun(in_x, delta_x, x_new);
+ Eigen::Matrix x(x_new);
+
+ // update cov
+ P = ((I_KH * P) * I_KH.transpose()) + ((KT.transpose() * R) * KT);
+
+ // copy out state
+ memcpy(in_x, x.data(), DIM * sizeof(double));
+ memcpy(in_P, P.data(), EDIM * EDIM * sizeof(double));
+ memcpy(in_z, y.data(), y.rows() * sizeof(double));
+}
+
+
diff --git a/selfdrive/locationd/kalman/ekf_sym.py b/selfdrive/locationd/kalman/ekf_sym.py
new file mode 100644
index 000000000..2dfefe92d
--- /dev/null
+++ b/selfdrive/locationd/kalman/ekf_sym.py
@@ -0,0 +1,564 @@
+import os
+from bisect import bisect_right
+import sympy as sp
+import numpy as np
+from numpy import dot
+from common.ffi_wrapper import compile_code, wrap_compiled
+from common.sympy_helpers import sympy_into_c
+import scipy
+from scipy.stats import chi2
+
+
+EXTERNAL_PATH = os.path.dirname(os.path.abspath(__file__))
+
+def solve(a, b):
+ if a.shape[0] == 1 and a.shape[1] == 1:
+ #assert np.allclose(b/a[0][0], np.linalg.solve(a, b))
+ return b/a[0][0]
+ else:
+ return np.linalg.solve(a, b)
+
+def null(H, eps=1e-12):
+ from scipy import linalg
+ u, s, vh = linalg.svd(H)
+ padding = max(0,np.shape(H)[1]-np.shape(s)[0])
+ null_mask = np.concatenate(((s <= eps), np.ones((padding,),dtype=bool)),axis=0)
+ null_space = scipy.compress(null_mask, vh, axis=0)
+ return scipy.transpose(null_space)
+
+def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=None, msckf_params=None, maha_test_kinds=[]):
+ # optional state transition matrix, H modifier
+ # and err_function if an error-state kalman filter (ESKF)
+ # is desired. Best described in "Quaternion kinematics
+ # for the error-state Kalman filter" by Joan Sola
+
+ if eskf_params:
+ err_eqs = eskf_params[0]
+ inv_err_eqs = eskf_params[1]
+ H_mod_sym = eskf_params[2]
+ f_err_sym = eskf_params[3]
+ x_err_sym = eskf_params[4]
+ else:
+ nom_x = sp.MatrixSymbol('nom_x',dim_x,1)
+ true_x = sp.MatrixSymbol('true_x',dim_x,1)
+ delta_x = sp.MatrixSymbol('delta_x',dim_x,1)
+ err_function_sym = sp.Matrix(nom_x + delta_x)
+ inv_err_function_sym = sp.Matrix(true_x - nom_x)
+ err_eqs = [err_function_sym, nom_x, delta_x]
+ inv_err_eqs = [inv_err_function_sym, nom_x, true_x]
+
+ H_mod_sym = sp.Matrix(np.eye(dim_x))
+ f_err_sym = f_sym
+ x_err_sym = x_sym
+
+ # This configures the multi-state augmentation
+ # needed for EKF-SLAM with MSCKF (Mourikis et al 2007)
+ if msckf_params:
+ msckf = True
+ dim_main = msckf_params[0] # size of the main state
+ dim_augment = msckf_params[1] # size of one augment state chunk
+ dim_main_err = msckf_params[2]
+ dim_augment_err = msckf_params[3]
+ N = msckf_params[4]
+ feature_track_kinds = msckf_params[5]
+ assert dim_main + dim_augment*N == dim_x
+ assert dim_main_err + dim_augment_err*N == dim_err
+ else:
+ msckf = False
+ dim_main = dim_x
+ dim_augment = 0
+ dim_main_err = dim_err
+ dim_augment_err = 0
+ N = 0
+
+ # linearize with jacobians
+ F_sym = f_err_sym.jacobian(x_err_sym)
+ for sym in x_err_sym:
+ F_sym = F_sym.subs(sym, 0)
+ for i in xrange(len(obs_eqs)):
+ obs_eqs[i].append(obs_eqs[i][0].jacobian(x_sym))
+ if msckf and obs_eqs[i][1] in feature_track_kinds:
+ obs_eqs[i].append(obs_eqs[i][0].jacobian(obs_eqs[i][2]))
+ else:
+ obs_eqs[i].append(None)
+
+ # collect sympy functions
+ sympy_functions = []
+
+ # error functions
+ sympy_functions.append(('err_fun', err_eqs[0], [err_eqs[1], err_eqs[2]]))
+ sympy_functions.append(('inv_err_fun', inv_err_eqs[0], [inv_err_eqs[1], inv_err_eqs[2]]))
+
+ # H modifier for ESKF updates
+ sympy_functions.append(('H_mod_fun', H_mod_sym, [x_sym]))
+
+ # state propagation function
+ sympy_functions.append(('f_fun', f_sym, [x_sym, dt_sym]))
+ sympy_functions.append(('F_fun', F_sym, [x_sym, dt_sym]))
+
+ # observation functions
+ for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
+ sympy_functions.append(('h_%d' % kind, h_sym, [x_sym, ea_sym]))
+ sympy_functions.append(('H_%d' % kind, H_sym, [x_sym, ea_sym]))
+ if msckf and kind in feature_track_kinds:
+ sympy_functions.append(('He_%d' % kind, He_sym, [x_sym, ea_sym]))
+
+ # Generate and wrap all th c code
+ header, code = sympy_into_c(sympy_functions)
+ extra_header = "#define DIM %d\n" % dim_x
+ extra_header += "#define EDIM %d\n" % dim_err
+ extra_header += "#define MEDIM %d\n" % dim_main_err
+ extra_header += "typedef void (*Hfun)(double *, double *, double *);\n"
+
+ extra_header += "\nvoid predict(double *x, double *P, double *Q, double dt);"
+
+ extra_post = ""
+
+ for h_sym, kind, ea_sym, H_sym, He_sym in obs_eqs:
+ if msckf and kind in feature_track_kinds:
+ He_str = 'He_%d' % kind
+ # ea_dim = ea_sym.shape[0]
+ else:
+ He_str = 'NULL'
+ # ea_dim = 1 # not really dim of ea but makes c function work
+ maha_thresh = chi2.ppf(0.95, int(h_sym.shape[0])) # mahalanobis distance for outlier detection
+ maha_test = kind in maha_test_kinds
+ extra_post += """
+ void update_%d(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
+ update<%d,%d,%d>(in_x, in_P, h_%d, H_%d, %s, in_z, in_R, in_ea, MAHA_THRESH_%d);
+ }
+ """ % (kind, h_sym.shape[0], 3, maha_test, kind, kind, He_str, kind)
+ extra_header += "\nconst static double MAHA_THRESH_%d = %f;" % (kind, maha_thresh)
+ extra_header += "\nvoid update_%d(double *, double *, double *, double *, double *);" % kind
+
+ code += "\n" + extra_header
+ code += "\n" + open(os.path.join(EXTERNAL_PATH, "ekf_c.c")).read()
+ code += "\n" + extra_post
+ header += "\n" + extra_header
+ compile_code(name, code, header, EXTERNAL_PATH)
+
+class EKF_sym(object):
+ def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
+ N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
+ '''
+ Generates process function and all
+ observation functions for the kalman
+ filter.
+ '''
+ if N > 0:
+ self.msckf = True
+ else:
+ self.msckf = False
+ self.N = N
+ self.dim_augment = dim_augment
+ self.dim_augment_err = dim_augment_err
+ self.dim_main = dim_main
+ self.dim_main_err = dim_main_err
+
+ # state
+ x_initial = x_initial.reshape((-1, 1))
+ self.dim_x = x_initial.shape[0]
+ self.dim_err = P_initial.shape[0]
+ assert dim_main + dim_augment*N == self.dim_x
+ assert dim_main_err + dim_augment_err*N == self.dim_err
+
+ # kinds that should get mahalanobis distance
+ # tested for outlier rejection
+ self.maha_test_kinds = maha_test_kinds
+
+ # process noise
+ self.Q = Q
+
+ # rewind stuff
+ self.rewind_t = []
+ self.rewind_states = []
+ self.rewind_obscache = []
+ self.init_state(x_initial, P_initial, None)
+
+ ffi, lib = wrap_compiled(name, EXTERNAL_PATH)
+ kinds, self.feature_track_kinds = [], []
+ for func in dir(lib):
+ if func[:2] == 'h_':
+ kinds.append(int(func[2:]))
+ if func[:3] == 'He_':
+ self.feature_track_kinds.append(int(func[3:]))
+
+ # wrap all the sympy functions
+ def wrap_1lists(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+ def wrap_2lists(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, lst2, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double *", lst2.ctypes.data),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+ def wrap_1list_1float(name):
+ func = eval("lib.%s" % name, {"lib":lib})
+ def ret(lst1, fl, out):
+ func(ffi.cast("double *", lst1.ctypes.data),
+ ffi.cast("double", fl),
+ ffi.cast("double *", out.ctypes.data))
+ return ret
+
+ self.f = wrap_1list_1float("f_fun")
+ self.F = wrap_1list_1float("F_fun")
+
+ self.err_function = wrap_2lists("err_fun")
+ self.inv_err_function = wrap_2lists("inv_err_fun")
+ self.H_mod = wrap_1lists("H_mod_fun")
+
+ self.hs, self.Hs, self.Hes = {}, {}, {}
+ for kind in kinds:
+ self.hs[kind] = wrap_2lists("h_%d" % kind)
+ self.Hs[kind] = wrap_2lists("H_%d" % kind)
+ if self.msckf and kind in self.feature_track_kinds:
+ self.Hes[kind] = wrap_2lists("He_%d" % kind)
+
+ # wrap the C++ predict function
+ def _predict_blas(x, P, dt):
+ lib.predict(ffi.cast("double *", x.ctypes.data),
+ ffi.cast("double *", P.ctypes.data),
+ ffi.cast("double *", self.Q.ctypes.data),
+ ffi.cast("double", dt))
+ return x, P
+
+ # wrap the C++ update function
+ def fun_wrapper(f, kind):
+ f = eval("lib.%s" % f, {"lib": lib})
+ def _update_inner_blas(x, P, z, R, extra_args):
+ f(ffi.cast("double *", x.ctypes.data),
+ ffi.cast("double *", P.ctypes.data),
+ ffi.cast("double *", z.ctypes.data),
+ ffi.cast("double *", R.ctypes.data),
+ ffi.cast("double *", extra_args.ctypes.data))
+ if self.msckf and kind in self.feature_track_kinds:
+ y = z[:-len(extra_args)]
+ else:
+ y = z
+ return x, P, y
+ return _update_inner_blas
+
+ self._updates = {}
+ for kind in kinds:
+ self._updates[kind] = fun_wrapper("update_%d" % kind, kind)
+
+ def _update_blas(x, P, kind, z, R, extra_args=[]):
+ return self._updates[kind](x, P, z, R, extra_args)
+
+ # assign the functions
+ self._predict = _predict_blas
+ #self._predict = self._predict_python
+ self._update = _update_blas
+ #self._update = self._update_python
+
+
+ def init_state(self, state, covs, filter_time):
+ self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
+ self.P = np.array(covs).astype(np.float64)
+ self.filter_time = filter_time
+ self.augment_times = [0]*self.N
+ self.rewind_obscache = []
+ self.rewind_t = []
+ self.rewind_states = []
+
+ def augment(self):
+ # TODO this is not a generalized way of doing
+ # this and implies that the augmented states
+ # are simply the first (dim_augment_state)
+ # elements of the main state.
+ assert self.msckf
+ d1 = self.dim_main
+ d2 = self.dim_main_err
+ d3 = self.dim_augment
+ d4 = self.dim_augment_err
+ # push through augmented states
+ self.x[d1:-d3] = self.x[d1+d3:]
+ self.x[-d3:] = self.x[:d3]
+ assert self.x.shape == (self.dim_x, 1)
+ # push through augmented covs
+ assert self.P.shape == (self.dim_err, self.dim_err)
+ P_reduced = self.P
+ P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=1)
+ P_reduced = np.delete(P_reduced, np.s_[d2:d2+d4], axis=0)
+ assert P_reduced.shape == (self.dim_err -d4, self.dim_err -d4)
+ to_mult = np.zeros((self.dim_err, self.dim_err - d4))
+ to_mult[:-d4,:] = np.eye(self.dim_err - d4)
+ to_mult[-d4:,:d4] = np.eye(d4)
+ self.P = to_mult.dot(P_reduced.dot(to_mult.T))
+ self.augment_times = self.augment_times[1:]
+ self.augment_times.append(self.filter_time)
+ assert self.P.shape == (self.dim_err, self.dim_err)
+
+ def state(self):
+ return np.array(self.x).flatten()
+
+ def covs(self):
+ return self.P
+
+ def rewind(self, t):
+ # find where we are rewinding to
+ idx = bisect_right(self.rewind_t, t)
+ assert self.rewind_t[idx-1] <= t
+ assert self.rewind_t[idx] > t # must be true, or rewind wouldn't be called
+
+ # set the state to the time right before that
+ self.filter_time = self.rewind_t[idx-1]
+ self.x[:] = self.rewind_states[idx-1][0]
+ self.P[:] = self.rewind_states[idx-1][1]
+
+ # return the observations we rewound over for fast forwarding
+ ret = self.rewind_obscache[idx:]
+
+ # throw away the old future
+ # TODO: is this making a copy?
+ self.rewind_t = self.rewind_t[:idx]
+ self.rewind_states = self.rewind_states[:idx]
+ self.rewind_obscache = self.rewind_obscache[:idx]
+
+ return ret
+
+ def checkpoint(self, obs):
+ # push to rewinder
+ self.rewind_t.append(self.filter_time)
+ self.rewind_states.append((np.copy(self.x), np.copy(self.P)))
+ self.rewind_obscache.append(obs)
+
+ # only keep a certain number around
+ REWIND_TO_KEEP = 512
+ self.rewind_t = self.rewind_t[-REWIND_TO_KEEP:]
+ self.rewind_states = self.rewind_states[-REWIND_TO_KEEP:]
+ self.rewind_obscache = self.rewind_obscache[-REWIND_TO_KEEP:]
+
+ def predict_and_update_batch(self, t, kind, z, R, extra_args=[[]], augment=False):
+ # TODO handle rewinding at this level"
+
+ # rewind
+ if t < self.filter_time:
+ if len(self.rewind_t) == 0 or t < self.rewind_t[0] or t < self.rewind_t[-1] -1.0:
+ print "observation too old at %.3f with filter at %.3f, ignoring" % (t, self.filter_time)
+ return None
+ rewound = self.rewind(t)
+ else:
+ rewound = []
+
+ ret = self._predict_and_update_batch(t, kind, z, R, extra_args, augment)
+
+ # optional fast forward
+ for r in rewound:
+ self._predict_and_update_batch(*r)
+
+ return ret
+
+ def _predict_and_update_batch(self, t, kind, z, R, extra_args, augment=False):
+ """The main kalman filter function
+ Predicts the state and then updates a batch of observations
+
+ dim_x: dimensionality of the state space
+ dim_z: dimensionality of the observation and depends on kind
+ n: number of observations
+
+ Args:
+ t (float): Time of observation
+ kind (int): Type of observation
+ z (vec [n,dim_z]): Measurements
+ R (mat [n,dim_z, dim_z]): Measurement Noise
+ extra_args (list, [n]): Values used in H computations
+ """
+ # initialize time
+ if self.filter_time is None:
+ self.filter_time = t
+
+ # predict
+ dt = t - self.filter_time
+ assert dt >= 0
+ self.x, self.P = self._predict(self.x, self.P, dt)
+ self.filter_time = t
+ xk_km1, Pk_km1 = np.copy(self.x).flatten(), np.copy(self.P)
+
+ # update batch
+ y = []
+ for i in xrange(len(z)):
+ # these are from the user, so we canonicalize them
+ z_i = np.array(z[i], dtype=np.float64, order='F')
+ R_i = np.array(R[i], dtype=np.float64, order='F')
+ extra_args_i = np.array(extra_args[i], dtype=np.float64, order='F')
+ # update
+ self.x, self.P, y_i = self._update(self.x, self.P, kind, z_i, R_i, extra_args=extra_args_i)
+ y.append(y_i)
+ xk_k, Pk_k = np.copy(self.x).flatten(), np.copy(self.P)
+
+ if augment:
+ self.augment()
+
+ # checkpoint
+ self.checkpoint((t, kind, z, R, extra_args))
+
+ return xk_km1, xk_k, Pk_km1, Pk_k, t, kind, y, z, extra_args
+
+ def _predict_python(self, x, P, dt):
+ x_new = np.zeros(x.shape, dtype=np.float64)
+ self.f(x, dt, x_new)
+
+ F = np.zeros(P.shape, dtype=np.float64)
+ self.F(x, dt, F)
+
+ if not self.msckf:
+ P = dot(dot(F, P), F.T)
+ else:
+ # Update the predicted state covariance:
+ # Pk+1|k = |F*Pii*FT + Q*dt F*Pij |
+ # |PijT*FT Pjj |
+ # Where F is the jacobian of the main state
+ # predict function, Pii is the main state's
+ # covariance and Q its process noise. Pij
+ # is the covariance between the augmented
+ # states and the main state.
+ #
+ d2 = self.dim_main_err # known at compile time
+ F_curr = F[:d2, :d2]
+ P[:d2, :d2] = (F_curr.dot(P[:d2, :d2])).dot(F_curr.T)
+ P[:d2, d2:] = F_curr.dot(P[:d2, d2:])
+ P[d2:, :d2] = P[d2:, :d2].dot(F_curr.T)
+
+ P += dt*self.Q
+ return x_new, P
+
+ def _update_python(self, x, P, kind, z, R, extra_args=[]):
+ # init vars
+ z = z.reshape((-1, 1))
+ h = np.zeros(z.shape, dtype=np.float64)
+ H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+ # C functions
+ self.hs[kind](x, extra_args, h)
+ self.Hs[kind](x, extra_args, H)
+
+ # y is the "loss"
+ y = z - h
+
+ # *** same above this line ***
+
+ if self.msckf and kind in self.Hes:
+ # Do some algebraic magic to decorrelate
+ He = np.zeros((z.shape[0], len(extra_args)), dtype=np.float64)
+ self.Hes[kind](x, extra_args, He)
+
+ # TODO: Don't call a function here, do projection locally
+ A = null(He.T)
+
+ y = A.T.dot(y)
+ H = A.T.dot(H)
+ R = A.T.dot(R.dot(A))
+
+ # TODO If nullspace isn't the dimension we want
+ if A.shape[1] + He.shape[1] != A.shape[0]:
+ print 'Warning: null space projection failed, measurement ignored'
+ return x, P, np.zeros(A.shape[0] - He.shape[1])
+
+ # if using eskf
+ H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+ self.H_mod(x, H_mod)
+ H = H.dot(H_mod)
+
+ # Do mahalobis distance test
+ # currently just runs on msckf observations
+ # could run on anything if needed
+ if self.msckf and kind in self.maha_test_kinds:
+ a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+ maha_dist = y.T.dot(a.dot(y))
+ if maha_dist > chi2.ppf(0.95, y.shape[0]):
+ R = 10e16*R
+
+ # *** same below this line ***
+
+ # Outlier resilient weighting as described in:
+ # "A Kalman Filter for Robust Outlier Detection - Jo-Anne Ting, ..."
+ weight = 1 #(1.5)/(1 + np.sum(y**2)/np.sum(R))
+
+ S = dot(dot(H, P), H.T) + R/weight
+ K = solve(S, dot(H, P.T)).T
+ I_KH = np.eye(P.shape[0]) - dot(K, H)
+
+ # update actual state
+ delta_x = dot(K, y)
+ P = dot(dot(I_KH, P), I_KH.T) + dot(dot(K, R), K.T)
+
+ # inject observed error into state
+ x_new = np.zeros(x.shape, dtype=np.float64)
+ self.err_function(x, delta_x, x_new)
+ return x_new, P, y.flatten()
+
+ def maha_test(self, x, P, kind, z, R, extra_args=[], maha_thresh=0.95):
+ # init vars
+ z = z.reshape((-1, 1))
+ h = np.zeros(z.shape, dtype=np.float64)
+ H = np.zeros((z.shape[0], self.dim_x), dtype=np.float64)
+
+ # C functions
+ self.hs[kind](x, extra_args, h)
+ self.Hs[kind](x, extra_args, H)
+
+ # y is the "loss"
+ y = z - h
+
+ # if using eskf
+ H_mod = np.zeros((x.shape[0], P.shape[0]), dtype=np.float64)
+ self.H_mod(x, H_mod)
+ H = H.dot(H_mod)
+
+ a = np.linalg.inv(H.dot(P).dot(H.T) + R)
+ maha_dist = y.T.dot(a.dot(y))
+ if maha_dist > chi2.ppf(maha_thresh, y.shape[0]):
+ return False
+ else:
+ return True
+
+
+
+
+ def rts_smooth(self, estimates, norm_quats=False):
+ '''
+ Returns rts smoothed results of
+ kalman filter estimates
+
+ If the kalman state is augmented with
+ old states only the main state is smoothed
+ '''
+ xk_n = estimates[-1][0]
+ Pk_n = estimates[-1][2]
+ Fk_1 = np.zeros(Pk_n.shape, dtype=np.float64)
+
+ states_smoothed = [xk_n]
+ covs_smoothed = [Pk_n]
+ for k in xrange(len(estimates) - 2, -1, -1):
+ xk1_n = xk_n
+ if norm_quats:
+ xk1_n[3:7] /= np.linalg.norm(xk1_n[3:7])
+ Pk1_n = Pk_n
+
+ xk1_k, _, Pk1_k, _, t2, _, _, _, _ = estimates[k + 1]
+ _, xk_k, _, Pk_k, t1, _, _, _, _ = estimates[k]
+ dt = t2 - t1
+ self.F(xk_k, dt, Fk_1)
+
+ d1 = self.dim_main
+ d2 = self.dim_main_err
+ Ck = np.linalg.solve(Pk1_k[:d2,:d2], Fk_1[:d2,:d2].dot(Pk_k[:d2,:d2].T)).T
+ xk_n = xk_k
+ delta_x = np.zeros((Pk_n.shape[0], 1), dtype=np.float64)
+ self.inv_err_function(xk1_k, xk1_n, delta_x)
+ delta_x[:d2] = Ck.dot(delta_x[:d2])
+ x_new = np.zeros((xk_n.shape[0], 1), dtype=np.float64)
+ self.err_function(xk_k, delta_x, x_new)
+ xk_n[:d1] = x_new[:d1,0]
+ Pk_n = Pk_k
+ Pk_n[:d2,:d2] = Pk_k[:d2,:d2] + Ck.dot(Pk1_n[:d2,:d2] - Pk1_k[:d2,:d2]).dot(Ck.T)
+ states_smoothed.append(xk_n)
+ covs_smoothed.append(Pk_n)
+
+ return np.flipud(np.vstack(states_smoothed)), np.stack(covs_smoothed, 0)[::-1]
diff --git a/selfdrive/locationd/kalman/kalman_helpers.py b/selfdrive/locationd/kalman/kalman_helpers.py
new file mode 100644
index 000000000..b15f1542e
--- /dev/null
+++ b/selfdrive/locationd/kalman/kalman_helpers.py
@@ -0,0 +1,165 @@
+import numpy as np
+import os
+from bisect import bisect
+from tqdm import tqdm
+
+
+class ObservationKind(object):
+ UNKNOWN = 0
+ NO_OBSERVATION = 1
+ GPS_NED = 2
+ ODOMETRIC_SPEED = 3
+ PHONE_GYRO = 4
+ GPS_VEL = 5
+ PSEUDORANGE_GPS = 6
+ PSEUDORANGE_RATE_GPS = 7
+ SPEED = 8
+ NO_ROT = 9
+ PHONE_ACCEL = 10
+ ORB_POINT = 11
+ ECEF_POS = 12
+ CAMERA_ODO_TRANSLATION = 13
+ CAMERA_ODO_ROTATION = 14
+ ORB_FEATURES = 15
+ MSCKF_TEST = 16
+ FEATURE_TRACK_TEST = 17
+ LANE_PT = 18
+ IMU_FRAME = 19
+ PSEUDORANGE_GLONASS = 20
+ PSEUDORANGE_RATE_GLONASS = 21
+ PSEUDORANGE = 22
+ PSEUDORANGE_RATE = 23
+
+ names = ['Unknown',
+ 'No observation',
+ 'GPS NED',
+ 'Odometric speed',
+ 'Phone gyro',
+ 'GPS velocity',
+ 'GPS pseudorange',
+ 'GPS pseudorange rate',
+ 'Speed',
+ 'No rotation',
+ 'Phone acceleration',
+ 'ORB point',
+ 'ECEF pos',
+ 'camera odometric translation',
+ 'camera odometric rotation',
+ 'ORB features',
+ 'MSCKF test',
+ 'Feature track test',
+ 'Lane ecef point',
+ 'imu frame eulers',
+ 'GLONASS pseudorange',
+ 'GLONASS pseudorange rate']
+
+ @classmethod
+ def to_string(cls, kind):
+ return cls.names[kind]
+
+
+
+SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
+ ObservationKind.PSEUDORANGE_RATE_GPS,
+ ObservationKind.PSEUDORANGE_GLONASS,
+ ObservationKind.PSEUDORANGE_RATE_GLONASS]
+
+
+def run_car_ekf_offline(kf, observations_by_kind):
+ from laika.raw_gnss import GNSSMeasurement
+ observations = []
+ # create list of observations with element format: [kind, time, data]
+ for kind in observations_by_kind:
+ for t, data in zip(observations_by_kind[kind][0], observations_by_kind[kind][1]):
+ observations.append([t, kind, data])
+ observations.sort(key=lambda obs: obs[0])
+
+ times, estimates = run_observations_through_filter(kf, observations)
+
+ forward_states = np.stack(e[1] for e in estimates)
+ forward_covs = np.stack(e[3] for e in estimates)
+ smoothed_states, smoothed_covs = kf.rts_smooth(estimates)
+
+ observations_dict = {}
+ # TODO assuming observations and estimates
+ # are same length may not work with VO
+ for e in estimates:
+ t = e[4]
+ kind = str(int(e[5]))
+ res = e[6]
+ z = e[7]
+ ea = e[8]
+ if len(z) == 0:
+ continue
+ if kind not in observations_dict:
+ observations_dict[kind] = {}
+ observations_dict[kind]['t'] = np.array(len(z)*[t])
+ observations_dict[kind]['z'] = np.array(z)
+ observations_dict[kind]['ea'] = np.array(ea)
+ observations_dict[kind]['residual'] = np.array(res)
+ else:
+ observations_dict[kind]['t'] = np.append(observations_dict[kind]['t'], np.array(len(z)*[t]))
+ observations_dict[kind]['z'] = np.vstack((observations_dict[kind]['z'], np.array(z)))
+ observations_dict[kind]['ea'] = np.vstack((observations_dict[kind]['ea'], np.array(ea)))
+ observations_dict[kind]['residual'] = np.vstack((observations_dict[kind]['residual'], np.array(res)))
+
+ # add svIds to gnss data
+ for kind in map(str, SAT_OBS):
+ if int(kind) in observations_by_kind and kind in observations_dict:
+ observations_dict[kind]['svIds'] = np.array([])
+ observations_dict[kind]['CNO'] = np.array([])
+ observations_dict[kind]['std'] = np.array([])
+ for obs in observations_by_kind[int(kind)][1]:
+ observations_dict[kind]['svIds'] = np.append(observations_dict[kind]['svIds'],
+ np.array([obs[:,GNSSMeasurement.PRN]]))
+ observations_dict[kind]['std'] = np.append(observations_dict[kind]['std'],
+ np.array([obs[:,GNSSMeasurement.PR_STD]]))
+ return smoothed_states, smoothed_covs, forward_states, forward_covs, times, observations_dict
+
+
+def run_observations_through_filter(kf, observations, filter_time=None):
+ estimates = []
+
+ for obs in tqdm(observations):
+ t = obs[0]
+ kind = obs[1]
+ data = obs[2]
+ estimates.append(kf.predict_and_observe(t, kind, data))
+ times = [x[4] for x in estimates]
+ return times, estimates
+
+
+def save_residuals_plot(obs, save_path, data_name):
+ import matplotlib.pyplot as plt
+ import mpld3
+ fig = plt.figure(figsize=(10,20))
+ fig.suptitle('Residuals of ' + data_name, fontsize=24)
+ n = len(obs.keys())
+ start_times = [obs[kind]['t'][0] for kind in obs]
+ start_time = min(start_times)
+ xlims = [start_time + 3, start_time + 60]
+
+ for i, kind in enumerate(obs):
+ ax = fig.add_subplot(n, 1, i+1)
+ ax.set_xlim(xlims)
+ t = obs[kind]['t']
+ res = obs[kind]['residual']
+ start_idx = bisect(t, xlims[0])
+ if len(res) == start_idx:
+ continue
+ ylim = max(np.linalg.norm(res[start_idx:], axis=1))
+ ax.set_ylim([-ylim, ylim])
+ if int(kind) in SAT_OBS:
+ svIds = obs[kind]['svIds']
+ for svId in set(svIds):
+ svId_idx = (svIds == svId)
+ t = obs[kind]['t'][svId_idx]
+ res = obs[kind]['residual'][svId_idx]
+ ax.plot(t, res, label='SV ' + str(int(svId)))
+ ax.legend(loc='right')
+ else:
+ ax.plot(t, res)
+ plt.title('Residual of kind ' + ObservationKind.to_string(int(kind)), fontsize=20)
+ plt.tight_layout()
+ os.makedirs(save_path)
+ mpld3.save_html(fig, save_path + 'residuals_plot.html')
diff --git a/selfdrive/locationd/kalman/loc_local_kf.py b/selfdrive/locationd/kalman/loc_local_kf.py
new file mode 100755
index 000000000..df6d41c77
--- /dev/null
+++ b/selfdrive/locationd/kalman/loc_local_kf.py
@@ -0,0 +1,128 @@
+#!/usr/bin/env python
+import numpy as np
+import loc_local_model
+
+from kalman_helpers import ObservationKind
+from ekf_sym import EKF_sym
+
+
+
+class States(object):
+ VELOCITY = slice(0,3) # device frame velocity in m/s
+ ANGULAR_VELOCITY = slice(3, 6) # roll, pitch and yaw rates in device frame in radians/s
+ GYRO_BIAS = slice(6, 9) # roll, pitch and yaw biases
+ ODO_SCALE = slice(9, 10) # odometer scale
+ ACCELERATION = slice(10, 13) # Acceleration in device frame in m/s**2
+
+
+class LocLocalKalman(object):
+ def __init__(self):
+ x_initial = np.array([0, 0, 0,
+ 0, 0, 0,
+ 0, 0, 0,
+ 1,
+ 0, 0, 0])
+
+ # state covariance
+ P_initial = np.diag([10**2, 10**2, 10**2,
+ 1**2, 1**2, 1**2,
+ 0.05**2, 0.05**2, 0.05**2,
+ 0.02**2,
+ 1**2, 1**2, 1**2])
+
+ # process noise
+ Q = np.diag([0.0**2, 0.0**2, 0.0**2,
+ .01**2, .01**2, .01**2,
+ (0.005/100)**2, (0.005/100)**2, (0.005/100)**2,
+ (0.02/100)**2,
+ 3**2, 3**2, 3**2])
+
+ self.obs_noise = {ObservationKind.ODOMETRIC_SPEED: np.atleast_2d(0.2**2),
+ ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2])}
+
+ # MSCKF stuff
+ self.dim_state = len(x_initial)
+ self.dim_main = self.dim_state
+
+ name = 'loc_local'
+ loc_local_model.gen_model(name, self.dim_state)
+
+ # init filter
+ self.filter = EKF_sym(name, Q, x_initial, P_initial, self.dim_main, self.dim_main)
+
+ @property
+ def x(self):
+ return self.filter.state()
+
+ @property
+ def t(self):
+ return self.filter.filter_time
+
+ @property
+ def P(self):
+ return self.filter.covs()
+
+ def predict(self, t):
+ if self.t:
+ # Does NOT modify filter state
+ return self.filter._predict(self.x, self.P, t - self.t)[0]
+ else:
+ raise RuntimeError("Request predict on filter with uninitialized time")
+
+ def rts_smooth(self, estimates):
+ return self.filter.rts_smooth(estimates, norm_quats=True)
+
+
+ def init_state(self, state, covs_diag=None, covs=None, filter_time=None):
+ if covs_diag is not None:
+ P = np.diag(covs_diag)
+ elif covs is not None:
+ P = covs
+ else:
+ P = self.filter.covs()
+ self.filter.init_state(state, P, filter_time)
+
+ def predict_and_observe(self, t, kind, data):
+ if len(data) > 0:
+ data = np.atleast_2d(data)
+ if kind == ObservationKind.CAMERA_ODO_TRANSLATION:
+ r = self.predict_and_update_odo_trans(data, t, kind)
+ elif kind == ObservationKind.CAMERA_ODO_ROTATION:
+ r = self.predict_and_update_odo_rot(data, t, kind)
+ elif kind == ObservationKind.ODOMETRIC_SPEED:
+ r = self.predict_and_update_odo_speed(data, t, kind)
+ else:
+ r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
+ return r
+
+ def get_R(self, kind, n):
+ obs_noise = self.obs_noise[kind]
+ dim = obs_noise.shape[0]
+ R = np.zeros((n, dim, dim))
+ for i in xrange(n):
+ R[i,:,:] = obs_noise
+ return R
+
+ def predict_and_update_odo_speed(self, speed, t, kind):
+ z = np.array(speed)
+ R = np.zeros((len(speed), 1, 1))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag([0.2**2])
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+ def predict_and_update_odo_trans(self, trans, t, kind):
+ z = trans[:,:3]
+ R = np.zeros((len(trans), 3, 3))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag(trans[i,3:]**2)
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+ def predict_and_update_odo_rot(self, rot, t, kind):
+ z = rot[:,:3]
+ R = np.zeros((len(rot), 3, 3))
+ for i, _ in enumerate(z):
+ R[i,:,:] = np.diag(rot[i,3:]**2)
+ return self.filter.predict_and_update_batch(t, kind, z, R)
+
+if __name__ == "__main__":
+ LocLocalKalman()
diff --git a/selfdrive/locationd/kalman/loc_local_model.py b/selfdrive/locationd/kalman/loc_local_model.py
new file mode 100644
index 000000000..2d69cac02
--- /dev/null
+++ b/selfdrive/locationd/kalman/loc_local_model.py
@@ -0,0 +1,80 @@
+import numpy as np
+import sympy as sp
+import os
+
+from kalman_helpers import ObservationKind
+from ekf_sym import gen_code
+
+
+def gen_model(name, dim_state):
+
+ # check if rebuild is needed
+ try:
+ dir_path = os.path.dirname(__file__)
+ deps = [dir_path + '/' + 'ekf_c.c',
+ dir_path + '/' + 'ekf_sym.py',
+ dir_path + '/' + 'loc_local_model.py',
+ dir_path + '/' + 'loc_local_kf.py']
+
+ outs = [dir_path + '/' + name + '.o',
+ dir_path + '/' + name + '.so',
+ dir_path + '/' + name + '.cpp']
+ out_times = map(os.path.getmtime, outs)
+ dep_times = map(os.path.getmtime, deps)
+ rebuild = os.getenv("REBUILD", False)
+ if min(out_times) > max(dep_times) and not rebuild:
+ return
+ map(os.remove, outs)
+ except OSError:
+ pass
+
+ # make functions and jacobians with sympy
+ # state variables
+ state_sym = sp.MatrixSymbol('state', dim_state, 1)
+ state = sp.Matrix(state_sym)
+ v = state[0:3,:]
+ omega = state[3:6,:]
+ vroll, vpitch, vyaw = omega
+ vx, vy, vz = v
+ roll_bias, pitch_bias, yaw_bias = state[6:9,:]
+ odo_scale = state[9,:]
+ accel = state[10:13,:]
+
+ dt = sp.Symbol('dt')
+
+ # Time derivative of the state as a function of state
+ state_dot = sp.Matrix(np.zeros((dim_state, 1)))
+ state_dot[:3,:] = accel
+
+ # Basic descretization, 1st order intergrator
+ # Can be pretty bad if dt is big
+ f_sym = sp.Matrix(state + dt*state_dot)
+
+ #
+ # Observation functions
+ #
+
+ # extra args
+ #imu_rot = euler_rotate(*imu_angles)
+ #h_gyro_sym = imu_rot*sp.Matrix([vroll + roll_bias,
+ # vpitch + pitch_bias,
+ # vyaw + yaw_bias])
+ h_gyro_sym = sp.Matrix([vroll + roll_bias,
+ vpitch + pitch_bias,
+ vyaw + yaw_bias])
+
+ speed = vx**2 + vy**2 + vz**2
+ h_speed_sym = sp.Matrix([sp.sqrt(speed)*odo_scale])
+
+ h_relative_motion = sp.Matrix(v)
+ h_phone_rot_sym = sp.Matrix([vroll,
+ vpitch,
+ vyaw])
+
+
+ obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
+ [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+ [h_phone_rot_sym, ObservationKind.NO_ROT, None],
+ [h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
+ [h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None]]
+ gen_code(name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state)
diff --git a/selfdrive/locationd/locationd_local.py b/selfdrive/locationd/locationd_local.py
new file mode 100755
index 000000000..0b1af2f4c
--- /dev/null
+++ b/selfdrive/locationd/locationd_local.py
@@ -0,0 +1,274 @@
+#!/usr/bin/env python
+import os
+import zmq
+import math
+import json
+
+os.environ["OMP_NUM_THREADS"] = "1"
+import numpy as np
+from bisect import bisect_right
+
+from cereal import car
+from common.params import Params
+from common.numpy_fast import clip
+import selfdrive.messaging as messaging
+from selfdrive.swaglog import cloudlog
+from selfdrive.controls.lib.vehicle_model import VehicleModel
+from selfdrive.services import service_list
+from selfdrive.locationd.kalman.loc_local_kf import LocLocalKalman
+from selfdrive.locationd.kalman.kalman_helpers import ObservationKind
+
+DEBUG = False
+kf = LocLocalKalman() # Make sure that model is generated on import time
+
+MAX_ANGLE_OFFSET = math.radians(10.)
+MAX_ANGLE_OFFSET_TH = math.radians(9.)
+MIN_STIFFNESS = 0.5
+MAX_STIFFNESS = 2.0
+MIN_SR = 0.5
+MAX_SR = 2.0
+MIN_SR_TH = 0.55
+MAX_SR_TH = 1.9
+
+LEARNING_RATE = 3
+
+class Localizer(object):
+ def __init__(self, disabled_logs=None, dog=None):
+ self.kf = LocLocalKalman()
+ self.reset_kalman()
+
+ self.max_age = .2 # seconds
+ self.calibration_valid = False
+
+ if disabled_logs is None:
+ self.disabled_logs = list()
+ else:
+ self.disabled_logs = disabled_logs
+
+ def reset_kalman(self):
+ self.filter_time = None
+ self.observation_buffer = []
+ self.converter = None
+ self.speed_counter = 0
+ self.sensor_counter = 0
+
+ def liveLocationMsg(self, time):
+ fix = messaging.log.KalmanOdometry.new_message()
+
+ predicted_state = self.kf.x
+ fix.trans = [float(predicted_state[0]), float(predicted_state[1]), float(predicted_state[2])]
+ fix.rot = [float(predicted_state[3]), float(predicted_state[4]), float(predicted_state[5])]
+
+ return fix
+
+ def update_kalman(self, time, kind, meas):
+ idx = bisect_right([x[0] for x in self.observation_buffer], time)
+ self.observation_buffer.insert(idx, (time, kind, meas))
+ while self.observation_buffer[-1][0] - self.observation_buffer[0][0] > self.max_age:
+ self.kf.predict_and_observe(*self.observation_buffer.pop(0))
+
+ def handle_cam_odo(self, log, current_time):
+ self.update_kalman(current_time, ObservationKind.CAMERA_ODO_ROTATION, np.concatenate([log.cameraOdometry.rot,
+ log.cameraOdometry.rotStd]))
+ self.update_kalman(current_time, ObservationKind.CAMERA_ODO_TRANSLATION, np.concatenate([log.cameraOdometry.trans,
+ log.cameraOdometry.transStd]))
+
+ def handle_car_state(self, log, current_time):
+ self.speed_counter += 1
+ if self.speed_counter % 5 == 0:
+ self.update_kalman(current_time, ObservationKind.ODOMETRIC_SPEED, np.array([log.carState.vEgo]))
+
+ def handle_sensors(self, log, current_time):
+ for sensor_reading in log.sensorEvents:
+ # TODO does not yet account for double sensor readings in the log
+ if sensor_reading.type == 4:
+ self.sensor_counter += 1
+ if self.sensor_counter % LEARNING_RATE == 0:
+ self.update_kalman(current_time, ObservationKind.PHONE_GYRO, [-sensor_reading.gyro.v[2], -sensor_reading.gyro.v[1], -sensor_reading.gyro.v[0]])
+
+ def handle_log(self, log):
+ current_time = 1e-9 * log.logMonoTime
+ typ = log.which
+ if typ in self.disabled_logs:
+ return
+ if typ == "sensorEvents":
+ self.handle_sensors(log, current_time)
+ elif typ == "carState":
+ self.handle_car_state(log, current_time)
+ elif typ == "cameraOdometry":
+ self.handle_cam_odo(log, current_time)
+
+
+class ParamsLearner(object):
+ def __init__(self, VM, angle_offset=0., stiffness_factor=1.0, steer_ratio=None, learning_rate=1.0):
+ self.VM = VM
+
+ self.ao = math.radians(angle_offset)
+ self.slow_ao = math.radians(angle_offset)
+ self.x = stiffness_factor
+ self.sR = VM.sR if steer_ratio is None else steer_ratio
+ self.MIN_SR = MIN_SR * self.VM.sR
+ self.MAX_SR = MAX_SR * self.VM.sR
+ self.MIN_SR_TH = MIN_SR_TH * self.VM.sR
+ self.MAX_SR_TH = MAX_SR_TH * self.VM.sR
+
+ self.alpha1 = 0.01 * learning_rate
+ self.alpha2 = 0.00025 * learning_rate
+ self.alpha3 = 0.1 * learning_rate
+ self.alpha4 = 1.0 * learning_rate
+
+ def get_values(self):
+ return {
+ 'angleOffsetAverage': math.degrees(self.slow_ao),
+ 'stiffnessFactor': self.x,
+ 'steerRatio': self.sR,
+ }
+
+ def update(self, psi, u, sa):
+ cF0 = self.VM.cF
+ cR0 = self.VM.cR
+ aR = self.VM.aR
+ aF = self.VM.aF
+ l = self.VM.l
+ m = self.VM.m
+
+ x = self.x
+ ao = self.ao
+ sR = self.sR
+
+ # Gradient descent: learn angle offset, tire stiffness and steer ratio.
+ if u > 10.0 and abs(math.degrees(sa)) < 15.:
+ self.ao -= self.alpha1 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ ao = self.slow_ao
+ self.slow_ao -= self.alpha2 * 2.0*cF0*cR0*l*u*x*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ self.x -= self.alpha3 * -2.0*cF0*cR0*l*m*u**3*(ao - sa)*(aF*cF0 - aR*cR0)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**2*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**3)
+
+ self.sR -= self.alpha4 * -2.0*cF0*cR0*l*u*x*(ao - sa)*(1.0*cF0*cR0*l*u*x*(ao - sa) + psi*sR*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0)))/(sR**3*(cF0*cR0*l**2*x - m*u**2*(aF*cF0 - aR*cR0))**2)
+
+ if DEBUG:
+ # s1 = "Measured yaw rate % .6f" % psi
+ # ao = 0.
+ # s2 = "Uncompensated yaw % .6f" % (1.0*u*(-ao + sa)/(l*sR*(1 - m*u**2*(aF*cF0*x - aR*cR0*x)/(cF0*cR0*l**2*x**2))))
+ # instant_ao = aF*m*psi*sR*u/(cR0*l*x) - aR*m*psi*sR*u/(cF0*l*x) - l*psi*sR/u + sa
+ s4 = "Instant AO: % .2f Avg. AO % .2f" % (math.degrees(self.ao), math.degrees(self.slow_ao))
+ s5 = "Stiffnes: % .3f x" % self.x
+ print s4, s5
+
+
+ self.ao = clip(self.ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
+ self.slow_ao = clip(self.slow_ao, -MAX_ANGLE_OFFSET, MAX_ANGLE_OFFSET)
+ self.x = clip(self.x, MIN_STIFFNESS, MAX_STIFFNESS)
+ self.sR = clip(self.sR, self.MIN_SR, self.MAX_SR)
+
+ # don't check stiffness for validity, as it can change quickly if sR is off
+ valid = abs(self.slow_ao) < MAX_ANGLE_OFFSET_TH and \
+ self.sR > self.MIN_SR_TH and self.sR < self.MAX_SR_TH
+
+ return valid
+
+
+def locationd_thread(gctx, addr, disabled_logs):
+ ctx = zmq.Context()
+ poller = zmq.Poller()
+
+ car_state_socket = messaging.sub_sock(ctx, service_list['carState'].port, poller, addr=addr, conflate=True)
+ sensor_events_socket = messaging.sub_sock(ctx, service_list['sensorEvents'].port, poller, addr=addr, conflate=True)
+ camera_odometry_socket = messaging.sub_sock(ctx, service_list['cameraOdometry'].port, poller, addr=addr, conflate=True)
+
+ kalman_odometry_socket = messaging.pub_sock(ctx, service_list['kalmanOdometry'].port)
+ live_parameters_socket = messaging.pub_sock(ctx, service_list['liveParameters'].port)
+
+ params_reader = Params()
+ cloudlog.info("Parameter learner is waiting for CarParams")
+ CP = car.CarParams.from_bytes(params_reader.get("CarParams", block=True))
+ VM = VehicleModel(CP)
+ cloudlog.info("Parameter learner got CarParams: %s" % CP.carFingerprint)
+
+ params = params_reader.get("LiveParameters")
+
+ # Check if car model matches
+ if params is not None:
+ params = json.loads(params)
+ if params.get('carFingerprint', None) != CP.carFingerprint:
+ cloudlog.info("Parameter learner found parameters for wrong car.")
+ params = None
+
+ if params is None:
+ params = {
+ 'carFingerprint': CP.carFingerprint,
+ 'angleOffsetAverage': 0.0,
+ 'stiffnessFactor': 1.0,
+ 'steerRatio': VM.sR,
+ }
+ cloudlog.info("Parameter learner resetting to default values")
+
+ cloudlog.info("Parameter starting with: %s" % str(params))
+ localizer = Localizer(disabled_logs=disabled_logs)
+
+ learner = ParamsLearner(VM,
+ angle_offset=params['angleOffsetAverage'],
+ stiffness_factor=params['stiffnessFactor'],
+ steer_ratio=params['steerRatio'],
+ learning_rate=LEARNING_RATE)
+
+ i = 0
+ while True:
+ for socket, event in poller.poll(timeout=1000):
+ log = messaging.recv_one(socket)
+ localizer.handle_log(log)
+
+ if socket is car_state_socket:
+ if not localizer.kf.t:
+ continue
+
+ if i % LEARNING_RATE == 0:
+ # carState is not updating the Kalman Filter, so update KF manually
+ localizer.kf.predict(1e-9 * log.logMonoTime)
+
+ predicted_state = localizer.kf.x
+ yaw_rate = -float(predicted_state[5])
+
+ steering_angle = math.radians(log.carState.steeringAngle)
+ params_valid = learner.update(yaw_rate, log.carState.vEgo, steering_angle)
+
+ params = messaging.new_message()
+ params.init('liveParameters')
+ params.liveParameters.valid = bool(params_valid)
+ params.liveParameters.angleOffset = float(math.degrees(learner.ao))
+ params.liveParameters.angleOffsetAverage = float(math.degrees(learner.slow_ao))
+ params.liveParameters.stiffnessFactor = float(learner.x)
+ params.liveParameters.steerRatio = float(learner.sR)
+ live_parameters_socket.send(params.to_bytes())
+
+ if i % 6000 == 0: # once a minute
+ params = learner.get_values()
+ params['carFingerprint'] = CP.carFingerprint
+ params_reader.put("LiveParameters", json.dumps(params))
+
+ i += 1
+ elif socket is camera_odometry_socket:
+ msg = messaging.new_message()
+ msg.init('kalmanOdometry')
+ msg.logMonoTime = log.logMonoTime
+ msg.kalmanOdometry = localizer.liveLocationMsg(log.logMonoTime * 1e-9)
+ kalman_odometry_socket.send(msg.to_bytes())
+ elif socket is sensor_events_socket:
+ pass
+
+
+def main(gctx=None, addr="127.0.0.1"):
+ IN_CAR = os.getenv("IN_CAR", False)
+ disabled_logs = os.getenv("DISABLED_LOGS", "").split(",")
+
+ # No speed for now
+ disabled_logs.append('carState')
+ if IN_CAR:
+ addr = "192.168.5.11"
+
+ locationd_thread(gctx, addr, disabled_logs)
+
+
+if __name__ == "__main__":
+ main()
diff --git a/selfdrive/locationd/ublox.py b/selfdrive/locationd/ublox.py
index 06f174fba..70e2d2fa0 100644
--- a/selfdrive/locationd/ublox.py
+++ b/selfdrive/locationd/ublox.py
@@ -207,10 +207,13 @@ class UBloxDescriptor:
def __init__(self,
name,
msg_format,
- fields=[],
+ fields=None,
count_field=None,
format2=None,
fields2=None):
+ if fields is None:
+ fields = []
+
self.name = name
self.msg_format = msg_format
self.fields = fields
@@ -465,7 +468,7 @@ msg_types = {
(CLASS_RXM, MSG_RXM_SFRB):
UBloxDescriptor('RXM_SFRB', '